diff --git a/.github/ISSUE_TEMPLATE/Bug_report.md b/.github/ISSUE_TEMPLATE/Bug_report.md index 5928264417b..2087e053d9e 100644 --- a/.github/ISSUE_TEMPLATE/Bug_report.md +++ b/.github/ISSUE_TEMPLATE/Bug_report.md @@ -39,7 +39,7 @@ assignees: '' ## Additional context - + --- diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index c530d562787..bf911321f14 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -2,7 +2,7 @@ name: Build firmware # Don't enable CI on push, just on PR. If you # are working on the main repo and want to trigger # a CI build submit a draft PR. -on: +on: push: branches: - '!maintenance-8.x.x' @@ -186,7 +186,7 @@ jobs: - uses: actions/checkout@v4 - name: Install dependencies run: | - brew install cmake ninja ruby + brew install ruby - name: Setup environment env: @@ -231,7 +231,7 @@ jobs: - name: Setup Cygwin uses: egor-tensin/setup-cygwin@v4 with: - packages: cmake ruby ninja gcc-g++ + packages: cmake ruby ninja gcc-g++ rubygems - name: Setup environment env: ACTIONS_ALLOW_UNSECURE_COMMANDS: true @@ -248,19 +248,23 @@ jobs: VERSION=$( grep project CMakeLists.txt|awk -F VERSION '{ gsub(/[ \t)]/, "", $2); print $2 }' ) echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV - + - name: Build SITL - run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja -j4 + run: gem install getoptlong && mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja -j4 - name: Strip version number run: | for f in ./build_SITL/*_SITL.exe; do mv $f $(echo $f | sed -e 's/_[0-9]\+\.[0-9]\+\.[0-9]\+//') done + - name: Copy cygwin1.dll + run: cp /bin/cygwin1.dll ./build_SITL/ - name: Upload artifacts uses: actions/upload-artifact@v4 with: name: ${{ env.BUILD_NAME }}_SITL-WIN - path: ./build_SITL/*.exe + path: | + ./build_SITL/*.exe + ./build_SITL/cygwin1.dll test: #needs: [build] diff --git a/.github/workflows/pr-branch-suggestion.yml b/.github/workflows/pr-branch-suggestion.yml new file mode 100644 index 00000000000..110deb98338 --- /dev/null +++ b/.github/workflows/pr-branch-suggestion.yml @@ -0,0 +1,41 @@ +name: PR Branch Suggestion + +on: + pull_request_target: + types: [opened] + branches: + - master + +jobs: + suggest-branch: + runs-on: ubuntu-latest + permissions: + pull-requests: write + steps: + - name: Suggest maintenance branch + uses: actions/github-script@v7 + with: + script: | + const comment = `### Branch Targeting Suggestion + + You've targeted the \`master\` branch with this PR. Please consider if a version branch might be more appropriate: + + - **\`maintenance-9.x\`** - If your change is backward-compatible and won't create compatibility issues between INAV firmware and Configurator 9.x versions. This will allow your PR to be included in the next 9.x release. + + - **\`maintenance-10.x\`** - If your change introduces compatibility requirements between firmware and configurator that would break 9.x compatibility. This is for PRs which will be included in INAV 10.x + + If \`master\` is the correct target for this change, no action is needed. + + --- + *This is an automated suggestion to help route contributions to the appropriate branch.*`; + + try { + await github.rest.issues.createComment({ + issue_number: context.issue.number, + owner: context.repo.owner, + repo: context.repo.repo, + body: comment + }); + } catch (err) { + core.setFailed(`Failed to post suggestion comment: ${err}`); + } diff --git a/.vscode/settings.json b/.vscode/settings.json index 65abb05cefe..c0f2c8f0ef7 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -8,7 +8,8 @@ "platform.h": "c", "timer.h": "c", "bus.h": "c", - "io.h": "c" + "io.h": "c", + "fc_core.h": "c" }, "editor.tabSize": 4, "editor.insertSpaces": true, diff --git a/CMakeLists.txt b/CMakeLists.txt index 87172bfc432..dc34f9da32b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,7 +51,9 @@ else() endif() endif() -project(INAV VERSION 8.1.0) + +project(INAV VERSION 9.0.0) + enable_language(ASM) diff --git a/Dockerfile b/Dockerfile index 4d2c144f57d..cb08a5d3298 100644 --- a/Dockerfile +++ b/Dockerfile @@ -2,23 +2,22 @@ FROM ubuntu:jammy ARG USER_ID ARG GROUP_ID -ENV DEBIAN_FRONTEND noninteractive +ARG GDB -RUN apt-get update && apt-get install -y git cmake make ruby gcc python3 python3-pip gcc-arm-none-eabi ninja-build +ENV DEBIAN_FRONTEND=noninteractive -RUN if [ "$GDB" = "yes" ]; then apt-get install -y gdb; fi +RUN apt-get update && apt-get install -y git cmake make ruby gcc python3 python3-yaml ninja-build gcc-arm-none-eabi -RUN pip install pyyaml +RUN if [ "$GDB" = "yes" ]; then apt-get install -y gdb; fi -# if either of these are already set the same as the user's machine, leave them be and ignore the error -RUN addgroup --gid $GROUP_ID inav; exit 0; -RUN adduser --disabled-password --gecos '' --uid $USER_ID --gid $GROUP_ID inav; exit 0; +# If a group and user with the same IDs already exist, rename the group and recreate the user after deleting the existing one. +RUN GROUP="$(id -n -g $GROUP_ID)"; if [ -n "$GROUP" ]; then groupmod -n inav "$GROUP"; else groupadd --gid $GROUP_ID inav; fi +RUN USER="$(id -n -u $USER_ID)"; if [ -n "$USER" ]; then userdel -r "$USER"; fi && useradd -m --uid $USER_ID --gid $GROUP_ID inav USER inav RUN git config --global --add safe.directory /src -VOLUME /src - WORKDIR /src/build + ENTRYPOINT ["/src/cmake/docker.sh"] diff --git a/build.sh b/build.sh index fe5a5281e42..cbb76f6f6d5 100755 --- a/build.sh +++ b/build.sh @@ -1,5 +1,5 @@ #!/usr/bin/env bash -set -e +set -euo pipefail if [[ $# == 0 ]]; then echo -e "\ @@ -8,6 +8,10 @@ Usage syntax: ./build.sh Notes: * You can specify multiple targets. ./build.sh + * To get a list of release targets use \"release_targets\" + ./build.sh release_targets + * To get a list of valid targets use \"valid_targets\" + ./build.sh valid_targets * To get a list of all targets use \"help\". Hint: pipe the output through a pager. ./build.sh help | less * To build all targets use \"all\" @@ -19,10 +23,20 @@ Notes: exit 1 fi +run_docker() { + docker run --rm -it -v "$(pwd)":/src inav-build "$@" +} + if [ -z "$(docker images -q inav-build)" ]; then - echo -e "*** Building image\n" - docker build -t inav-build --build-arg USER_ID="$(id -u)" --build-arg GROUP_ID="$(id -g)" . - echo -ne "\n" + echo "*** Building Docker image" + docker build -t inav-build \ + --build-arg USER_ID="$(id -u)" \ + --build-arg GROUP_ID="$(id -g)" . +else + docker build -q -t inav-build \ + --build-arg USER_ID="$(id -u)" \ + --build-arg GROUP_ID="$(id -g)" . >/dev/null || + { echo "*** Building Docker image: ERROR"; exit 1; } fi if [ ! -d ./build ]; then @@ -40,10 +54,20 @@ if [ ! -d ./tools ]; then mkdir ./tools && chmod 777 ./tools fi -echo -e "*** Building targets [$@]\n" -docker run --rm -it -v "$(pwd)":/src inav-build $@ +case "$1" in + release_targets) + run_docker targets | sed -n 's/^Release targets: \(.*\)/\1/p'|tr ' ' '\n' + ;; + valid_targets) + run_docker targets | sed -n 's/^Valid targets: \(.*\)/\1/p'|tr ' ' '\n' + ;; + *) + echo -e "*** Building targets [$@]\n" + run_docker "$@" + if ls ./build/*.hex &> /dev/null; then + echo -e "\n*** Built targets in ./build:" + stat -c "%n (%.19y)" ./build/*.hex + fi + ;; +esac -if [ -z "$(ls ./build/*.hex &> /dev/null)" ]; then - echo -e "\n*** Built targets in ./build:" - stat -c "%n (%.19y)" ./build/*.hex -fi diff --git a/cmake/arm-none-eabi-checks.cmake b/cmake/arm-none-eabi-checks.cmake index f31a26c3e3a..a379f559213 100644 --- a/cmake/arm-none-eabi-checks.cmake +++ b/cmake/arm-none-eabi-checks.cmake @@ -125,12 +125,23 @@ function(arm_none_eabi_gcc_check) if(NOT version) message("-- could not find ${prog}") arm_none_eabi_gcc_install() + gcc_get_version(version + TRIPLET ${arm_none_eabi_triplet} + PROGRAM_NAME prog + PROGRAM_PATH prog_path + ) return() endif() message("-- found ${prog} ${version} at ${prog_path}") if(COMPILER_VERSION_CHECK AND NOT arm_none_eabi_gcc_version STREQUAL version) message("-- expecting ${prog} version ${arm_none_eabi_gcc_version}, but got version ${version} instead") arm_none_eabi_gcc_install() + unset(gcc CACHE) + gcc_get_version(version + TRIPLET ${arm_none_eabi_triplet} + PROGRAM_NAME prog + PROGRAM_PATH prog_path + ) return() endif() endfunction() diff --git a/cmake/at32-bootloader.cmake b/cmake/at32-bootloader.cmake index 857ae08a524..0caba562747 100644 --- a/cmake/at32-bootloader.cmake +++ b/cmake/at32-bootloader.cmake @@ -21,7 +21,7 @@ main_sources(BOOTLOADER_SOURCES drivers/time.c drivers/timer.c drivers/flash_m25p16.c - drivers/flash_w25n01g.c + drivers/flash_w25n.c drivers/flash.c fc/firmware_update_common.c diff --git a/cmake/docker.sh b/cmake/docker.sh index 843e03a48a2..b975bae771d 100755 --- a/cmake/docker.sh +++ b/cmake/docker.sh @@ -11,7 +11,7 @@ initialize_cmake() { } # Check if CMake has never been initialized -if [ ! -f Makefile ]; then +if [ ! -f build.ninja ]; then initialize_cmake fi diff --git a/docs/ADSB.md b/docs/ADSB.md index 3a36d1aa341..2e5bef69dfe 100644 --- a/docs/ADSB.md +++ b/docs/ADSB.md @@ -7,6 +7,26 @@ is an air traffic surveillance technology that enables aircraft to be accurately OSD can be configured to shows the closest aircraft. +## OSD ADSB Info element +* "-" no ADSB device detected +* "H" IMU heading is not valid +* "G" no GPS fix or less than 4 stats +* "[Number]" count of ADSB aircrafts + +## OSD ADSB Warning element +OSD can be configured to simple view (one line) or to extended view (two lines) by \ +`set osd_adsb_warning_style=EXTENDED` + +### Simple view +`{distance to vehicle} {direction to vehicle} {altitude diff}` + +### Extended view +`{distance to vehicle} {direction to vehicle} {altitude diff}` \ +`{Emiter Type} {Vehicle direction} {Vehicle Speed}` + +![ADSB OSD](assets/images/adsb-info.png) + + ## Hardware All ADSB receivers which can send Mavlink [ADSB_VEHICLE](https://mavlink.io/en/messages/common.html#ADSB_VEHICLE) message are supported diff --git a/docs/Battery.md b/docs/Battery.md index e519c325410..a66e6ae5361 100644 --- a/docs/Battery.md +++ b/docs/Battery.md @@ -169,6 +169,155 @@ current_meter_scale = (reported_draw_mAh / charging_data_mAh) * old_current_mete = 435 ``` +## Power and Current Limiting + +INAV includes an advanced power and current limiting system to protect your battery and ESCs from excessive discharge rates. This feature automatically reduces throttle output when current or power draw exceeds configured limits. + +### Why Use Power Limiting? + +Power and current limiting helps: +- **Protect batteries** from exceeding their C-rating and getting damaged +- **Prevent voltage sag** and brown-outs during high-throttle maneuvers +- **Extend battery lifespan** by avoiding excessive discharge rates +- **Improve safety** by preventing ESC or battery overheating +- **Comply with regulations** that may limit power output + +### How It Works + +The power limiter uses a PI (Proportional-Integral) controller to smoothly reduce throttle when current or power exceeds limits. It supports two operating modes: + +1. **Continuous Limit**: The sustained current/power that can be drawn indefinitely +2. **Burst Limit**: A higher current/power allowed for a short duration before falling back to the continuous limit + +This burst mode allows brief high-power maneuvers (like punch-outs or quick climbs) while protecting the battery during sustained high-throttle flight. + +### Configuration + +Power limiting requires a current sensor (`CURRENT_METER` feature). Power-based limiting additionally requires voltage measurement (`VBAT` feature). + +#### Basic Settings (per battery profile) + +| Setting | Description | Unit | Range | +|---------|-------------|------|-------| +| `limit_cont_current` | Continuous current limit | dA (deci-amps) | 0-2000 (0-200A) | +| `limit_burst_current` | Burst current limit | dA | 0-2000 (0-200A) | +| `limit_burst_current_time` | Duration burst is allowed | ds (deci-seconds) | 0-600 (0-60s) | +| `limit_burst_current_falldown_time` | Ramp-down duration from burst to continuous | ds | 0-600 (0-60s) | +| `limit_cont_power` | Continuous power limit | dW (deci-watts) | 0-20000 (0-2000W) | +| `limit_burst_power` | Burst power limit | dW | 0-20000 (0-2000W) | +| `limit_burst_power_time` | Duration burst power is allowed | ds | 0-600 (0-60s) | +| `limit_burst_power_falldown_time` | Ramp-down duration for power | ds | 0-600 (0-60s) | + +**Note**: Set any limit to `0` to disable that specific limiter. + +#### Advanced Tuning Settings + +| Setting | Description | Default | Range | +|---------|-------------|---------|-------| +| `limit_pi_p` | Proportional gain for PI controller | 100 | 10-500 | +| `limit_pi_i` | Integral gain for PI controller | 15 | 10-200 | +| `limit_attn_filter_cutoff` | Low-pass filter cutoff frequency | 50 Hz | 10-200 | + +### Example Configurations + +#### Example 1: Simple Current Limiting (50A continuous) + +Protect a 1500mAh 4S 50C battery (75A max burst, 50A continuous safe): + +``` +battery_profile 1 + +set limit_cont_current = 500 # 50A continuous +set limit_burst_current = 750 # 75A burst +set limit_burst_current_time = 100 # 10 seconds +set limit_burst_current_falldown_time = 20 # 2 second ramp-down +``` + +#### Example 2: Power Limiting for Racing (500W limit) + +Limit total system power for racing class restrictions: + +``` +battery_profile 1 + +set limit_cont_power = 4500 # 450W continuous +set limit_burst_power = 5000 # 500W burst +set limit_burst_power_time = 50 # 5 seconds +set limit_burst_power_falldown_time = 10 # 1 second ramp-down +``` + +#### Example 3: Combined Current and Power Limiting + +Protect both battery (current) and ESCs (power): + +``` +battery_profile 1 + +# Current limits (battery protection) +set limit_cont_current = 600 # 60A continuous +set limit_burst_current = 800 # 80A burst +set limit_burst_current_time = 100 # 10 seconds + +# Power limits (ESC protection) +set limit_cont_power = 8000 # 800W continuous +set limit_burst_power = 10000 # 1000W burst +set limit_burst_power_time = 100 # 10 seconds +``` + +### Understanding Burst Mode + +When you exceed the continuous limit, the system uses "burst reserve" (like a capacitor): +- **Burst reserve** starts full and depletes when current/power exceeds the continuous limit +- When reserve is empty, the limit drops to the continuous value +- The `falldown_time` setting creates a smooth ramp-down instead of an abrupt drop +- Reserve recharges when current/power drops below the continuous limit + +**Example timeline** (60A continuous, 80A burst, 10s burst time, 2s falldown): +``` +Time Limit Reason +---- ----- ------ +0s 80A Full burst reserve +5s 80A Still have reserve (using 5s of 10s) +10s 80A Reserve depleted +10-12s 80→60A Ramping down over 2 seconds +12s+ 60A Continuous limit active +``` + +### OSD Elements + +Three OSD elements display power limiting status: + +- **`OSD_PLIMIT_REMAINING_BURST_TIME`**: Shows remaining burst time in seconds +- **`OSD_PLIMIT_ACTIVE_CURRENT_LIMIT`**: Shows current limit being enforced (blinks when limiting) +- **`OSD_PLIMIT_ACTIVE_POWER_LIMIT`**: Shows power limit being enforced (blinks when limiting) + +Enable these in the OSD tab to monitor limiting during flight. + +### Calibration Tips + +1. **Find your battery's limits**: Check manufacturer specifications for continuous and burst C-ratings + - Continuous limit = `battery_capacity_mAh × continuous_C_rating / 100` (in dA) + - Burst limit = `battery_capacity_mAh × burst_C_rating / 100` (in dA) + +2. **Test incrementally**: Start with conservative limits and increase gradually + +3. **Monitor in flight**: Use OSD elements to see when limiting activates + +4. **Calibrate current sensor**: Accurate current readings are critical - see "Current Monitoring" section above + +5. **Tune PI controller**: If limiting feels abrupt or causes oscillation, adjust `limit_pi_p` and `limit_pi_i`: + - Increase P for faster response (may cause oscillation) + - Increase I for better steady-state accuracy + - Decrease if throttle oscillates during limiting + +### Notes + +- Power limiting is part of the battery profile system - each profile can have different limits +- Both current and power limiting can be active simultaneously - the most restrictive applies +- Limiting is applied smoothly via PI controller to avoid abrupt throttle cuts +- The system uses instantaneous current/power readings for responsive limiting +- Set limits to `0` to disable a specific limiter while keeping others active + ## Battery capacity monitoring For the capacity monitoring to work you need a current sensor (`CURRENT_METER` feature). For monitoring energy in milliWatt hour you also need voltage measurement (`VBAT` feature). For best results the current and voltage readings have to be calibrated. diff --git a/docs/Blackbox.md b/docs/Blackbox.md index d0e9fe5dc61..b4528d8dd23 100644 --- a/docs/Blackbox.md +++ b/docs/Blackbox.md @@ -120,6 +120,9 @@ These chips are also supported: * Winbond W25Q64 - 64 Mbit / 8 MByte * Micron N25Q0128 - 128 Mbit / 16 MByte * Winbond W25Q128 - 128 Mbit / 16 MByte +* Puya PY25Q128HA - 128 Mbit / 16 MByte +* Winbond W25N01 - 1 Gbit / 128 MByte +* Winbond W25N02 - 2 Gbit / 256 MByte #### Enable recording to dataflash On the Configurator's CLI tab, you must enter `set blackbox_device=SPIFLASH` to switch to logging to an onboard dataflash chip, then save. diff --git a/docs/Control Profiles.md b/docs/Control Profiles.md index af41fed53b9..c59c8ecdf73 100644 --- a/docs/Control Profiles.md +++ b/docs/Control Profiles.md @@ -4,7 +4,7 @@ A profile is a set of configuration settings. Currently, INAV gives you three control profiles. The default control profile is `1`. -## Changing contorl profiles +## Changing control profiles ### Stick Commands Control profiles can be selected using a GUI or the following stick combinations: diff --git a/docs/Fixed Wing Landing.md b/docs/Fixed Wing Landing.md index c50bef9c567..74887b30516 100644 --- a/docs/Fixed Wing Landing.md +++ b/docs/Fixed Wing Landing.md @@ -15,7 +15,7 @@ This enables up to 4 different approach directions, based on the landing site an 3. The landing direction and the approach waypoints are calculated on the basis of the measured wind parameters. If no headwind landing is possible or the wind strength is greater than "Max. tailwind" (see Global Parameters), return to point 2. 4. The landing is initiated. The aircraft flies the downwind course, "Approach Altitude" is held. 5. Base Leg: the altitude is reduced from 2/3 of "Approach Altitude". -6. Final Appraoch: The engine power is reduced using "Pitch2throttle modifier" to reduce speed and the altitude is gradually reduced towards "Land Altitude" while approaching the Safehome coordinates. +6. Final Approach: The engine power is reduced using "Pitch2throttle modifier" to reduce speed and the altitude is gradually reduced towards "Land Altitude" while approaching the Safehome coordinates. 7. Glide: When "Glide Altitude" is reached, the motor is switched off and the pitch angle of "Glide Pitch" is held. 7. Flare: Only if a LIDAR/Rangefinder sensor is present: the motor remains switched off and the pitch angle of "Flare Pitch" is held 8. Landing: As soon as INAV has detected the landing, it is automatically disarmed, see setting `nav_disarm_on_landing`. diff --git a/docs/GPS_fix_estimation.md b/docs/GPS_fix_estimation.md index cb3895004c8..258afd7d82b 100644 --- a/docs/GPS_fix_estimation.md +++ b/docs/GPS_fix_estimation.md @@ -1,4 +1,4 @@ -# GPS Fix estimation (dead reconing, RTH without GPS) for fixed wing +# GPS Fix estimation (dead reckoning, RTH without GPS) for fixed wing Video demonstration @@ -12,7 +12,7 @@ Plane should have the following sensors: - acceleromenter, gyroscope - barometer - GPS -- magnethometer (optional, highly recommended) +- magnetometer (optional, highly recommended) - pitot (optional) By befault, all navigation modes are disabled when GPS fix is lost. If RC signal is lost also, plane will not be able to enable RTH. Plane will switch to LANDING instead. When flying above unreachable spaces, plane will be lost. @@ -41,11 +41,11 @@ It is assumed, that plane will fly in roughly estimated direction to home positi *Plane has to acquire GPS fix and store home position before takeoff. Estimation completely without GPS fix will not work*. -# Estimation without magnethometer +# Estimation without magnetometer -Without magnethometer, navigation accuracy is very poor. The problem is heading drift. +Without magnetometer, navigation accuracy is very poor. The problem is heading drift. -The longer plane flies without magnethometer or GPS, the bigger is course estimation error. +The longer plane flies without magnetometer or GPS, the bigger is course estimation error. After few minutes and few turns, "North" direction estimation can be completely broken. In general, accuracy is enough to perform RTH U-turn when both RC controls and GPS are lost, and roughtly keep RTH direction in areas with occasional GPS outages. @@ -54,7 +54,7 @@ In general, accuracy is enough to perform RTH U-turn when both RC controls and G (purple line - estimated position, black line - real position). -It is recommened to use GPS fix estimation without magnethometer as last resort only. For example, if plane is flying above lake, landing means loss of plane. With GPS Fix estimation, plane will try to do RTH in very rought direction, instead of landing. +It is recommened to use GPS fix estimation without magnetometer as last resort only. For example, if plane is flying above lake, landing means loss of plane. With GPS Fix estimation, plane will try to do RTH in very rought direction, instead of landing. It is up to user to estimate the risk of fly-away. diff --git a/docs/LedStrip.md b/docs/LedStrip.md index cecd630ecc9..262d91b183a 100644 --- a/docs/LedStrip.md +++ b/docs/LedStrip.md @@ -71,7 +71,7 @@ Enable the Led Strip feature via the GUI under setup. Configure the leds from the Led Strip tab in the INAV GUI. First setup how the led's are laid out so that you can visualize it later as you configure and so the flight controller knows how many led's there are available. -There is a step by step guide on how to use the GUI to configure the Led Strip feature using the GUI http://blog.oscarliang.net/setup-rgb-led-cleanflight/ which was published early 2015 by Oscar Liang which may or may not be up-to-date by the time you read this. +There is a step by step guide on how to use the GUI to configure the Led Strip feature using the GUI https://oscarliang.com/setup-led-betaflight/ which was published early 2015 by Oscar Liang which may or may not be up-to-date by the time you read this. CLI: Enable the `LED_STRIP` feature via the cli: @@ -605,4 +605,4 @@ This also means that you can make sure that each R,G and B LED in each LED modul After a short delay the LEDs will show the unarmed color sequence and or low-battery warning sequence. -Also check that the feature `LED_STRIP` was correctly enabled and that it does not conflict with other features, as above. \ No newline at end of file +Also check that the feature `LED_STRIP` was correctly enabled and that it does not conflict with other features, as above. diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index cd9702b5a32..685e17a4e70 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -99,7 +99,7 @@ TailSitter is supported by add a 90deg offset to the board alignment. Set the bo - motor_direction_inverted, and more······· - **Motor Mixing (mmix)** - **Servo Mixing (smix)** -- **PID Profile** +- **Control Profile** - PIDs for Roll, Pitch, Yaw - PIDs for Navigation Modes - TPA (Throttle PID Attenuation) Settings diff --git a/docs/OSD.md b/docs/OSD.md index a03f02a9eb6..e64aaef9d9b 100644 --- a/docs/OSD.md +++ b/docs/OSD.md @@ -58,7 +58,7 @@ Here are the OSD Elements provided by INAV. | 23 | OSD_HOME_DIST | 1.6.0 | | | 24 | OSD_HEADING | 1.6.0 | | | 25 | OSD_VARIO | 1.6.0 | | -| 26 | OSD_VARIO_NUM | 1.6.0 | | +| 26 | OSD_VERTICAL_SPEED_INDICATOR | 1.6.0 | | | 27 | OSD_AIR_SPEED | 1.7.3 | | | 28 | OSD_ONTIME_FLYTIME | 1.8.0 | | | 29 | OSD_RTC_TIME | 1.8.0 | | @@ -199,6 +199,7 @@ Here are the OSD Elements provided by INAV. | 164 | OSD_H_DIST_TO_FENCE | 8.0.0 | | | 165 | OSD_V_DIST_TO_FENCE | 8.0.0 | | | 166 | OSD_NAV_FW_ALT_CONTROL_RESPONSE | 8.0.0 | | +| 167 | OSD_NAV_MIN_GROUND_SPEED | 9.0.0 | | # Pilot Logos diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index c5970be0a43..4233f78487d 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -26,6 +26,22 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn **Note:** IPF uses integer math. If your programming line returns a decimal, it will be truncated to an integer. So if your math is `1` / `3` = , IPF will truncate the decimal and return `0`. +## JavaScript-Based Programming (Alternative) + +INAV also supports a JavaScript-based programming interface that provides a more +familiar syntax for those comfortable with JavaScript. The JavaScript code is transpiled +(converted) into traditional logic conditions, so both methods ultimately use the same +underlying system. + +See the [JavaScript Programming Guide](javascript_programming/JAVASCRIPT_PROGRAMMING_GUIDE.md) +for complete documentation on using JavaScript to program your flight controller. + +**Benefits of JavaScript programming:** +- Modern code editor with IntelliSense autocomplete +- Real-time syntax validation and error messages +- Familiar programming constructs (if statements, functions, variables) +- Automatic conversion to logic conditions + ## Logic Conditions ### CLI @@ -102,6 +118,7 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn | 53 | Disable GPS Sensor Fix | Disables the GNSS sensor fix. For testing GNSS failure. | | 54 | Mag calibration | Trigger a magnetometer calibration. | | 55 | Set Gimbal Sensitivity | Scales `Operand A` from [`-16` : `15`] +| 56 | Override Minimum Ground Speed | When active, sets the minimum ground speed to the value specified in `Operand A` [m/s]. Minimum allowed value is set in `nav_min_ground_speed`. Maximum value is `150` | ### Operands @@ -164,6 +181,12 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn | 41 | FW Land Sate | Integer `1` - `5`, indicates the status of the FW landing, 0 Idle, 1 Downwind, 2 Base Leg, 3 Final Approach, 4 Glide, 5 Flare | | 42 | Current battery profile | The active battery profile. Integer `[1..MAX_PROFILE_COUNT]` | | 43 | Flown Loiter Radius [m] | The actual loiter radius flown by a fixed wing during hold modes, in `meters` | +| 44 | Downlink Link Quality | | +| 45 | Uplink RSSI [dBm] | | +| 46 | Minimum Ground Speed [m/s] | The current minimum ground speed allowed in navigation flight modes | +| 47 | Horizontal Wind Speed [cm/s] | Estimated wind speed. If the wind estimator is unavailble or the wind estimation is invalid, -1 is returned | +| 48 | Wind Direction [deg] | Estimated wind direction. If the wind estimator is unavailble or the wind estimation is invalid, -1 is returned | +| 49 | Relative Wind Offset [deg] | The relative offset between the heading of the aircraft and the heading of the wind. 0 indicates flying directly into a headwing. Negative numbers are a left offset. For example, if -20° is shown, turning right will correct towards 0. If the wind estimator is unavailble or the wind estimation is invalid, 0 is returned | #### FLIGHT_MODE @@ -349,3 +372,9 @@ choose *value* and enter the channel number. Choosing "get RC value" is a common which does something other than what you probably want. ![screenshot of override an RC channel with a value](./assets/images/ipf_set_get_rc_channel.png) + +## Related Documentation + +- [JavaScript Programming Guide](javascript_programming/JAVASCRIPT_PROGRAMMING_GUIDE.md) - Alternative JavaScript-based syntax for programming logic conditions +- [Operations Reference](javascript_programming/OPERATIONS_REFERENCE.md) - Complete reference for all supported operations in JavaScript +- [Timer and Change Detection Examples](javascript_programming/TIMER_WHENCHANGED_EXAMPLES.md) - Practical examples for time-based patterns diff --git a/docs/Rx.md b/docs/Rx.md index c52fe151571..43f9d852716 100644 --- a/docs/Rx.md +++ b/docs/Rx.md @@ -200,7 +200,7 @@ Note: * It is necessary to update `MSP_SET_RAW_RC` at 5Hz or faster. * `MSP_SET_RAW_RC` uses the defined RC channel map * `MSP_RC` returns `AERT` regardless of channel map -* You can combine "real" RC radio and MSP RX by compiling custom firmware with `USE_MSP_RC_OVERRIDE` defined. Then use `msp_override_channels` to set the channels to be overridden. +* You can combine "real" RC radio and MSP RX by using `msp_override_channels` to set the channels to be overridden. * The [wiki Remote Control and Management article](https://github.com/iNavFlight/inav/wiki/INAV-Remote-Management,-Control-and-Telemetry) provides more information, including links to 3rd party projects that exercise `MSP_SET_RAW_RC` and `USE_MSP_RC_OVERRIDE` ## SIM (SITL) Joystick diff --git a/docs/Settings.md b/docs/Settings.md index a357b5f1e51..b886f6702ac 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -432,6 +432,16 @@ Max Antigravity gain. `1` means Antigravity is disabled, `2` means Iterm is allo --- +### apa_pow + +Use airspeed instead of throttle position for PID attenuation if airspeed is available on fixedwing. pid_multiplier = (referenceAirspeed/airspeed)**(apa_pow/100). Set to 0 will disable this feature and use throttle based PID attenuation; + +| Default | Min | Max | +| --- | --- | --- | +| 120 | 0 | 200 | + +--- + ### applied_defaults Internal (configurator) hint. Should not be changed manually @@ -542,6 +552,16 @@ Allows disabling PWM mode for beeper on some targets. Switch from ON to OFF if t --- +### blackbox_arm_control + +Determines behaviour of logging in relation to Arm state. For settings from 0 to 60 logging will start on Arm with the setting determining how long logging will continue after disarm in seconds, i.e. set to 0 to stop logging at disarm or 10 to stop logging 10s after disarm. Set to -1 to start logging from boot up until power off (Use with caution - mainly for debugging and best used with BLACKBOX mode). + +| Default | Min | Max | +| --- | --- | --- | +| 0 | -1 | 60 | + +--- + ### blackbox_device Selection of where to write blackbox data @@ -572,16 +592,6 @@ Blackbox logging rate numerator. Use num/denom settings to decide if a frame sho --- -### controlrate_profile - -Control rate profile to switch to when the battery profile is selected, 0 to disable and keep the currently selected control rate profile - -| Default | Min | Max | -| --- | --- | --- | -| 0 | 0 | 3 | - ---- - ### cruise_power Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit @@ -624,7 +634,7 @@ This sets the output voltage to current scaling for the current sensor in 0.1 mV ### current_meter_type -ADC , VIRTUAL, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position. +ADC, VIRTUAL, FAKE, ESC, SMARTPORT, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position. | Default | Min | Max | | --- | --- | --- | @@ -2238,7 +2248,7 @@ Weight of barometer climb rate measurements in estimated climb rate. Setting is | Default | Min | Max | | --- | --- | --- | -| 0.1 | 0 | 10 | +| 0.35 | 0 | 10 | --- @@ -2248,7 +2258,7 @@ Weight of GPS altitude measurements in estimated altitude. Setting is used on bo | Default | Min | Max | | --- | --- | --- | -| 0.2 | 0 | 10 | +| 0.35 | 0 | 10 | --- @@ -2258,7 +2268,7 @@ Weight of GPS climb rate measurements in estimated climb rate. Setting is used o | Default | Min | Max | | --- | --- | --- | -| 0.1 | 0 | 10 | +| 0.35 | 0 | 10 | --- @@ -2594,7 +2604,7 @@ Servo travel multiplier for the PITCH axis in `MANUAL` flight mode [0-100]% ### manual_rc_expo -Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100] +Exponential value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100] | Default | Min | Max | | --- | --- | --- | @@ -2604,7 +2614,7 @@ Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100 ### manual_rc_yaw_expo -Exposition value used for the YAW axis by the `MANUAL` flight mode [0-100] +Exponential value used for the YAW axis by the `MANUAL` flight mode [0-100] | Default | Min | Max | | --- | --- | --- | @@ -2632,6 +2642,16 @@ Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]% --- +### mavlink_autopilot_type + +Autopilot type to advertise for MAVLink telemetry + +| Default | Min | Max | +| --- | --- | --- | +| GENERIC | | | + +--- + ### mavlink_ext_status_rate Rate of the extended status message for MAVLink telemetry @@ -2712,6 +2732,16 @@ Rate of the RC channels message for MAVLink telemetry --- +### mavlink_sysid + +MAVLink System ID + +| Default | Min | Max | +| --- | --- | --- | +| 1 | 1 | 255 | + +--- + ### mavlink_version Version of MAVLink to use @@ -2962,9 +2992,9 @@ If set to on, This mixer_profile will try to switch to another mixer_profile whe --- -### mixer_pid_profile_linking +### mixer_control_profile_linking -If enabled, pid profile_index will follow mixer_profile index. Set to OFF(default) if you want to handle PID profile by your self. Recommend to set to ON on all mixer_profiles to let the mixer_profile handle the PID profile switching on a VTOL or mixed platform type setup. +If enabled, control_profile_index will follow mixer_profile index. Set to OFF(default) if you want to handle control_profile by your self. Recommend to set to ON on all mixer_profiles to let the mixer_profile handle the control_profile switching on a VTOL or mixed platform type setup. | Default | Min | Max | | --- | --- | --- | @@ -3054,11 +3084,11 @@ If enabled, motor will stop when throttle is low on this mixer_profile ### msp_override_channels -Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires custom firmware with `USE_RX_MSP` and `USE_MSP_RC_OVERRIDE` compile options and the `MSP RC Override` flight mode. +Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires the `MSP RC Override` flight mode. | Default | Min | Max | | --- | --- | --- | -| 0 | 0 | 65535 | +| 0 | 0 | 4294967295 | --- @@ -3152,6 +3182,16 @@ Adjusts the deceleration response of fixed wing altitude control as the target a --- +### nav_fw_alt_use_position + +Use position for fixed wing altitude control rather than velocity (default method). + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### nav_fw_auto_climb_rate Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] @@ -3634,11 +3674,11 @@ D gain of altitude PID controller (Fixedwing) ### nav_fw_pos_z_ff -FF gain of altitude PID controller (Fixedwing) +FF gain of altitude PID controller. Not used if nav_fw_alt_use_position is set ON (Fixedwing) | Default | Min | Max | | --- | --- | --- | -| 10 | 0 | 255 | +| 30 | 0 | 255 | --- @@ -4472,6 +4512,16 @@ Ignore adsb planes above, limit, 0 disabled (meters) --- +### osd_adsb_warning_style + +ADSB warning element style, how rich information on scree will be, Possible values are `COMPACT` and `EXTENDED` + +| Default | Min | Max | +| --- | --- | --- | +| COMPACT | | | + +--- + ### osd_ahi_bordered Shows a border/corners around the AHI region (pixel OSD only) @@ -5142,9 +5192,9 @@ Degrees either side of the pan servo centre; where it is assumed camera is wante --- -### osd_pan_servo_pwm2centideg +### osd_pan_servo_range_decadegrees -Centidegrees of pan servo rotation us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. +Decadegrees of pan servo rotation. A servo with 180 degrees of rotation typically needs `18` for this setting. Using a negative value will reverse the direction. | Default | Min | Max | | --- | --- | --- | @@ -5288,7 +5338,7 @@ Value below which Crossfire SNR Alarm pops-up. (dB) | Default | Min | Max | | --- | --- | --- | -| 4 | -20 | 99 | +| 4 | -20 | 30 | --- @@ -5484,7 +5534,7 @@ Use custom pilot logo with/instead of the INAV logo. The pilot logo must be char ### osd_video_system -Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, 'DJIWTF', 'AVATAR' and `BF43COMPAT` +Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, 'DJIWTF', 'AVATAR', `BF43COMPAT`, `BFHDCOMPAT` and `DJI_NATIVE` | Default | Min | Max | | --- | --- | --- | @@ -5584,11 +5634,11 @@ Selection of pitot hardware. ### pitot_lpf_milli_hz -Pitot tube lowpass filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay +Pitot tube lowpass filter cutoff frequency in milli Hz(0.001hz). Set as 0 to disable LPF Lower cutoff frequencies result in smoother response at expense of command control delay | Default | Min | Max | | --- | --- | --- | -| 350 | 0 | 10000 | +| 3000 | 0 | 50000 | --- @@ -5734,7 +5784,7 @@ The end stick weight for Rate Dynamics ### rc_expo -Exposition value used for the PITCH/ROLL axes by all the stabilized flights modes (all but `MANUAL`) +Exponential value used for the PITCH/ROLL axes by all the stabilized flights modes (all but `MANUAL`) | Default | Min | Max | | --- | --- | --- | @@ -5774,7 +5824,7 @@ The RC filter smoothing factor. The higher the value, the more smoothing but als ### rc_yaw_expo -Exposition value used for the YAW axis by all the stabilized flights modes (all but `MANUAL`) +Exponential value used for the YAW axis by all the stabilized flights modes (all but `MANUAL`) | Default | Min | Max | | --- | --- | --- | @@ -5782,16 +5832,6 @@ Exposition value used for the YAW axis by all the stabilized flights modes (all --- -### reboot_character - -Special character used to trigger reboot - -| Default | Min | Max | -| --- | --- | --- | -| 82 | 48 | 126 | - ---- - ### receiver_type Selection of receiver (RX) type. Additional configuration of a `serialrx_provider` and a UART will be needed for `SERIAL` @@ -6242,6 +6282,16 @@ General switch of the statistics recording feature (a.k.a. odometer) --- +### stats_flight_count + +Total number of flights. The value is updated on every disarm when "stats" are enabled. + +| Default | Min | Max | +| --- | --- | --- | +| 0 | | 65535 | + +--- + ### stats_total_dist Total flight distance [in meters]. The value is updated on every disarm when "stats" are enabled. @@ -6334,7 +6384,7 @@ Weight used for the throttle compensation based on battery voltage. See the [bat ### thr_expo -Throttle exposition value +Throttle exponential value | Default | Min | Max | | --- | --- | --- | @@ -6402,13 +6452,53 @@ Throttle PID attenuation also reduces influence on YAW for multi-rotor, Should b --- +### tpa_pitch_compensation + +Pitch angle based throttle compensation for fixed wing. Positive values will increase throttle when pitching up, and decrease throttle when pitching down. + +| Default | Min | Max | +| --- | --- | --- | +| 8 | 0 | 20 | + +--- + ### tpa_rate -Throttle PID attenuation reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. +Throttle based PID attenuation(TPA) reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. On multirotor, For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. for fixedwing modifies PIDFF. See **PID Attenuation and scaling** Wiki for full details. | Default | Min | Max | | --- | --- | --- | -| 0 | 0 | 100 | +| 0 | 0 | 200 | + +--- + +### transition_pid_mmix_multiplier_pitch + +pitch axis pid multiplier applied to motor mixer only on mixer trasition mode, 1000(default) is 1.0x, -1000 is 1.0x on opposite + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | -5000 | 5000 | + +--- + +### transition_pid_mmix_multiplier_roll + +roll axis pid multiplier applied to motor mixer only on mixer trasition mode, 1000(default) is 1.0x, -1000 is 1.0x on opposite + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | -5000 | 5000 | + +--- + +### transition_pid_mmix_multiplier_yaw + +yaw axis pid multiplier applied to motor mixer only on mixer trasition mode, 1000(default) is 1.0x, -1000 is 1.0x on opposite + +| Default | Min | Max | +| --- | --- | --- | +| 1000 | -5000 | 5000 | --- @@ -6452,6 +6542,16 @@ Time zone offset from UTC, in minutes. This is applied to the GPS time for loggi --- +### use_control_profile + +Control profile to switch to when the battery profile is selected, 0 to disable and keep the currently selected control profile + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 3 | + +--- + ### vbat_adc_channel ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled @@ -6484,7 +6584,7 @@ Maximum voltage per cell in 0.01V units, default is 4.20V ### vbat_meter_type -Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running +Vbat voltage source. Possible values: `NONE`, `ADC`, `SMARTPORT`, `ESC`. `ESC` requires ESC telemetry enabled and running. `SMARTPORT` requires SmartPort Master enabled and running. | Default | Min | Max | | --- | --- | --- | diff --git a/docs/Telemetry.md b/docs/Telemetry.md index d5d448f55c6..67e43312406 100644 --- a/docs/Telemetry.md +++ b/docs/Telemetry.md @@ -102,12 +102,14 @@ The following sensors are transmitted * **VSpd** : vertical speed, unit is cm/s. * **Hdg** : heading, North is 0°, South is 180°. * **AccX,Y,Z** : accelerometer values (not sent if `frsky_pitch_roll = ON`). -* **470** : flight mode, sent as 5 digits. Number is sent as **ABCDE** detailed below. The numbers are additives (for example: if digit C is 6, it means both position hold and altitude hold are active) : - * **A** : 1 = flaperon mode, 2 = auto tune mode, 4 = failsafe mode - * **B** : 1 = return to home, 2 = waypoint mode, 4 = headfree mode - * **C** : 1 = heading hold, 2 = altitude hold, 4 = position hold - * **D** : 1 = angle mode, 2 = horizon mode, 4 = passthru mode - * **E** : 1 = ok to arm, 2 = arming is prevented, 4 = armed +* **470** : flight mode, sent as 7 digits. Number is sent as **ABCDEFG** detailed below. The numbers are additives (for example: if digit C is 6, it means both position hold and altitude hold are active) : + * **A** : 1 = WRTH mode, 2 = Angle hold mode + * **B** : 1 = Fixed Wing Auto-land, 2 = Turtle mode, 4 = Geofence action mode, 8 = Loiter mode + * **C** : 1 = flaperon mode, 2 = auto tune mode, 4 = failsafe mode + * **D** : 1 = return to home, 2 = waypoint mode, 4 = headfree mode, 8 = Course Hold + * **E** : 1 = heading hold, 2 = altitude hold, 4 = position hold + * **F** : 1 = angle mode, 2 = horizon mode, 4 = passthru mode + * **G** : 1 = ok to arm, 2 = arming is prevented, 4 = armed _NOTE_ This sensor used to be **Tmp1**. The ID has been reassigned in INAV 8.0. The old ID of **Tmp1** can still be used, by using `set frsky_use_legacy_gps_mode_sensor_ids = ON`. This is deprecated and will be removed in INAV 10.0. All tools and scripts using the old IDs should be updated to use the new ID. * **480** : GPS lock status, accuracy, home reset trigger, and number of satellites. Number is sent as **ABCD** detailed below. Typical minimum GPS 3D lock value is 3906 (GPS locked and home fixed, HDOP highest accuracy, 6 satellites). diff --git a/docs/VTOL.md b/docs/VTOL.md index 025a7711c68..8341c81086d 100644 --- a/docs/VTOL.md +++ b/docs/VTOL.md @@ -22,7 +22,7 @@ We highly value your feedback as it plays a crucial role in the development and # VTOL Configuration Steps -### The VTOL functionality is achieved by switching/transitioning between two configurations stored in the FC. VTOL specific configurations are Mixer Profiles with associated PID profiles. One profile set is for fixed-wing(FW) mode, One is for multi-copter(MC) mode. Configuration/Settings other than Mixer/PID profiles are shared among two modes +### The VTOL functionality is achieved by switching/transitioning between two configurations stored in the FC. VTOL specific configurations are Mixer Profiles with associated control profiles. One profile set is for fixed-wing(FW) mode, One is for multi-copter(MC) mode. Configuration/Settings other than Mixer/control profiles are shared among two modes ![Alt text](Screenshots/mixerprofile_flow.png) 0. **Find a DIFF ALL file for your model and start from there if possible** @@ -78,7 +78,7 @@ set platform_type = TRICOPTER set model_preview_type = 1 set mixer_pid_profile_linking = ON -profile 1 #pid profile +profile 1 #control profile set dterm_lpf_hz = 10 set d_boost_min = 1.000 set d_boost_max = 1.000 @@ -115,11 +115,12 @@ save # STEP 1: Configuring as a normal fixed-wing in Profile 1 -1. **Select the fisrt Mixer Profile and PID Profile:** - - In the CLI, switch to the mixer_profile and pid_profile you wish to set first. You can also switch mixer_profile/pid_profile through gui if with aforementioned presets loaded. +1. **Select the first Mixer Profile and Control Profile:** + - In the CLI, switch to the mixer_profile and control_profile you wish to set first. You can also switch + mixer_profile/control_profile through gui with aforementioned presets loaded. ``` mixer_profile 1 #in this example, we set profile 1 first - set mixer_pid_profile_linking = ON # Let the mixer_profile handle the pid_profile switch on this mixer_profile + set mixer_pid_profile_linking = ON # Let the mixer_profile handle the control profile (formerly pid_profile) switch on this mixer_profile set platform_type = AIRPLANE save ``` @@ -132,11 +133,13 @@ save ![Alt text](Screenshots/mixerprofile_fw_mixer.png) +You must also assign the tilting servos values using the MAX values. If you don't do this the motors will point in the direction assigned by the transition mode. # STEP 2: Configuring as a Multi-Copter in Profile 2 -1. **Switch to Another Mixer Profile with PID Profile:** - - In the CLI, switch to another mixer_profile along with the appropriate pid_profile. You can also switch mixer_profile/pid_profile through gui if with aforementioned presets loaded. +1. **Switch to Another Mixer Profile with Control Profile:** + - In the CLI, switch to another mixer_profile along with the appropriate control profile. You can also switch + mixer_profile/control_profile through gui with aforementioned presets loaded. ``` mixer_profile 2 set mixer_pid_profile_linking = ON @@ -145,10 +148,10 @@ save ``` 2. **Configure the Multicopter/tricopter:** - - Set up your multi-copter/fixed-wing as usual, this time for mixer_profile 2 and pid_profile 2. + - Set up your multi-copter/fixed-wing as usual, this time for mixer_profile 2 and control_profile 2. - Utilize the 'MAX' input in the servo mixer to tilt the motors without altering the servo midpoint. - At this stage, focus on configuring profile-specific settings. You can streamline this process by copying and pasting the default PID settings. - - you can set -1 in motor mixer throttle as a place holder: disable that motor but will load following motor rules + - you can set -1 in motor mixer throttle as a place holder: this will disable that motor but will load following the motor rules - compass is required to enable navigation modes for multi-rotor profile. - Consider conducting a test flight to ensure that everything operates as expected. And tune the settings. - It is advisable to have a certain degree of control surface (elevon / elevator) mapping for stabilization even in multi-copter mode. This helps improve control authority when airspeed is high. It might be unable to recover from a dive without them. @@ -157,19 +160,21 @@ save 5. **Tailsitters:planned for INAV 7.1** - Configure the fixed-wing mode/profile sets normally. Use MultiCopter platform type for tail_sitting flying mode/profile sets. - - The baseline board aliment is FW mode(ROLL axis is the trust axis). So set `tailsitter_orientation_offset = ON ` in the tail_sitting MC mode. - - Configure mixer ROLL/YAW mixing according to tail_sitting orientation in the tail_sitting MC mode. YAW axis is the trust axis. + - The baseline board aliment is FW mode (ROLL axis is the thrust axis). Set `tailsitter_orientation_offset = ON ` in the tail_sitting MC mode. + - Configure mixer ROLL/YAW mixing according to tail_sitting orientation in the tail_sitting MC mode. YAW axis is the thrust axis. - Conduct a bench test and see the orientation of the model changes in inav-configurator setup tab + # STEP 3: Mode Tab Settings: -### We recommend using an 3-pos switch on you radio to activate these modes, So pilot can jump in or bell out at any moment. +### We recommend using an 3-pos switch on you radio to activate these modes, So the pilot can jump in or bail out at any moment. + ### Here is a example, in the bottom of inav-configurator Modes tab: ![Alt text](Screenshots/mixer_profile.png) | 1000~1300 | 1300~1700 | 1700~2000 | | :-- | :-- | :-- | | Profile1(FW) with transition off | Profile2(MC) with transition on | Profile2(MC) with transition off | -- Profile file switching becomes available after completing the runtime sensor calibration(15-30s after booting). And It is **not available** when a navigation mode or position hold is active. +- Profile file switching becomes available after completing the runtime sensor calibration (15-30s after booting). And It is **not available** when a navigation mode or position hold is active. - By default, `mixer_profile 1` is used. `mixer_profile 2` is used when the `MIXER PROFILE 2` mode is activate. Once configured successfully, you will notice that the profiles and model preview changes accordingly when you refresh the relevant INAV Configurator tabs. @@ -177,17 +182,19 @@ save Conduct a bench test on the model (without props attached). The model can now switch between fixed-wing and multi-copter modes while armed. Furthermore, it is capable of mid-air switching, resulting in an immediate stall upon entering fixed-wing profile + # STEP 4: Tilting Servo Setup (Recommended) -### Setting up the tilting servos to operate correclty is crucial for correct yaw control of the craft. Using the default setup works, but will most likely result in your craft crawling forward with evey yaw input. -The steps below describe how you can fine-tune the tilting servos such as to obtian the desired result. +### Setting up the tilting servos to operate correctly is crucial for correct yaw control of the craft. Using the default setup works, but will most likely result in your craft crawling forward with evey yaw input. +The steps below describe how you can fine-tune the tilting servos to obtian the optimum result. + 1. **Set the tilt servos at 45 degrees:** - Connect and power the tilting servos with your flight controller. - Enter transition mode (your switch should be in the mid-position). - - Check from the Outputs tab output that the tilt servo channels are exactly at 1500μs. + - Check the Outputs tab and make sure that the tilt servo channels are exactly at 1500μs. - In this mode, your tilt servos should be at the 45-degree position and you can now mount the motor and prop to your tilt servo such that the angle of the motor mounting plate is at 45 degrees upwards. - - NOTE1: If you have dedicated tilt servos, you may have engraved indeces on the servos and tilting motor sassembly to help you with this step. If the servos don't end up exactly at 45 degrees due to the teeth on the servo and the control arm/plate, don't worry, this will be automatically adjusted after completing the other steps below. - - NOTE2: If you are using control rods to adjust the tilt of the servos, adjust the lenth of your control rod and the position of the control arm position the control arm as close as possible to the mid position. It will depend on the oriatation of the servo, but generally speaking, the control arm of the servo should be pointed perpendicular to the fuselage when the motor mounts are at the 45 degree setting. + - NOTE 1: If you have dedicated tilt servos, you may have engraved indices on the servos and tilting motor assembly to help you with this step. If the servos don't end up exactly at 45 degrees due to the teeth on the servo and the control arm/plate, don't worry, this will be automatically adjusted after completing the other steps below. + - NOTE 2: If you are using control rods to adjust the tilt of the servos, adjust the lenth of your control rod and the position of the control arm to position the control arm as close as possible to the mid position. It will depend on the oriatation of the servo, but generally speaking, the control arm of the servo should be pointed perpendicular to the fuselage when the motor mounts are at the 45 degree setting. 2. **Switch to Multicopter/Tricopter:** - Assuming that you have set up your mixer similar to STEP1 and STEP2, you can now switch to the tricopter/multicopter mode and your servos should be tilting the motors upwards. If this is not the case, reverse the servo(s) in the Outputs tab such that the servo(s) is/are pointed upwards. @@ -195,9 +202,9 @@ The steps below describe how you can fine-tune the tilting servos such as to obt - Also, ensure that your MAX values in the Mixer tab are at 100 and -100, so that your servo will move to the maximum position, as shown in the screenshots in STEP1 and STEP2. 3. **Adjust the maximum throws for the Multicopter/Tricopter mode:** - - While in tricopter mode, go to the Outputs tab and adjust the endpoints MIN and MAX values such that when your motors are pointed slightly backwards. + - While in tricopter mode, navigate to the Outputs tab and adjust the MIN and MAX endpoint values to position the motors slightly backward. - Rotate the prop such that it is pointed backwards towards the wing/motor mount and ensure that the gap is the same on both sides by adjusting the MIN and MAX values for the tilt servo channels. - - NOTE: You can check the distance with calipers or gauge blocks. Alternatively, you can adjust the MIN and MAX for your tilting servos such that the props are just touching the top of the wing or motor mount, and then you can increase/degrease the MIN and MAX values for each channel by the same ammount for both servos. This should ensure that you have the same gap between the tip of the prop and the wing or motor mount for both sides. + - NOTE: You can check the distance with calipers or gauge blocks. Alternatively, you can adjust the MIN and MAX for your tilting servos such that the props are just touching the top of the wing or motor mount, and then you can increase/decrease the MIN and MAX values for each channel by the same ammount for both servos. This should ensure that you have the same gap between the tip of the prop and the wing or motor mount for both sides. 4. **Adjust the minimum position for the Fixed-wing mode:** - Repeat the same step as point 3 with the model in fixed-wing mode, where the servos are tilted forwards. @@ -226,7 +233,7 @@ The steps below describe how you can fine-tune the tilting servos such as to obt Optional Setup Step for Tilt Servos: 8. **Reversing tilt servos and mixer signs:** -If you have set up the mixer as suggested in STEP1 and STEP2, you may have to deal with negative values for the mixer. You may wish to reverese a servo so that you don't have to deal with the negative signs. In that case, you may have to adjust the MIN and MAX values from point 4 again, so that your tilt servos are operating correctly. Check the operation of the servos once again for the YAW control in multicopter/tricipter mode as well as the horixontal position of the tilt servos in fixed-wing mode. +If you have set up the mixer as suggested in STEP1 and STEP2, you may have to deal with negative values for the mixer. You may wish to reverese a servo so that you don't have to deal with the negative signs. In that case, you may have to adjust the MIN and MAX values from point 4 again, so that your tilt servos are operating correctly. Check the operation of the servos once again for the YAW control in multicopter/tricipter mode as well as the horizontal position of the tilt servos in fixed-wing mode. # STEP 5: Transition Mixing (Multi-Rotor Profile)(Recommended) @@ -241,12 +248,12 @@ Add new servo mixer rules, and select 'Mixer Transition' in input. Set the weigh ## Motor 'Transition Mixing': Dedicated forward motor configuration In motor mixer set: - -2.0 < throttle < -1.0: The motor will spin regardless of the radio's throttle position at a speed of `abs(throttle) - 1` multiplied by throttle range only when Mixer Transition is activated. -- Airmode type should be set to "STICK_CENTER". Airmode type must NOT be set to "THROTTLE_THRESHOLD". If set to throttle threshold the (-) motor will spin till throttle threshold is passed. +- Airmode type should be set to "STICK_CENTER". Airmode type must NOT be set to "THROTTLE_THRESHOLD". If set to throttle threshold the (-) motor will spin until the throttle threshold is passed. ![Alt text](Screenshots/mixerprofile_4puls1_mix.png) ## TailSitter 'Transition Mixing': -No additional settings needed, 45deg off set will be added to target pitch angle for angle mode in the firmware. +No additional settings needed, 45 deg offset will be added to target pitch angle for angle mode in the firmware. ### With aforementioned settings, your model should be able to enter fixed-wing profile without stalling. diff --git a/docs/assets/images/adsb-info.png b/docs/assets/images/adsb-info.png new file mode 100644 index 00000000000..ce5f41a635e Binary files /dev/null and b/docs/assets/images/adsb-info.png differ diff --git a/docs/boards/FlyingRC F4Wing Mini.md b/docs/boards/FlyingRC F4Wing Mini.md new file mode 100644 index 00000000000..b51907870dc --- /dev/null +++ b/docs/boards/FlyingRC F4Wing Mini.md @@ -0,0 +1,42 @@ +# Board - FLYINGRCF4WINGMINI_NOT_RECOMMENDED + +This is a cheap flight controller (prices range from $16US to $40US) from an unknown company. Many of the components on this FC are likely to have high tollerances due to the low cost. They sold this FC as compatible with INAV without reaching out to the team or having an official target made. The target only exists thanks to a community contributor (dixi83). + +Hardware issues have been reported on these flight controllers. They are also missing many features. Unlike most other _wing_ flight controllers. This is not an all in one solution. It requires an external power source for servos. So is not as small or light as it first appears. + +> [!WARNING] +> We recommend you only use this flight controller on very light aircraft that you will keep within line-of-sight distances. Reliability of the hardware is far from guaranteed. So fitting to a larger, heavier aircraft adds unneccesary safety risks. Also, there are essential features missing for other types of flights. Please keep this for small park fliers only, if used at all. +> +> Also, if you insist on buying one of these. Make sure it's from a somewhere selling it at $16US. Spending $40US on this is a waste of money. You can get better FCs for around that money. + +## Specifications +| | | +|-----|-----| +| MCU | STM32F405RTG6 | +| Gyro | ICM-42605 | +| Baro | SPL06 | +| UARTS | 1, 2 (RX only - SBUS), 4 (DJI), 5 | +| PWM | Six + One (S12 used for LED control) | +| I2C | 1 | +| Size | 27.9 x 20.3 x 11.2 mm | +| Weight | 2.8g | + +> [!NOTE] +> There is conflicting information for the power this FC can handle. There are 2 specs providied: +> | | | +> |---|---| +> | Voltmeter | 2.5-30V | +> | Power input | 5v | +> +> There is no ADC for "voltmeter" input. So potentially this FC can run at 1S to 6S. However there are only 2 LDO regulators on the FC itself. + +## Notable missing features +* Current sensor +* Blackbox recording +* Analogue OSD +* PINIO (for VTX power switching etc) +* ADCs (external current sensor, airspeed sensor, rssi, etc) +* On-board power rail for servos +* Filtered power for video + +All the above can be found on the Matek F405-WMO. Which is a $45US flight controller. Which is 32 x 22 x 12.7 mm and 9g, and has a definite input voltage range of 2S to 6S, and able to handle up to 132A. \ No newline at end of file diff --git a/docs/development/Building SITL.md b/docs/development/Building SITL.md index 33ba7ee3d5c..bd7eff88d7b 100644 --- a/docs/development/Building SITL.md +++ b/docs/development/Building SITL.md @@ -1,6 +1,14 @@ ## Building SITL +### Compiler requirements + +* Modern GCC. Must be a *real* GCC, unless you're using MacOS; faking it with clang will not work. gcc13 or later is recommended. +* Unix sockets networking. Cygwin is required on Windows (vice `winsock`). +* Pthreads + + ### Linux and FreeBSD: + Almost like normal, ruby, cmake and make are also required. With cmake, the option "-DSITL=ON" must be specified. @@ -12,12 +20,17 @@ make ``` ### Windows: -Compile under cygwin, then as in Linux. + +Compile under cygwin, using the Linux instructions. Note that depending on the Cygwin packaging _du jour_ of `ruby` it _may_ also be necessary to: + +* Install the `rubygems` package. +* run `gem install getoptlong` + Copy cygwin1.dll into the directory, or include cygwin's /bin/ directory in the environment variable PATH. If the build fails (segfault, possibly out of memory), adding `-DCMAKE_BUILD_TYPE=MinRelSize` to the `cmake` command may help. -#### Build manager +### Build manager `ninja` may also be used (parallel builds without `-j $(nproc)`): @@ -26,14 +39,9 @@ cmake -GNinja -DSITL=ON .. ninja ``` -### Compiler requirements - -* Modern GCC. Must be a *real* GCC, macOS faking it with clang will not work. GCC 10 to GCC 13 are known to work. -* Unix sockets networking. Cygwin is required on Windows (vice `winsock`). -* Pthreads - -## Supported environments +### Supported environments -* Linux on x86_64, ia-32, Aarch64 (e.g. Rpi4), RISCV64 (e.g. VisionFive2) +* Linux on x86_64, ia-32, Aarch64 (e.g. Rpi), RISCV64 (e.g. VisionFive2) * Windows on x86_64 -* FreeBSD (x86_64 at least). +* FreeBSD x86_64 (at least). +* MacOS on x86_64 and Aarch64 diff --git a/docs/development/Building in Github Codespace.md b/docs/development/Building in Github Codespace.md new file mode 100644 index 00000000000..112e4aa4559 --- /dev/null +++ b/docs/development/Building in Github Codespace.md @@ -0,0 +1,49 @@ +# Building in GitHub Codespaces +> **A codespace is a cloud-hosted development environment.** +>
This guide provides a simple method to modify and build a version of INAV in the cloud. + +## Setup +1. Navigate to the version of INAV you want to modify. +2. Create a new codespace for that version. `Code> Codespaces> Create codespace on 8.x.x.` +image + +> A new codespace will launch in your browser. + + +## Modify +3. Modify the code as required. + You can freeley modify your copy of the INAV code from within the cloud environment. + +## Build +Use the terminal inside the codespace environment to run the following commands: + +### Prepare +#### 4. Prepare the build: +```bash +cmake -B build -S . +``` +image + +> Note: You may need to use ctrl+C to exit the process and return to the shell once it completes. + +### Target +#### 5. (Optional) If you don't know your target name, list the availble targests with: +```bash +cmake --build build --target help +``` +image + +### Build +#### 6. Build the binary (replace the target name with your specific one): +``` bash +cmake --build build --target NEUTRONRCF435MINI +``` +image + +## Download +7. Once the build process has complete, download the .hex binary from the build folder on the right. + Example: `inav_8.0.1_NEUTRONRCF435MINI.hex` (Right-click > Download. +image + +> Note: Codespaces are automatically deleted after a period of inactivity, or you can manually deleted them at https://github.com/codespaces + diff --git a/docs/development/Development.md b/docs/development/Development.md index 03dbc5b0952..de8fbdecbfe 100755 --- a/docs/development/Development.md +++ b/docs/development/Development.md @@ -107,11 +107,13 @@ The main flow for a contributing is as follows: 7. `git add ` 8. `git commit` 9. `git push origin my-new-code` -10. Create pull request using github UI to merge your changes from your new branch into `inav/master` +10. Create pull request using github UI to merge your changes from your new branch into the appropriate target branch (see "Branching and release workflow" below) 11. Repeat from step 4 for new other changes. The primary thing to remember is that separate pull requests should be created for separate branches. Never create a pull request from your `master` branch. +**Important:** Most contributions should target a maintenance branch, not `master`. See the branching section below for guidance on choosing the correct target branch. + Later, you can get the changes from the INAV repo into your `master` branch by adding INAV as a git remote and merging from it as follows: 1. `git remote add upstream https://github.com/iNavFlight/inav.git` @@ -125,10 +127,105 @@ You can also perform the git commands using the git client inside Eclipse. Refe ## Branching and release workflow -Normally, all development occurs on the `master` branch. Every release will have it's own branch named `release_x.y.z`. +INAV uses maintenance branches for active development and releases. The `master` branch receives merges from maintenance branches. + +### Branch Types + +#### Maintenance Branches (Current and Next Major Version) + +**Current version branch** (e.g., `maintenance-9.x`): +- Used for backward-compatible changes +- Bug fixes, new features, and improvements that don't break compatibility +- Changes here will be included in the next release of the current major version (e.g., 9.1, 9.2) +- Does not create compatibility issues between firmware and configurator within the same major version + +**Next major version branch** (e.g., `maintenance-10.x`): +- Used for changes that introduce compatibility requirements +- Breaking changes that would cause issues between different major versions +- New features that require coordinated firmware and configurator updates +- Changes here will be included in the next major version release (e.g., 10.0) + +#### Master Branch + +The `master` branch receives periodic merges from maintenance branches. + +### Choosing the Right Target Branch + +When creating a pull request, target the appropriate branch: + +**Target the current version branch** (e.g., `maintenance-9.x`) if your change: +- Fixes a bug +- Adds a new feature that is backward-compatible +- Updates documentation +- Adds or updates hardware targets +- Makes improvements that work with existing releases + +**Target the next major version branch** (e.g., `maintenance-10.x`) if your change: +- Breaks compatibility with the current major version +- Requires coordinated firmware and configurator updates +- Changes MSP protocol in an incompatible way +- Modifies data structures in a breaking way + +### Release Workflow + +1. Development occurs on the current version maintenance branch (e.g., `maintenance-9.x`) +2. When ready for release, a release candidate is tagged from the maintenance branch +3. Bug fixes during the RC period continue on the maintenance branch +4. After final release, the maintenance branch is periodically merged into `master` +5. The cycle continues with the maintenance branch receiving new changes for the next release + +### Propagating Changes Between Maintenance Branches + +Changes committed to the current version branch should be merged forward to the next major version branch to prevent regressions. -During release candidate cycle we will follow the process outlined below: +**Maintainer workflow:** +- Changes merged to `maintenance-9.x` should be regularly merged into `maintenance-10.x` +- This ensures fixes and features aren't lost when the next major version is released +- Prevents users from experiencing bugs in v10.0 that were already fixed in v9.x -1. Create a release branch `release_x.y.z` -2. All bug fixes found in the release candidates will be merged into `release_x.y.z` branch and not into the `master`. -3. After final release is made, the branch `release_x.y.z` is locked, and merged into `master` bringing all of the bug fixes into the development branch. Merge conflicts that may arise at this stage are resolved manually. +**Example:** +```bash +# Merge changes from current to next major version +git checkout maintenance-10.x +git merge maintenance-9.x +git push upstream maintenance-10.x +``` + +### Example Timeline + +Current state (example): +- `maintenance-9.x` - Active development for INAV 9.1, 9.2, etc. +- `maintenance-10.x` - Breaking changes for future INAV 10.0 +- `master` - Receives periodic merges from maintenance branches + +After INAV 10.0 is released: +- `maintenance-10.x` - Active development for INAV 10.1, 10.2, etc. +- `maintenance-11.x` - Breaking changes for future INAV 11.0 +- `master` - Continues receiving merges + +### Working with Maintenance Branches + +To branch from the current maintenance branch instead of master: + +```bash +# Fetch latest changes +git fetch upstream + +# Create your feature branch from the maintenance branch +git checkout -b my-new-feature upstream/maintenance-9.x + +# Make changes, commit, and push +git push origin my-new-feature + +# Create PR targeting maintenance-9.x (not master) +``` + +When updating your fork: + +```bash +# Get the latest maintenance branch changes +git fetch upstream + +# Push directly from upstream to your fork (no local checkout needed) +git push origin upstream/maintenance-9.x:maintenance-9.x +``` diff --git a/docs/development/msp/docs_v2_header.md b/docs/development/msp/docs_v2_header.md new file mode 100644 index 00000000000..88c057f898e --- /dev/null +++ b/docs/development/msp/docs_v2_header.md @@ -0,0 +1,30 @@ + +# INAV MSP Messages reference + +**This page is auto-generated from the [master INAV MSP definitions file](https://github.com/iNavFlight/inav/blob/master/docs/development/msp/msp_messages.json)** + +For details on the structure of MSP, see [The wiki page](https://github.com/iNavFlight/inav/wiki/MSP-V2) + +For list of enums, see [Enum documentation page](https://github.com/iNavFlight/inav/wiki/Enums-reference) + +For current generation code, see [documentation project](https://github.com/xznhj8129/msp_documentation) (temporary until official implementation) + + +**JSON file rev: ** + +**Warning: Verification needed, exercise caution until completely verified for accuracy and cleared, especially for integer signs. Source-based generation/validation is forthcoming. Refer to source for absolute certainty** + +**If you find an error, it must be corrected in the JSON spec, not this markdown.** + +**Guide:** + +* **MSP Versions:** + * **MSPv1:** The original protocol. Uses command IDs from 0 to 254. + * **MSPv2:** An extended version. Uses command IDs from 0x1000 onwards. +* **Request Payload:** The request payload sent to the destination (usually the flight controller). May be empty or hold data for setting or requesting data from the FC. +* **Reply Payload:** The reply sent from the FC to the sender. May be empty or hold data. +* **Notes:** Pay attention to message notes and description. + + + +--- diff --git a/docs/development/msp/format.md b/docs/development/msp/format.md new file mode 100644 index 00000000000..7349b11ad7c --- /dev/null +++ b/docs/development/msp/format.md @@ -0,0 +1,131 @@ +# Format: +## JSON format example: +``` + "MSP_API_VERSION": { + "code": 1, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "mspProtocolVersion", + "ctype": "uint8_t", + "units": "", + "desc": "MSP Protocol version (`MSP_PROTOCOL_VERSION`, typically 0)." + }, + { + "name": "apiVersionMajor", + "ctype": "uint8_t", + "units": "", + "desc": "INAV API Major version (`API_VERSION_MAJOR`)." + }, + { + "name": "apiVersionMinor", + "ctype": "uint8_t", + "units": "", + "desc": "INAV API Minor version (`API_VERSION_MINOR`)." + } + ], + }, + "notes": "Used by configurators to check compatibility.", + "description": "Provides the MSP protocol version and the INAV API version." + }, +``` +## Message fields: +**name**: MSP message name\ +**code**: Integer message code\ +**description**: String with description of message\ +**request**: null or dict of data sent\ +**reply**: null or dict of data received\ +**variable_len**: Optional boolean, if true, message does not have a predefined fixed length and needs appropriate handling\ +**variants**: Optional special case, message has different cases of reply/request. Key/description is not a strict expression or code; just a readable condition\ +**not_implemented**: Optional special case, message is not implemented\ +**notes**: String with details of message + +## Data dict fields: +**payload**: Array of payload fields\ +**repeating**: Optional Special Case, integer or string of how many times the *entire* payload is repeated + +## Payload fields: +### Fields: +**name**: field name from code\ +**ctype**: Base C type of the value. Arrays list their element type here as well\ +**desc**: Optional string with description and details of field\ +**units**: Optional defined units\ +**enum**: Optional string of enum struct if value is an enum +**array**: Optional boolean to denote field is array of more values\ +**array_size**: If array, integer count of elements. Use `0` when the length is indeterminate/variable\ +**array_size_define**: Optional string naming the source `#define` that provides the size (informational only)\ +**repeating**: Optional Special case, contains array of more payload fields that are added Times * Key\ +**payload**: If repeating, contains more payload fields\ +**polymorph**: Optional boolean special case, field does not have a defined C type and could be anything + +**Simple value** +``` +{ + "name": "mspProtocolVersion", + "ctype": "uint8_t", + "units": "", + "desc": "MSP Protocol version (`MSP_PROTOCOL_VERSION`, typically 0)." +}, +``` +**Fixed length array** +``` +{ + "name": "fcVariantIdentifier", + "ctype": "char", + "desc": "4-character identifier string (e.g., \"INAV\"). Defined by `flightControllerIdentifier", + "array": true, + "array_size": 4, + "units": "" +} +``` +**Array sized via define** +``` +{ + "name": "buildDate", + "ctype": "char", + "desc": "Build date string (e.g., \"Dec 31 2023\").", + "array": true, + "array_size": 11, + "array_size_define": "BUILD_DATE_LENGTH", + "units": "" +} +``` +**Undefined length array** +``` +{ + "name": "firmwareChunk", + "ctype": "uint8_t", + "desc": "Chunk of firmware data", + "array": true, + "array_size": 0, +} +``` +**As of yet unknown length array** +``` +{ + "name": "elementText", + "ctype": "char", + "desc": "Static text bytes, not NUL-terminated and not yet sized.", + "array": true, + "array_size": 0 +} +``` +**Nested array with struct** +``` +{ + "repeating": "maxVehicles", + "payload": [ + { + "name": "adsbVehicle", + "ctype": "adsbVehicle_t", + "desc": "Array of `adsbVehicle_t` Repeated `maxVehicles` times", + "repeating": "maxVehicles", + "array": true, + "array_size": 0, + "units": "" + } + ] +} +``` diff --git a/docs/development/msp/gen_docs.sh b/docs/development/msp/gen_docs.sh new file mode 100644 index 00000000000..389d34d114b --- /dev/null +++ b/docs/development/msp/gen_docs.sh @@ -0,0 +1,27 @@ +INAV_MAIN_PATH="../../../src/main" + +echo "###########" +echo get_all_inav_enums_h.py +python get_all_inav_enums_h.py --inav-root "$INAV_MAIN_PATH" + +echo "###########" +echo "msp_messages.json checksum" +actual="$(md5sum msp_messages.json | awk '{print $1}')" +expected="$(awk '{print $1}' msp_messages.checksum)" +echo "Hash:" $actual +if [[ "$actual" != "$expected" ]]; then + n="$(cat rev)" + echo $((n + 1)) > rev + echo "File changed, incrementing revision" + echo $actual > msp_messages.checksum +fi + +echo "###########" +echo gen_msp_md.py +python gen_msp_md.py + +echo "###########" +echo gen_enum_md.py +python gen_enum_md.py +rm all_enums.h +read -n 1 -s -r -p "Press any key to continue" diff --git a/docs/development/msp/gen_enum_md.py b/docs/development/msp/gen_enum_md.py new file mode 100644 index 00000000000..b15c38e67cd --- /dev/null +++ b/docs/development/msp/gen_enum_md.py @@ -0,0 +1,294 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +enumdoc.py — Generate Markdown documentation from C enums (no expression eval). + +Rules: +- One Value column only. + * If explicit assignment is a plain int literal (dec/hex/bin/oct) -> show that number. + * If explicit assignment is anything else -> show the raw expression text. + * If no assignment -> auto-increment. +- If auto-increment occurs inside an active preprocessor condition, wrap the number + in parentheses to indicate conditional numbering: e.g., 3, 4, #ifdef, (5), (6). +- Tracks nested #if/#ifdef/#ifndef/#elif/#else/#endif and shows Condition text. +- Handles multiline enumerators (split at the first top-level comma). +""" + +import sys +import re +from pathlib import Path +from typing import List, Optional +import json + +# ---------- Helpers ---------- + +BLOCK_COMMENT_RE = re.compile(r'/\*.*?\*/', re.DOTALL) + +def strip_comments(s: str) -> str: + s = BLOCK_COMMENT_RE.sub('', s) + s = re.sub(r'//.*', '', s) + return s + +def find_top_level_comma(s: str) -> int: + depth = 0 + for i, ch in enumerate(s): + if ch == '(': + depth += 1 + elif ch == ')': + depth = max(0, depth - 1) + elif ch == ',' and depth == 0: + return i + return -1 + +def is_plain_int_literal(expr: str) -> Optional[int]: + """ + Return int value if expr is a plain integer literal (dec/hex/bin/oct), + otherwise None. Whitespace ok; no unary ops/casts/suffixes. + """ + t = expr.strip() + if not t: + return None + if re.fullmatch(r'0[xX][0-9A-Fa-f]+', t) or \ + re.fullmatch(r'0[bB][01]+', t) or \ + re.fullmatch(r'0[0-7]*', t) or \ + re.fullmatch(r'[1-9][0-9]*', t) or \ + t == '0': + try: + return int(t, 0) + except Exception: + return None + return None + +# ---------- Parsing regexes ---------- + +RE_ENUM_START = re.compile(r'^\s*typedef\s+enum\s*\{') +RE_ENUM_END = re.compile(r'^\s*\}\s*([A-Za-z_]\w*)\s*;') +RE_LINE_COMMENT = re.compile(r'^\s*//\s*(.+?)\s*$') + +RE_IFDEF = re.compile(r'^\s*#\s*ifdef\s+(\w+)') +RE_IFNDEF = re.compile(r'^\s*#\s*ifndef\s+(\w+)') +RE_IF = re.compile(r'^\s*#\s*if\s+(.+)$') +RE_ELIF = re.compile(r'^\s*#\s*elif\s+(.+)$') +RE_ELSE = re.compile(r'^\s*#\s*else\s*$') +RE_ENDIF = re.compile(r'^\s*#\s*endif\b') + +def normalize_condition_text(text: str) -> str: + t = text.strip() + t = re.sub(r'\bdefined\s*\(\s*(\w+)\s*\)', r'\1', t) + t = re.sub(r'\s+', ' ', t) + return t + +class ConditionStack: + def __init__(self): + self.stack: List[str] = [] + def push_ifdef(self, sym: str): self.stack.append(sym) + def push_ifndef(self, sym: str): self.stack.append(f'!{sym}') + def push_if(self, expr: str): self.stack.append(normalize_condition_text(expr)) + def elif_(self, expr: str): + if self.stack: self.stack.pop() + self.stack.append(normalize_condition_text(expr)) + def else_(self): + if not self.stack: return + top = self.stack.pop() + if top.startswith('!'): self.stack.append(top[1:]) + elif top and all(ch.isalnum() or ch == '_' for ch in top): self.stack.append(f'!{top}') + else: self.stack.append(f'NOT({top})') + def endif(self): + if self.stack: self.stack.pop() + def current(self) -> str: + return " AND ".join(self.stack) if self.stack else "" + def has_active(self) -> bool: + return bool(self.stack) + +# ---------- Model ---------- + +class EnumItem: + def __init__(self, name: str, value_display: str, cond: str): + self.name = name + self.value_display = value_display # number, (number), or raw expr string + self.cond = cond + +class EnumDef: + def __init__(self, name: str, source_note: str): + self.name = name + self.source_note = source_note + self.items: List[EnumItem] = [] + +# ---------- Core parsing ---------- + +def parse_files(paths: List[Path]) -> List[EnumDef]: + enums: List[EnumDef] = [] + outer_cond = ConditionStack() + + for path in paths: + lines = path.read_text(encoding="utf-8", errors="ignore").splitlines() + i = 0 + recent_comment: Optional[str] = None + + while i < len(lines): + line = lines[i] + + # Track outer preproc + if m := RE_IFDEF.match(line): outer_cond.push_ifdef(m.group(1)); i += 1; continue + if m := RE_IFNDEF.match(line): outer_cond.push_ifndef(m.group(1)); i += 1; continue + if m := RE_IF.match(line): outer_cond.push_if(m.group(1)); i += 1; continue + if m := RE_ELIF.match(line): outer_cond.elif_(m.group(1)); i += 1; continue + if RE_ELSE.match(line): outer_cond.else_(); i += 1; continue + if RE_ENDIF.match(line): outer_cond.endif(); i += 1; continue + + # Source comment directly above typedef + mcom = RE_LINE_COMMENT.match(line) + if mcom: + recent_comment = mcom.group(1) + + if RE_ENUM_START.match(line): + source_note = recent_comment or str(path) + recent_comment = None + + body_lines: List[str] = [] + i += 1 + local_i = i + while local_i < len(lines): + ln = lines[local_i] + if RE_ENUM_END.match(ln): + enum_name = RE_ENUM_END.match(ln).group(1) + enum = EnumDef(enum_name, source_note) + + # second pass: parse enumerators + inner = ConditionStack() + current_numeric: Optional[int] = -1 # known numeric head; None means unknown + + idx = 0 + while idx < len(body_lines): + bl = body_lines[idx] + + # inner preproc + if m := RE_IFDEF.match(bl): inner.push_ifdef(m.group(1)); idx += 1; continue + if m := RE_IFNDEF.match(bl): inner.push_ifndef(m.group(1)); idx += 1; continue + if m := RE_IF.match(bl): inner.push_if(m.group(1)); idx += 1; continue + if m := RE_ELIF.match(bl): inner.elif_(m.group(1)); idx += 1; continue + if RE_ELSE.match(bl): inner.else_(); idx += 1; continue + if RE_ENDIF.match(bl): inner.endif(); idx += 1; continue + + # accumulate one item across lines + buf = [bl] + while True: + combined = strip_comments(" ".join(buf)).strip() + if not combined: + break + comma_pos = find_top_level_comma(combined) + if comma_pos != -1: + item_text = combined[:comma_pos].strip() + break + if idx + 1 >= len(body_lines): + item_text = combined + break + nxt = body_lines[idx + 1] + if RE_ENUM_END.match(nxt): + item_text = combined + break + idx += 1 + buf.append(body_lines[idx]) + + if not combined: + idx += 1 + continue + + # NAME or NAME = expr + mitem = re.match(r'^\s*([A-Za-z_]\w*)\s*(?:=\s*(.*))?$', item_text) + if not mitem: + idx += 1 + continue + + name = mitem.group(1) + expr = (mitem.group(2) or "").strip() + + # active condition text + cond_parts = [p for p in (outer_cond.current(), inner.current()) if p] + cond_text = " AND ".join(cond_parts) + + # determine display value + if expr: + lit = is_plain_int_literal(expr) + if lit is not None: + # explicit numeric literal + value_display = str(lit) + current_numeric = lit + else: + # show raw expression; numeric chain becomes unknown + value_display = expr + current_numeric = None + else: + # auto-increment if we know a numeric head; else unknown + if current_numeric is None: + value_display = "" + else: + current_numeric += 1 + if inner.has_active(): + value_display = f"({current_numeric})" + else: + value_display = str(current_numeric) + + enum.items.append(EnumItem(name=name, value_display=value_display, cond=cond_text)) + idx += 1 + + enums.append(enum) + i = local_i + 1 + break + else: + body_lines.append(lines[local_i]) + local_i += 1 + else: + i = local_i + continue + else: + i += 1 + + return enums + +# ---------- Markdown rendering ---------- + +def render_markdown(enums: List[EnumDef]) -> str: + jsonfile = {} + out = [] + out.append("# Enumerations\n") + out.append("**Auto-generated reference for MSP, refer to source for development, not this file, due to variations with #ifdefs which needs verification.**\n") + out.append("## Table of contents\n") + for e in sorted(enums, key=lambda x: x.name.lower()): + out.append(f"- [{e.name}](#enum-{e.name.lower()})") + out.append("") + for e in sorted(enums, key=lambda x: x.name.lower()): + jsonfile[e.name] = {} + out.append("---") + out.append(f"## `{e.name}`\n") + if e.source_note: + out.append(f"> Source: {e.source_note}\n") + jsonfile[e.name]['_source'] = e.source_note + out.append("| Enumerator | Value | Condition |") + out.append("|---|---:|---|") + for it in e.items: + name_md = f"`{it.name}`" + val = it.value_display + cond = it.cond + out.append(f"| {name_md} | {val} | {cond} |") + jsonfile[e.name][name_md.strip('`')] = [val, cond] if len(cond)>0 else val + out.append("") + # While we're at it, chuck this all into a JSON file + Path("inav_enums.json").write_text(json.dumps(jsonfile,indent=4), encoding="utf-8") + return "\n".join(out) + +# ---------- Main ---------- + +def main() -> int: + path = Path("all_enums.h") + if not path.exists(): + print(f"Error: {path} not found", file=sys.stderr) + return 1 + enums = parse_files([path]) + md = render_markdown(enums) + Path("inav_enums_ref.md").write_text(md, encoding="utf-8") + return 0 + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/docs/development/msp/gen_msp_md.py b/docs/development/msp/gen_msp_md.py new file mode 100644 index 00000000000..7a82a732aa0 --- /dev/null +++ b/docs/development/msp/gen_msp_md.py @@ -0,0 +1,441 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +Generate Markdown documentation from an MSP message definitions JSON. + +Strict + Index: +- STRICT: If a code exists in one (MSPCodes vs JSON) but not the other, crash with details. +- Index items link to headings via GitHub-style auto-anchors. +- Tight layout; identical Request/Reply tables; skip complex=true with a stub. +- Default input: msp_messages.json ; default output: MSP_Doc.md +""" + +import sys +import json +import re +import unicodedata +from pathlib import Path +from typing import Any, Dict, List, Optional, Tuple, Type + +import enum + +def build_msp_codes_enum(defs: Dict[str, Any]) -> Type[enum.IntEnum]: + members: Dict[str, int] = {} + for name, body in defs.items(): + try: + code = int(body.get("code", -1)) + except (TypeError, ValueError): + continue + members[name] = code + return enum.IntEnum("MSPCodes", members) + + +# ---- C type size helpers ---------------------------------------------------- + +BASE_SIZES = { + "uint8_t": 1, "int8_t": 1, "char": 1, + "uint16_t": 2, "int16_t": 2, + "uint32_t": 4, "int32_t": 4, + "uint64_t": 8, "int64_t": 8, + "float": 4, "double": 8, +} + +array_brackets_re = re.compile(r"^(?P[A-Za-z_0-9]+)\[(?P.*)\]$") + +def parse_ctype(ctype: str) -> Tuple[str, Optional[str]]: + m = array_brackets_re.match(ctype.strip()) + if not m: + return ctype.strip(), None + return m.group("base").strip(), m.group("size").strip() + +def format_ctype(field: Dict[str, Any]) -> str: + raw = (field.get("ctype") or "").strip() + if not raw: + return "-" + + base, bracket = parse_ctype(raw) + has_array_meta = bool(field.get("array", False)) + is_array = has_array_meta or (bracket is not None) + if not is_array: + return raw + + size_define = (field.get("array_size_define") or "").strip() + array_size = field.get("array_size") + size_expr = "" + + if size_define: + size_expr = size_define + else: + if isinstance(array_size, int): + if array_size > 0: + size_expr = str(array_size) + elif isinstance(array_size, str): + cleaned = array_size.strip() + if cleaned and cleaned != "0": + size_expr = cleaned + + if not size_expr and bracket is not None: + size_expr = bracket.strip() + + if size_expr == "0": + size_expr = "" + + base_part = base or raw + return f"{base_part}[{size_expr}]" + +def describe_array_bytes(array_size_meta: Any, base_bytes: Optional[int], base_name: str) -> str: + """ + Returns a printable byte-count (or symbolic string) for an array entry. + """ + if isinstance(array_size_meta, int): + if array_size_meta <= 0: + return "array" + if base_bytes is None: + return str(array_size_meta) + return str(array_size_meta * base_bytes) + + if isinstance(array_size_meta, str): + expr = array_size_meta.strip() + if not expr: + return "array" + if base_bytes is None or base_name == "char": + return expr + return f"{expr} * {base_bytes}" + + return "array" + +def sizeof_entry(field: Dict[str, Any]) -> str: + ctype = field.get("ctype", "").strip() + base, bracket = parse_ctype(ctype) + + is_array = bool(field.get("array", False)) + array_size_meta = field.get("array_size", None) + array_size_define = (field.get("array_size_define") or "").strip() + + if is_array or bracket is not None: + base_for_size = base if (base and is_array) else (base or ctype) + base_bytes = BASE_SIZES.get(base_for_size, None) + + if is_array: + size_str = describe_array_bytes(array_size_meta, base_bytes, base_for_size) + if array_size_define: + if size_str in {"array", "-"}: + size_str = array_size_define + else: + size_str = f"{size_str} ({array_size_define})" + return size_str + + if bracket is not None: + if bracket == "": + return "array" + if bracket.isdigit(): + n = int(bracket) + return str(n * base_bytes) if base_bytes is not None else str(n) + if base_bytes is None or base == "char": + return bracket + return f"{bracket} * {base_bytes}" + + return "array" + + base_bytes = BASE_SIZES.get(base, None) + return str(base_bytes) if base_bytes is not None else "-" + + +# ---- Markdown rendering ----------------------------------------------------- + +#inav_wiki_url = "https://github.com/xznhj8129/msp_documentation/blob/master/docs/inav_enums_ref.md" +inav_wiki_url = "https://github.com/iNavFlight/inav/wiki/Enums-reference" + +def units_cell(field: Dict[str, Any]) -> str: + if "enum" in field: + if field["enum"]=="?_e": + return "[ENUM_NAME](LINK_TO_ENUM)" + else: + return f"[{field['enum']}]({inav_wiki_url}#enum-{field['enum'].lower()})" + u = (field.get("units") or "").strip() + return u if u else "-" + +def has_fields(section: Any) -> bool: + if not isinstance(section, dict): + return False + payload = section.get("payload") + return isinstance(payload, list) and len(payload) > 0 + +def get_fields(section: Any) -> List[Dict[str, Any]]: + if not isinstance(section, dict): + return [] + payload = section.get("payload") + return payload if isinstance(payload, list) else [] + +def flatten_fields_with_repeats(fields: List[Dict[str, Any]]) -> List[Dict[str, Any]]: + """ + Flattens one level of partially repeating payload blocks: + Items with {"repeating": "SOME_SYMBOL", "payload": [...]} are expanded so each child + field gets a symbolic multiplier in the size column. + """ + out: List[Dict[str, Any]] = [] + for f in fields: + if isinstance(f, dict) and "repeating" in f and isinstance(f.get("payload"), list): + repeat_sym = str(f["repeating"]) + for child in f["payload"]: + if isinstance(child, dict): + c = dict(child) + # Mark repeat multiplier for the size column + c["_repeat_multiplier"] = repeat_sym + out.append(c) + else: + out.append(f) + return out + + +def table_with_units(fields: List[Dict[str, Any]], label: str) -> str: + flat_fields = flatten_fields_with_repeats(fields) + has_repeats = any(isinstance(f, dict) and f.get("_repeat_multiplier") for f in flat_fields) + has_units = any( + isinstance(f, dict) and ( + ((f.get("units") or "").strip()) or ("enum" in f) + ) + for f in flat_fields + ) + + # Build dynamic header + cols = ["Field", "C Type"] + if has_repeats: + cols.append("Repeats") + cols.append("Size (Bytes)") + if has_units: + cols.append("Units") + cols.append("Description") + + header = " \n**{label}:**\n".format(label=label) + header += "|" + "|".join(cols) + "|\n" + header += "|" + "|".join(["---"] * len(cols)) + "|\n" + + # Rows + rows: List[str] = [] + for f in flat_fields: + name = f.get("name", "") + size = sizeof_entry(f) + if size == "0": + size = "-" + + row_cells = [f"`{name}`", f"`{format_ctype(f)}`"] + + if has_repeats: + repeats = f.get("_repeat_multiplier") or "-" + row_cells.append(repeats) + + row_cells.append(size) + + if has_units: + units = units_cell(f) + row_cells.append(units) + + desc = (f.get("desc") or "").strip() + row_cells.append(desc) + + rows.append("| " + " | ".join(row_cells) + " |") + + return header + "\n".join(rows) + "\n" + + + +def render_variant(parent_name: str, variant_name: str, variant_def: Dict[str, Any]) -> str: + """ + Renders a single variant block (subsection header, description, request/reply tables). + """ + out: List[str] = [] + vdesc = (variant_def.get("description") or "").strip() + + # GitHub auto-anchors will work off this header text + out.append(f"#### Variant: `{variant_name}`\n\n") + if vdesc: + out.append(f"**Description:** {vdesc} \n") + + req = variant_def.get("request", None) + rep = variant_def.get("reply", None) + + if has_fields(req): + out.append(table_with_units(get_fields(req), "Request Payload")) + else: + out.append("\n**Request Payload:** **None** \n") + + if has_fields(rep): + out.append(table_with_units(get_fields(rep), "Reply Payload")) + else: + out.append("\n**Reply Payload:** **None** \n") + + out.append("\n") + return "".join(out) + +def render_message(name: str, msg: Dict[str, Any]) -> Tuple[str, str]: + """ + Returns (section_markdown, heading_text_for_anchor) + """ + code = msg.get("code", 0) + hex_str = msg.get("hex", hex(code)) + description = (msg.get("description") or "").strip() + notes = (msg.get("notes") or "").strip() + complex_flag = bool(msg.get("complex", False)) + + heading = f'## `{name} ({code} / {hex_str})`' + out = [heading + "\n"] + + if description: + out.append(f"**Description:** {description} \n") + + #if complex_flag: + # out.append("**Special case, skipped for now**\n\n") + # return "".join(out), heading + + # NEW: variant-aware rendering + variants = msg.get("variants") + if isinstance(variants, dict) and variants: + # For variant messages, render a compact per-variant table set + for vname, vdef in variants.items(): + out.append(render_variant(name, vname, vdef)) + else: + # Fallback: single request/reply like before + req = msg.get("request", None) + rep = msg.get("reply", None) + + if has_fields(req): + out.append(table_with_units(get_fields(req), "Request Payload")) + else: + out.append("\n**Request Payload:** **None** \n") + + if has_fields(rep): + out.append(table_with_units(get_fields(rep), "Reply Payload")) + else: + out.append("\n**Reply Payload:** **None** \n") + + if notes: + out.append(f"\n**Notes:** {notes}\n") + + out.append("\n") + return "".join(out), heading + +# ---- Index + strict consistency -------------------------------------------- + +def build_maps(defs: Dict[str, Any], codes_cls: Type[enum.IntEnum]) -> Tuple[Dict[int, str], Dict[int, str]]: + """ + Returns: + json_by_code: {code -> message_name_from_json} + mw_by_code: {code -> enum_name_from codes_cls} + Only for codes in the enforced ranges (v1 and v2). + """ + v1_range = range(0, 255) + v2_range = range(4096, 20001) + + # JSON: build by code (restrict to ranges) + json_by_code: Dict[int, str] = {} + for name, body in defs.items(): + code = int(body.get("code", -1)) + if code in v1_range or code in v2_range: + json_by_code[code] = name + + # MSPCodes: probe the same ranges + mw_by_code: Dict[int, str] = {} + def try_get(code: int) -> Optional[str]: + try: + e = codes_cls(code) + return e.name + except Exception: + return None + + for code in list(v1_range) + list(v2_range): + ename = try_get(code) + if ename is not None: + mw_by_code[code] = ename + + return json_by_code, mw_by_code + +def enforce_strict_match(json_by_code: Dict[int, str], mw_by_code: Dict[int, str]) -> None: + json_codes = set(json_by_code.keys()) + mw_codes = set(mw_by_code.keys()) + + only_in_json = sorted(json_codes - mw_codes) + only_in_mw = sorted(mw_codes - json_codes) + + if only_in_json or only_in_mw: + lines = ["MSP code mismatch detected:"] + if only_in_json: + lines.append(" Present in JSON but missing in MSPCodes:") + for c in only_in_json: + lines.append(f" {c}\t{json_by_code[c]}") + if only_in_mw: + lines.append(" Present in MSPCodes but missing in JSON:") + for c in only_in_mw: + lines.append(f" {c}\t{mw_by_code[c]}") + raise SystemExit("\n".join(lines)) + +def build_index(json_by_code: Dict[int, str]) -> str: + """ + Build a compact index linking to each heading. + """ + v1 = [] + v2 = [] + for code, name in sorted(json_by_code.items()): + hex_str = hex(code) + item = f"[{code} - {name}](#{name.lower()}) " + if 0 <= code <= 255: + v1.append(item) + elif 4096 <= code <= 20000: + v2.append(item) + + parts = ["## Index", "### MSPv1"] + parts.extend(v1) + parts.append("\n### MSPv2") + parts.extend(v2) + parts.append("") # trailing newline + return "\n".join(parts) + +# ---- Orchestration ---------------------------------------------------------- + +def generate_markdown(defs: Dict[str, Any]) -> str: + # Strict maps & check + codes_enum = build_msp_codes_enum(defs) + json_by_code, mw_by_code = build_maps(defs, codes_enum) + enforce_strict_match(json_by_code, mw_by_code) + + # Build sections, remembering headings for slugging (already handled in index) + items = sorted(((int(body.get("code", 0)), name, body) for name, body in defs.items()), + key=lambda t: t[0]) + + sections = [] + for _, name, body in items: + sec, _heading = render_message(name, body) + sections.append(sec) + + with open("docs_v2_header.md", "r", encoding="utf-8") as f: + header = f.read() + + with open("format.md", "r", encoding="utf-8") as f: + fmt = f.read() + + with open("msp_messages.checksum", "r", encoding="utf-8") as f: + chksum = f.read().split(' ')[0] + with open("rev", "r", encoding="utf-8") as f: + rev = f.read() + + header = header.replace('',fmt) + header = header.replace('',rev) + header = header.replace('',chksum) + + index_md = build_index(json_by_code) + return header + "\n" + index_md + "\n" + "".join(sections) + +def main(): + in_path = Path(sys.argv[1]) if len(sys.argv) >= 2 else Path("msp_messages.json") + out_path = Path(sys.argv[2]) if len(sys.argv) >= 3 else Path("msp_ref.md") + + with in_path.open("r", encoding="utf-8") as f: + defs = json.load(f) + + md = generate_markdown(defs) + out_path.write_text(md, encoding="utf-8") + print(f"Wrote {out_path}") + +if __name__ == "__main__": + main() diff --git a/docs/development/msp/get_all_inav_enums_h.py b/docs/development/msp/get_all_inav_enums_h.py new file mode 100644 index 00000000000..971d077d5f4 --- /dev/null +++ b/docs/development/msp/get_all_inav_enums_h.py @@ -0,0 +1,121 @@ +#!/usr/bin/env python3 +import argparse +import datetime +import re +from pathlib import Path + +SUBDIRS = [ + 'common', + 'blackbox', + 'navigation', + 'sensors', + 'programming', + 'rx', + 'telemetry', + 'io', + 'flight', + 'fc', + 'drivers', +] + +def strip_comments(text: str) -> str: + text = re.sub(r'/\*.*?\*/', '', text, flags=re.DOTALL) # block comments + text = re.sub(r'//.*', '', text) # line comments + return text + +def extract_enums(fn: str, text: str): + src = strip_comments(text) + out = [] + + # typedef enum { ... } Alias; + i = 0 + while True: + m = re.search(r'\btypedef\s+enum\b', src[i:]) + if not m: break + start = i + m.start() + lb = src.find('{', i + m.end()) + if lb == -1: break + depth = 0 + k = lb + while k < len(src): + if src[k] == '{': depth += 1 + elif src[k] == '}': + depth -= 1 + if depth == 0: + semi = src.find(';', k) + if semi == -1: break + tail = src[k+1:semi] + alias = re.findall(r'\b([A-Za-z_]\w*)\b', tail) + if alias: + block = src[start:semi+1].strip() + out += [f'// {fn}\n', block + '\n\n'] + i = semi + 1 + break + k += 1 + else: + break + + # enum Tag { ... }; → also emit typedef enum Tag Tag; + i = 0 + while True: + m = re.search(r'\benum\s+([A-Za-z_]\w*)\s*{', src[i:]) + if not m: break + start = i + m.start() + tag = m.group(1) + lb = src.find('{', i + m.end() - 1) + if lb == -1: break + depth = 0 + k = lb + while k < len(src): + if src[k] == '{': depth += 1 + elif src[k] == '}': + depth -= 1 + if depth == 0: + semi = src.find(';', k) + if semi == -1: break + block = src[start:semi+1].strip() + out += [ + f'// {fn}\n', + block + '\n', + f'typedef enum {tag} {tag};\n\n' + ] + i = semi + 1 + break + k += 1 + else: + break + + return out + + + +all_enums = [] +def parse_args(): + parser = argparse.ArgumentParser(description='Collect all enums from INAV sources.') + parser.add_argument( + '--inav-root', + default='../inav/src/main', + help="Path to the INAV 'src/main' directory (default: %(default)s)", + ) + return parser.parse_args() + + +args = parse_args() +base_dir = Path(args.inav_root).expanduser() +for sd in SUBDIRS: + root = base_dir / sd + if not root.is_dir(): + continue + for fn in root.rglob('*'): + print(fn) + if fn.suffix in ('.c', '.h'): + txt = fn.read_text(errors='ignore') + ret = extract_enums(fn, txt) + if ret: print(fn) + all_enums.extend(ret) + +with open('all_enums.h', 'w') as out: + out.write(f"// Consolidated enums — generated on {datetime.datetime.now()}\n\n") + out.writelines(all_enums) + +print(f"Found {len(all_enums)} enums. Wrote all_enums.h.") diff --git a/docs/development/msp/inav_enums_ref.md b/docs/development/msp/inav_enums_ref.md new file mode 100644 index 00000000000..2d60f270183 --- /dev/null +++ b/docs/development/msp/inav_enums_ref.md @@ -0,0 +1,5768 @@ +# Enumerations + +**Auto-generated reference for MSP, refer to source for development, not this file, due to variations with #ifdefs which needs verification.** + +## Table of contents + +- [accelerationSensor_e](#enum-accelerationsensor_e) +- [accEvent_t](#enum-accevent_t) +- [adcChannel_e](#enum-adcchannel_e) +- [adcFunction_e](#enum-adcfunction_e) +- [adjustmentFunction_e](#enum-adjustmentfunction_e) +- [adjustmentMode_e](#enum-adjustmentmode_e) +- [afatfsAppendFreeClusterPhase_e](#enum-afatfsappendfreeclusterphase_e) +- [afatfsAppendSuperclusterPhase_e](#enum-afatfsappendsuperclusterphase_e) +- [afatfsCacheBlockState_e](#enum-afatfscacheblockstate_e) +- [afatfsClusterSearchCondition_e](#enum-afatfsclustersearchcondition_e) +- [afatfsDeleteFilePhase_e](#enum-afatfsdeletefilephase_e) +- [afatfsError_e](#enum-afatfserror_e) +- [afatfsExtendSubdirectoryPhase_e](#enum-afatfsextendsubdirectoryphase_e) +- [afatfsFATPattern_e](#enum-afatfsfatpattern_e) +- [afatfsFileOperation_e](#enum-afatfsfileoperation_e) +- [afatfsFilesystemState_e](#enum-afatfsfilesystemstate_e) +- [afatfsFileType_e](#enum-afatfsfiletype_e) +- [afatfsFindClusterStatus_e](#enum-afatfsfindclusterstatus_e) +- [afatfsFreeSpaceSearchPhase_e](#enum-afatfsfreespacesearchphase_e) +- [afatfsInitializationPhase_e](#enum-afatfsinitializationphase_e) +- [afatfsOperationStatus_e](#enum-afatfsoperationstatus_e) +- [afatfsSaveDirectoryEntryMode_e](#enum-afatfssavedirectoryentrymode_e) +- [afatfsSeek_e](#enum-afatfsseek_e) +- [afatfsTruncateFilePhase_e](#enum-afatfstruncatefilephase_e) +- [airmodeHandlingType_e](#enum-airmodehandlingtype_e) +- [angle_index_t](#enum-angle_index_t) +- [armingFlag_e](#enum-armingflag_e) +- [axis_e](#enum-axis_e) +- [barometerState_e](#enum-barometerstate_e) +- [baroSensor_e](#enum-barosensor_e) +- [batCapacityUnit_e](#enum-batcapacityunit_e) +- [batteryState_e](#enum-batterystate_e) +- [batVoltageSource_e](#enum-batvoltagesource_e) +- [baudRate_e](#enum-baudrate_e) +- [beeperMode_e](#enum-beepermode_e) +- [biquadFilterType_e](#enum-biquadfiltertype_e) +- [blackboxBufferReserveStatus_e](#enum-blackboxbufferreservestatus_e) +- [blackboxFeatureMask_e](#enum-blackboxfeaturemask_e) +- [bmi270Register_e](#enum-bmi270register_e) +- [bootLogEventCode_e](#enum-bootlogeventcode_e) +- [bootLogFlags_e](#enum-bootlogflags_e) +- [boxId_e](#enum-boxid_e) +- [busIndex_e](#enum-busindex_e) +- [busSpeed_e](#enum-busspeed_e) +- [busType_e](#enum-bustype_e) +- [channelType_t](#enum-channeltype_t) +- [climbRateToAltitudeControllerMode_e](#enum-climbratetoaltitudecontrollermode_e) +- [colorComponent_e](#enum-colorcomponent_e) +- [colorId_e](#enum-colorid_e) +- [crsfActiveAntenna_e](#enum-crsfactiveantenna_e) +- [crsfAddress_e](#enum-crsfaddress_e) +- [crsfFrameType_e](#enum-crsfframetype_e) +- [crsfFrameTypeIndex_e](#enum-crsfframetypeindex_e) +- [crsrRfMode_e](#enum-crsrrfmode_e) +- [crsrRfPower_e](#enum-crsrrfpower_e) +- [currentSensor_e](#enum-currentsensor_e) +- [devHardwareType_e](#enum-devhardwaretype_e) +- [deviceFlags_e](#enum-deviceflags_e) +- [displayCanvasBitmapOption_t](#enum-displaycanvasbitmapoption_t) +- [displayCanvasColor_e](#enum-displaycanvascolor_e) +- [displayCanvasOutlineType_e](#enum-displaycanvasoutlinetype_e) +- [displayportMspCommand_e](#enum-displayportmspcommand_e) +- [displayTransactionOption_e](#enum-displaytransactionoption_e) +- [displayWidgetType_e](#enum-displaywidgettype_e) +- [DjiCraftNameElements_t](#enum-djicraftnameelements_t) +- [dshotCommands_e](#enum-dshotcommands_e) +- [dumpFlags_e](#enum-dumpflags_e) +- [dynamicGyroNotchMode_e](#enum-dynamicgyronotchmode_e) +- [emergLandState_e](#enum-emerglandstate_e) +- [escSensorFrameStatus_t](#enum-escsensorframestatus_t) +- [escSensorState_t](#enum-escsensorstate_t) +- [failsafeChannelBehavior_e](#enum-failsafechannelbehavior_e) +- [failsafePhase_e](#enum-failsafephase_e) +- [failsafeProcedure_e](#enum-failsafeprocedure_e) +- [failsafeRxLinkState_e](#enum-failsaferxlinkstate_e) +- [failureMode_e](#enum-failuremode_e) +- [fatFilesystemType_e](#enum-fatfilesystemtype_e) +- [features_e](#enum-features_e) +- [filterType_e](#enum-filtertype_e) +- [fixedWingLaunchEvent_t](#enum-fixedwinglaunchevent_t) +- [fixedWingLaunchMessage_t](#enum-fixedwinglaunchmessage_t) +- [fixedWingLaunchState_t](#enum-fixedwinglaunchstate_t) +- [flashPartitionType_e](#enum-flashpartitiontype_e) +- [flashType_e](#enum-flashtype_e) +- [flight_dynamics_index_t](#enum-flight_dynamics_index_t) +- [flightModeFlags_e](#enum-flightmodeflags_e) +- [flightModeForTelemetry_e](#enum-flightmodefortelemetry_e) +- [flyingPlatformType_e](#enum-flyingplatformtype_e) +- [fport2_control_frame_type_e](#enum-fport2_control_frame_type_e) +- [frame_state_e](#enum-frame_state_e) +- [frame_type_e](#enum-frame_type_e) +- [frskyOSDColor_e](#enum-frskyosdcolor_e) +- [frskyOSDLineOutlineType_e](#enum-frskyosdlineoutlinetype_e) +- [frskyOSDRecvState_e](#enum-frskyosdrecvstate_e) +- [frskyOSDTransactionOptions_e](#enum-frskyosdtransactionoptions_e) +- [fw_autotune_rate_adjustment_e](#enum-fw_autotune_rate_adjustment_e) +- [fwAutolandApproachDirection_e](#enum-fwautolandapproachdirection_e) +- [fwAutolandState_t](#enum-fwautolandstate_t) +- [fwAutolandWaypoint_t](#enum-fwautolandwaypoint_t) +- [geoAltitudeConversionMode_e](#enum-geoaltitudeconversionmode_e) +- [geoAltitudeDatumFlag_e](#enum-geoaltitudedatumflag_e) +- [geoOriginResetMode_e](#enum-geooriginresetmode_e) +- [geozoneActionState_e](#enum-geozoneactionstate_e) +- [geozoneMessageState_e](#enum-geozonemessagestate_e) +- [ghstAddr_e](#enum-ghstaddr_e) +- [ghstDl_e](#enum-ghstdl_e) +- [ghstFrameTypeIndex_e](#enum-ghstframetypeindex_e) +- [ghstUl_e](#enum-ghstul_e) +- [gimbal_htk_mode_e](#enum-gimbal_htk_mode_e) +- [gimbalDevType_e](#enum-gimbaldevtype_e) +- [gimbalHeadtrackerState_e](#enum-gimbalheadtrackerstate_e) +- [gpsAutoBaud_e](#enum-gpsautobaud_e) +- [gpsAutoConfig_e](#enum-gpsautoconfig_e) +- [gpsBaudRate_e](#enum-gpsbaudrate_e) +- [gpsDynModel_e](#enum-gpsdynmodel_e) +- [gpsFixChar_e](#enum-gpsfixchar_e) +- [gpsFixType_e](#enum-gpsfixtype_e) +- [gpsProvider_e](#enum-gpsprovider_e) +- [gpsState_e](#enum-gpsstate_e) +- [gyroFilterMode_e](#enum-gyrofiltermode_e) +- [gyroHardwareLpf_e](#enum-gyrohardwarelpf_e) +- [gyroSensor_e](#enum-gyrosensor_e) +- [HardwareMotorTypes_e](#enum-hardwaremotortypes_e) +- [hardwareSensorStatus_e](#enum-hardwaresensorstatus_e) +- [headTrackerDevType_e](#enum-headtrackerdevtype_e) +- [hottEamAlarm1Flag_e](#enum-hotteamalarm1flag_e) +- [hottEamAlarm2Flag_e](#enum-hotteamalarm2flag_e) +- [hottState_e](#enum-hottstate_e) +- [hsvColorComponent_e](#enum-hsvcolorcomponent_e) +- [I2CSpeed](#enum-i2cspeed) +- [i2cState_t](#enum-i2cstate_t) +- [i2cTransferDirection_t](#enum-i2ctransferdirection_t) +- [ibusCommand_e](#enum-ibuscommand_e) +- [ibusSensorType1_e](#enum-ibussensortype1_e) +- [ibusSensorType_e](#enum-ibussensortype_e) +- [ibusSensorValue_e](#enum-ibussensorvalue_e) +- [inputSource_e](#enum-inputsource_e) +- [itermRelax_e](#enum-itermrelax_e) +- [led_pin_pwm_mode_e](#enum-led_pin_pwm_mode_e) +- [ledBaseFunctionId_e](#enum-ledbasefunctionid_e) +- [ledDirectionId_e](#enum-leddirectionid_e) +- [ledModeIndex_e](#enum-ledmodeindex_e) +- [ledOverlayId_e](#enum-ledoverlayid_e) +- [ledSpecialColorIds_e](#enum-ledspecialcolorids_e) +- [logicConditionFlags_e](#enum-logicconditionflags_e) +- [logicConditionsGlobalFlags_t](#enum-logicconditionsglobalflags_t) +- [logicFlightModeOperands_e](#enum-logicflightmodeoperands_e) +- [logicFlightOperands_e](#enum-logicflightoperands_e) +- [logicOperation_e](#enum-logicoperation_e) +- [logicWaypointOperands_e](#enum-logicwaypointoperands_e) +- [logTopic_e](#enum-logtopic_e) +- [lsm6dxxConfigMasks_e](#enum-lsm6dxxconfigmasks_e) +- [lsm6dxxConfigValues_e](#enum-lsm6dxxconfigvalues_e) +- [lsm6dxxRegister_e](#enum-lsm6dxxregister_e) +- [ltm_frame_e](#enum-ltm_frame_e) +- [ltm_modes_e](#enum-ltm_modes_e) +- [ltmUpdateRate_e](#enum-ltmupdaterate_e) +- [magSensor_e](#enum-magsensor_e) +- [mavlinkAutopilotType_e](#enum-mavlinkautopilottype_e) +- [mavlinkRadio_e](#enum-mavlinkradio_e) +- [measurementSteps_e](#enum-measurementsteps_e) +- [mixerProfileATRequest_e](#enum-mixerprofileatrequest_e) +- [mixerProfileATState_e](#enum-mixerprofileatstate_e) +- [modeActivationOperator_e](#enum-modeactivationoperator_e) +- [motorPwmProtocolTypes_e](#enum-motorpwmprotocoltypes_e) +- [motorStatus_e](#enum-motorstatus_e) +- [mpu9250CompassReadState_e](#enum-mpu9250compassreadstate_e) +- [mspFlashfsFlags_e](#enum-mspflashfsflags_e) +- [mspPassthroughType_e](#enum-msppassthroughtype_e) +- [mspSDCardFlags_e](#enum-mspsdcardflags_e) +- [mspSDCardState_e](#enum-mspsdcardstate_e) +- [multi_function_e](#enum-multi_function_e) +- [multiFunctionFlags_e](#enum-multifunctionflags_e) +- [nav_reset_type_e](#enum-nav_reset_type_e) +- [navAGLEstimateQuality_e](#enum-navaglestimatequality_e) +- [navArmingBlocker_e](#enum-navarmingblocker_e) +- [navDefaultAltitudeSensor_e](#enum-navdefaultaltitudesensor_e) +- [navExtraArmingSafety_e](#enum-navextraarmingsafety_e) +- [navFwLaunchStatus_e](#enum-navfwlaunchstatus_e) +- [navigationEstimateStatus_e](#enum-navigationestimatestatus_e) +- [navigationFSMEvent_t](#enum-navigationfsmevent_t) +- [navigationFSMState_t](#enum-navigationfsmstate_t) +- [navigationFSMStateFlags_t](#enum-navigationfsmstateflags_t) +- [navigationHomeFlags_t](#enum-navigationhomeflags_t) +- [navigationPersistentId_e](#enum-navigationpersistentid_e) +- [navMcAltHoldThrottle_e](#enum-navmcaltholdthrottle_e) +- [navMissionRestart_e](#enum-navmissionrestart_e) +- [navOverridesMotorStop_e](#enum-navoverridesmotorstop_e) +- [navPositionEstimationFlags_e](#enum-navpositionestimationflags_e) +- [navRTHAllowLanding_e](#enum-navrthallowlanding_e) +- [navRTHClimbFirst_e](#enum-navrthclimbfirst_e) +- [navSetWaypointFlags_t](#enum-navsetwaypointflags_t) +- [navSystemStatus_Error_e](#enum-navsystemstatus_error_e) +- [navSystemStatus_Flags_e](#enum-navsystemstatus_flags_e) +- [navSystemStatus_Mode_e](#enum-navsystemstatus_mode_e) +- [navSystemStatus_State_e](#enum-navsystemstatus_state_e) +- [navWaypointActions_e](#enum-navwaypointactions_e) +- [navWaypointFlags_e](#enum-navwaypointflags_e) +- [navWaypointHeadings_e](#enum-navwaypointheadings_e) +- [navWaypointP3Flags_e](#enum-navwaypointp3flags_e) +- [opflowQuality_e](#enum-opflowquality_e) +- [opticalFlowSensor_e](#enum-opticalflowsensor_e) +- [osd_adsb_warning_style_e](#enum-osd_adsb_warning_style_e) +- [osd_ahi_style_e](#enum-osd_ahi_style_e) +- [osd_alignment_e](#enum-osd_alignment_e) +- [osd_crosshairs_style_e](#enum-osd_crosshairs_style_e) +- [osd_crsf_lq_format_e](#enum-osd_crsf_lq_format_e) +- [osd_items_e](#enum-osd_items_e) +- [osd_sidebar_arrow_e](#enum-osd_sidebar_arrow_e) +- [osd_sidebar_scroll_e](#enum-osd_sidebar_scroll_e) +- [osd_SpeedTypes_e](#enum-osd_speedtypes_e) +- [osd_stats_energy_unit_e](#enum-osd_stats_energy_unit_e) +- [osd_unit_e](#enum-osd_unit_e) +- [osdCustomElementType_e](#enum-osdcustomelementtype_e) +- [osdCustomElementTypeVisibility_e](#enum-osdcustomelementtypevisibility_e) +- [osdDrawPointType_e](#enum-osddrawpointtype_e) +- [osdDriver_e](#enum-osddriver_e) +- [osdSpeedSource_e](#enum-osdspeedsource_e) +- [outputMode_e](#enum-outputmode_e) +- [pageId_e](#enum-pageid_e) +- [persistentObjectId_e](#enum-persistentobjectid_e) +- [pidAutotuneState_e](#enum-pidautotunestate_e) +- [pidControllerFlags_e](#enum-pidcontrollerflags_e) +- [pidIndex_e](#enum-pidindex_e) +- [pidType_e](#enum-pidtype_e) +- [pinLabel_e](#enum-pinlabel_e) +- [pitotSensor_e](#enum-pitotsensor_e) +- [pollType_e](#enum-polltype_e) +- [portSharing_e](#enum-portsharing_e) +- [pwmInitError_e](#enum-pwminiterror_e) +- [quadrant_e](#enum-quadrant_e) +- [QUADSPIClockDivider_e](#enum-quadspiclockdivider_e) +- [quadSpiMode_e](#enum-quadspimode_e) +- [rangefinderType_e](#enum-rangefindertype_e) +- [RCDEVICE_5key_connection_event_e](#enum-rcdevice_5key_connection_event_e) +- [rcdevice_5key_simulation_operation_e](#enum-rcdevice_5key_simulation_operation_e) +- [rcdevice_camera_control_opeation_e](#enum-rcdevice_camera_control_opeation_e) +- [rcdevice_features_e](#enum-rcdevice_features_e) +- [rcdevice_protocol_version_e](#enum-rcdevice_protocol_version_e) +- [rcdeviceCamSimulationKeyEvent_e](#enum-rcdevicecamsimulationkeyevent_e) +- [rcdeviceResponseStatus_e](#enum-rcdeviceresponsestatus_e) +- [resolutionType_e](#enum-resolutiontype_e) +- [resourceOwner_e](#enum-resourceowner_e) +- [resourceType_e](#enum-resourcetype_e) +- [reversibleMotorsThrottleState_e](#enum-reversiblemotorsthrottlestate_e) +- [rollPitchStatus_e](#enum-rollpitchstatus_e) +- [rssiSource_e](#enum-rssisource_e) +- [rthState_e](#enum-rthstate_e) +- [rthTargetMode_e](#enum-rthtargetmode_e) +- [rthTrackbackMode_e](#enum-rthtrackbackmode_e) +- [rxFrameState_e](#enum-rxframestate_e) +- [rxReceiverType_e](#enum-rxreceivertype_e) +- [rxSerialReceiverType_e](#enum-rxserialreceivertype_e) +- [safehomeUsageMode_e](#enum-safehomeusagemode_e) +- [sbasMode_e](#enum-sbasmode_e) +- [sbusDecoderState_e](#enum-sbusdecoderstate_e) +- [sdcardBlockOperation_e](#enum-sdcardblockoperation_e) +- [sdcardOperationStatus_e](#enum-sdcardoperationstatus_e) +- [sdcardReceiveBlockStatus_e](#enum-sdcardreceiveblockstatus_e) +- [sdcardReceiveBlockStatus_e](#enum-sdcardreceiveblockstatus_e) +- [sdcardState_e](#enum-sdcardstate_e) +- [SDIODevice](#enum-sdiodevice) +- [sensor_align_e](#enum-sensor_align_e) +- [sensorIndex_e](#enum-sensorindex_e) +- [sensors_e](#enum-sensors_e) +- [sensorTempCalState_e](#enum-sensortempcalstate_e) +- [serialPortFunction_e](#enum-serialportfunction_e) +- [serialPortIdentifier_e](#enum-serialportidentifier_e) +- [servoAutotrimState_e](#enum-servoautotrimstate_e) +- [servoIndex_e](#enum-servoindex_e) +- [servoProtocolType_e](#enum-servoprotocoltype_e) +- [setting_mode_e](#enum-setting_mode_e) +- [setting_section_e](#enum-setting_section_e) +- [setting_type_e](#enum-setting_type_e) +- [simATCommandState_e](#enum-simatcommandstate_e) +- [simModuleState_e](#enum-simmodulestate_e) +- [simReadState_e](#enum-simreadstate_e) +- [simTelemetryState_e](#enum-simtelemetrystate_e) +- [simTransmissionState_e](#enum-simtransmissionstate_e) +- [simTxFlags_e](#enum-simtxflags_e) +- [simulatorFlags_t](#enum-simulatorflags_t) +- [smartAudioVersion_e](#enum-smartaudioversion_e) +- [smartportFuelUnit_e](#enum-smartportfuelunit_e) +- [softSerialPortIndex_e](#enum-softserialportindex_e) +- [SPIClockSpeed_e](#enum-spiclockspeed_e) +- [Srxl2BindRequest](#enum-srxl2bindrequest) +- [Srxl2BindType](#enum-srxl2bindtype) +- [Srxl2ControlDataCommand](#enum-srxl2controldatacommand) +- [Srxl2DeviceId](#enum-srxl2deviceid) +- [Srxl2DeviceType](#enum-srxl2devicetype) +- [Srxl2PacketType](#enum-srxl2packettype) +- [Srxl2State](#enum-srxl2state) +- [stateFlags_t](#enum-stateflags_t) +- [stickPositions_e](#enum-stickpositions_e) +- [systemState_e](#enum-systemstate_e) +- [systemState_e](#enum-systemstate_e) +- [tchDmaState_e](#enum-tchdmastate_e) +- [tempSensorType_e](#enum-tempsensortype_e) +- [throttleStatus_e](#enum-throttlestatus_e) +- [throttleStatusType_e](#enum-throttlestatustype_e) +- [timerMode_e](#enum-timermode_e) +- [timerUsageFlag_e](#enum-timerusageflag_e) +- [timId_e](#enum-timid_e) +- [tristate_e](#enum-tristate_e) +- [tz_automatic_dst_e](#enum-tz_automatic_dst_e) +- [UARTDevice_e](#enum-uartdevice_e) +- [uartInverterLine_e](#enum-uartinverterline_e) +- [ublox_nav_sig_health_e](#enum-ublox_nav_sig_health_e) +- [ublox_nav_sig_quality](#enum-ublox_nav_sig_quality) +- [ubs_nav_fix_type_t](#enum-ubs_nav_fix_type_t) +- [ubx_ack_state_t](#enum-ubx_ack_state_t) +- [ubx_nav_status_bits_t](#enum-ubx_nav_status_bits_t) +- [ubx_protocol_bytes_t](#enum-ubx_protocol_bytes_t) +- [vcselPeriodType_e](#enum-vcselperiodtype_e) +- [videoSystem_e](#enum-videosystem_e) +- [voltageSensor_e](#enum-voltagesensor_e) +- [vs600Band_e](#enum-vs600band_e) +- [vs600Power_e](#enum-vs600power_e) +- [vtxDevType_e](#enum-vtxdevtype_e) +- [vtxFrequencyGroups_e](#enum-vtxfrequencygroups_e) +- [vtxLowerPowerDisarm_e](#enum-vtxlowerpowerdisarm_e) +- [vtxProtoResponseType_e](#enum-vtxprotoresponsetype_e) +- [vtxProtoState_e](#enum-vtxprotostate_e) +- [vtxScheduleParams_e](#enum-vtxscheduleparams_e) +- [warningFlags_e](#enum-warningflags_e) +- [warningLedState_e](#enum-warningledstate_e) +- [widgetAHIOptions_t](#enum-widgetahioptions_t) +- [widgetAHIStyle_e](#enum-widgetahistyle_e) +- [wpFwTurnSmoothing_e](#enum-wpfwturnsmoothing_e) +- [wpMissionPlannerStatus_e](#enum-wpmissionplannerstatus_e) +- [zeroCalibrationState_e](#enum-zerocalibrationstate_e) + +--- +## `accelerationSensor_e` + +> Source: ../../../src/main/sensors/acceleration.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `ACC_NONE` | 0 | | +| `ACC_AUTODETECT` | 1 | | +| `ACC_MPU6000` | 2 | | +| `ACC_MPU6500` | 3 | | +| `ACC_MPU9250` | 4 | | +| `ACC_BMI160` | 5 | | +| `ACC_ICM20689` | 6 | | +| `ACC_BMI088` | 7 | | +| `ACC_ICM42605` | 8 | | +| `ACC_BMI270` | 9 | | +| `ACC_LSM6DXX` | 10 | | +| `ACC_FAKE` | 11 | | +| `ACC_MAX` | ACC_FAKE | | + +--- +## `accEvent_t` + +> Source: ../../../src/main/telemetry/sim.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `ACC_EVENT_NONE` | 0 | | +| `ACC_EVENT_HIGH` | 1 | | +| `ACC_EVENT_LOW` | 2 | | +| `ACC_EVENT_NEG_X` | 3 | | + +--- +## `adcChannel_e` + +> Source: ../../../src/main/drivers/adc.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `ADC_CHN_NONE` | 0 | | +| `ADC_CHN_1` | 1 | | +| `ADC_CHN_2` | 2 | | +| `ADC_CHN_3` | 3 | | +| `ADC_CHN_4` | 4 | | +| `ADC_CHN_5` | 5 | | +| `ADC_CHN_6` | 6 | | +| `ADC_CHN_MAX` | ADC_CHN_6 | | +| `ADC_CHN_COUNT` | | | + +--- +## `adcFunction_e` + +> Source: ../../../src/main/drivers/adc.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `ADC_BATTERY` | 0 | | +| `ADC_RSSI` | 1 | | +| `ADC_CURRENT` | 2 | | +| `ADC_AIRSPEED` | 3 | | +| `ADC_FUNCTION_COUNT` | 4 | | + +--- +## `adjustmentFunction_e` + +> Source: ../../../src/main/fc/rc_adjustments.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `ADJUSTMENT_NONE` | 0 | | +| `ADJUSTMENT_RC_RATE` | 1 | | +| `ADJUSTMENT_RC_EXPO` | 2 | | +| `ADJUSTMENT_THROTTLE_EXPO` | 3 | | +| `ADJUSTMENT_PITCH_ROLL_RATE` | 4 | | +| `ADJUSTMENT_YAW_RATE` | 5 | | +| `ADJUSTMENT_PITCH_ROLL_P` | 6 | | +| `ADJUSTMENT_PITCH_ROLL_I` | 7 | | +| `ADJUSTMENT_PITCH_ROLL_D` | 8 | | +| `ADJUSTMENT_PITCH_ROLL_FF` | 9 | | +| `ADJUSTMENT_PITCH_P` | 10 | | +| `ADJUSTMENT_PITCH_I` | 11 | | +| `ADJUSTMENT_PITCH_D` | 12 | | +| `ADJUSTMENT_PITCH_FF` | 13 | | +| `ADJUSTMENT_ROLL_P` | 14 | | +| `ADJUSTMENT_ROLL_I` | 15 | | +| `ADJUSTMENT_ROLL_D` | 16 | | +| `ADJUSTMENT_ROLL_FF` | 17 | | +| `ADJUSTMENT_YAW_P` | 18 | | +| `ADJUSTMENT_YAW_I` | 19 | | +| `ADJUSTMENT_YAW_D` | 20 | | +| `ADJUSTMENT_YAW_FF` | 21 | | +| `ADJUSTMENT_RATE_PROFILE` | 22 | | +| `ADJUSTMENT_PITCH_RATE` | 23 | | +| `ADJUSTMENT_ROLL_RATE` | 24 | | +| `ADJUSTMENT_RC_YAW_EXPO` | 25 | | +| `ADJUSTMENT_MANUAL_RC_EXPO` | 26 | | +| `ADJUSTMENT_MANUAL_RC_YAW_EXPO` | 27 | | +| `ADJUSTMENT_MANUAL_PITCH_ROLL_RATE` | 28 | | +| `ADJUSTMENT_MANUAL_ROLL_RATE` | 29 | | +| `ADJUSTMENT_MANUAL_PITCH_RATE` | 30 | | +| `ADJUSTMENT_MANUAL_YAW_RATE` | 31 | | +| `ADJUSTMENT_NAV_FW_CRUISE_THR` | 32 | | +| `ADJUSTMENT_NAV_FW_PITCH2THR` | 33 | | +| `ADJUSTMENT_ROLL_BOARD_ALIGNMENT` | 34 | | +| `ADJUSTMENT_PITCH_BOARD_ALIGNMENT` | 35 | | +| `ADJUSTMENT_LEVEL_P` | 36 | | +| `ADJUSTMENT_LEVEL_I` | 37 | | +| `ADJUSTMENT_LEVEL_D` | 38 | | +| `ADJUSTMENT_POS_XY_P` | 39 | | +| `ADJUSTMENT_POS_XY_I` | 40 | | +| `ADJUSTMENT_POS_XY_D` | 41 | | +| `ADJUSTMENT_POS_Z_P` | 42 | | +| `ADJUSTMENT_POS_Z_I` | 43 | | +| `ADJUSTMENT_POS_Z_D` | 44 | | +| `ADJUSTMENT_HEADING_P` | 45 | | +| `ADJUSTMENT_VEL_XY_P` | 46 | | +| `ADJUSTMENT_VEL_XY_I` | 47 | | +| `ADJUSTMENT_VEL_XY_D` | 48 | | +| `ADJUSTMENT_VEL_Z_P` | 49 | | +| `ADJUSTMENT_VEL_Z_I` | 50 | | +| `ADJUSTMENT_VEL_Z_D` | 51 | | +| `ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE` | 52 | | +| `ADJUSTMENT_VTX_POWER_LEVEL` | 53 | | +| `ADJUSTMENT_TPA` | 54 | | +| `ADJUSTMENT_TPA_BREAKPOINT` | 55 | | +| `ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS` | 56 | | +| `ADJUSTMENT_FW_TPA_TIME_CONSTANT` | 57 | | +| `ADJUSTMENT_FW_LEVEL_TRIM` | 58 | | +| `ADJUSTMENT_NAV_WP_MULTI_MISSION_INDEX` | 59 | | +| `ADJUSTMENT_NAV_FW_ALT_CONTROL_RESPONSE` | 60 | | +| `ADJUSTMENT_FUNCTION_COUNT` | 61 | | + +--- +## `adjustmentMode_e` + +> Source: ../../../src/main/fc/rc_adjustments.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `ADJUSTMENT_MODE_STEP` | 0 | | +| `ADJUSTMENT_MODE_SELECT` | 1 | | + +--- +## `afatfsAppendFreeClusterPhase_e` + +> Source: ../../../src/main/io/asyncfatfs/asyncfatfs.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `AFATFS_APPEND_FREE_CLUSTER_PHASE_INITIAL` | 0 | | +| `AFATFS_APPEND_FREE_CLUSTER_PHASE_FIND_FREESPACE` | 0 | | +| `AFATFS_APPEND_FREE_CLUSTER_PHASE_UPDATE_FAT1` | 1 | | +| `AFATFS_APPEND_FREE_CLUSTER_PHASE_UPDATE_FAT2` | 2 | | +| `AFATFS_APPEND_FREE_CLUSTER_PHASE_UPDATE_FILE_DIRECTORY` | 3 | | +| `AFATFS_APPEND_FREE_CLUSTER_PHASE_COMPLETE` | 4 | | +| `AFATFS_APPEND_FREE_CLUSTER_PHASE_FAILURE` | 5 | | + +--- +## `afatfsAppendSuperclusterPhase_e` + +> Source: ../../../src/main/io/asyncfatfs/asyncfatfs.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `AFATFS_APPEND_SUPERCLUSTER_PHASE_INIT` | 0 | | +| `AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FREEFILE_DIRECTORY` | 1 | | +| `AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FAT` | 2 | | +| `AFATFS_APPEND_SUPERCLUSTER_PHASE_UPDATE_FILE_DIRECTORY` | 3 | | + +--- +## `afatfsCacheBlockState_e` + +> Source: ../../../src/main/io/asyncfatfs/asyncfatfs.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `AFATFS_CACHE_STATE_EMPTY` | 0 | | +| `AFATFS_CACHE_STATE_IN_SYNC` | 1 | | +| `AFATFS_CACHE_STATE_READING` | 2 | | +| `AFATFS_CACHE_STATE_WRITING` | 3 | | +| `AFATFS_CACHE_STATE_DIRTY` | 4 | | + +--- +## `afatfsClusterSearchCondition_e` + +> Source: ../../../src/main/io/asyncfatfs/asyncfatfs.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `CLUSTER_SEARCH_FREE_AT_BEGINNING_OF_FAT_SECTOR` | 0 | | +| `CLUSTER_SEARCH_FREE` | 1 | | +| `CLUSTER_SEARCH_OCCUPIED` | 2 | | + +--- +## `afatfsDeleteFilePhase_e` + +> Source: ../../../src/main/io/asyncfatfs/asyncfatfs.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `AFATFS_DELETE_FILE_DELETE_DIRECTORY_ENTRY` | 0 | | +| `AFATFS_DELETE_FILE_DEALLOCATE_CLUSTERS` | 1 | | + +--- +## `afatfsError_e` + +> Source: ../../../src/main/io/asyncfatfs/asyncfatfs.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `AFATFS_ERROR_NONE` | 0 | | +| `AFATFS_ERROR_GENERIC` | 1 | | +| `AFATFS_ERROR_BAD_MBR` | 2 | | +| `AFATFS_ERROR_BAD_FILESYSTEM_HEADER` | 3 | | + +--- +## `afatfsExtendSubdirectoryPhase_e` + +> Source: ../../../src/main/io/asyncfatfs/asyncfatfs.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `AFATFS_EXTEND_SUBDIRECTORY_PHASE_INITIAL` | 0 | | +| `AFATFS_EXTEND_SUBDIRECTORY_PHASE_ADD_FREE_CLUSTER` | 0 | | +| `AFATFS_EXTEND_SUBDIRECTORY_PHASE_WRITE_SECTORS` | 1 | | +| `AFATFS_EXTEND_SUBDIRECTORY_PHASE_SUCCESS` | 2 | | +| `AFATFS_EXTEND_SUBDIRECTORY_PHASE_FAILURE` | 3 | | + +--- +## `afatfsFATPattern_e` + +> Source: ../../../src/main/io/asyncfatfs/asyncfatfs.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `AFATFS_FAT_PATTERN_UNTERMINATED_CHAIN` | 0 | | +| `AFATFS_FAT_PATTERN_TERMINATED_CHAIN` | 1 | | +| `AFATFS_FAT_PATTERN_FREE` | 2 | | + +--- +## `afatfsFileOperation_e` + +> Source: ../../../src/main/io/asyncfatfs/asyncfatfs.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `AFATFS_FILE_OPERATION_NONE` | 0 | | +| `AFATFS_FILE_OPERATION_CREATE_FILE` | 1 | | +| `AFATFS_FILE_OPERATION_SEEK` | 2 | | +| `AFATFS_FILE_OPERATION_CLOSE` | 3 | | +| `AFATFS_FILE_OPERATION_TRUNCATE` | 4 | | +| `AFATFS_FILE_OPERATION_UNLINK` | 5 | | +| `AFATFS_FILE_OPERATION_APPEND_SUPERCLUSTER` | (6) | AFATFS_USE_FREEFILE | +| `AFATFS_FILE_OPERATION_LOCKED` | (7) | AFATFS_USE_FREEFILE | +| `AFATFS_FILE_OPERATION_APPEND_FREE_CLUSTER` | 8 | | +| `AFATFS_FILE_OPERATION_EXTEND_SUBDIRECTORY` | 9 | | + +--- +## `afatfsFilesystemState_e` + +> Source: ../../../src/main/io/asyncfatfs/asyncfatfs.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `AFATFS_FILESYSTEM_STATE_UNKNOWN` | 0 | | +| `AFATFS_FILESYSTEM_STATE_FATAL` | 1 | | +| `AFATFS_FILESYSTEM_STATE_INITIALIZATION` | 2 | | +| `AFATFS_FILESYSTEM_STATE_READY` | 3 | | + +--- +## `afatfsFileType_e` + +> Source: ../../../src/main/io/asyncfatfs/asyncfatfs.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `AFATFS_FILE_TYPE_NONE` | 0 | | +| `AFATFS_FILE_TYPE_NORMAL` | 1 | | +| `AFATFS_FILE_TYPE_FAT16_ROOT_DIRECTORY` | 2 | | +| `AFATFS_FILE_TYPE_DIRECTORY` | 3 | | + +--- +## `afatfsFindClusterStatus_e` + +> Source: ../../../src/main/io/asyncfatfs/asyncfatfs.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `AFATFS_FIND_CLUSTER_IN_PROGRESS` | 0 | | +| `AFATFS_FIND_CLUSTER_FOUND` | 1 | | +| `AFATFS_FIND_CLUSTER_FATAL` | 2 | | +| `AFATFS_FIND_CLUSTER_NOT_FOUND` | 3 | | + +--- +## `afatfsFreeSpaceSearchPhase_e` + +> Source: ../../../src/main/io/asyncfatfs/asyncfatfs.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `AFATFS_FREE_SPACE_SEARCH_PHASE_FIND_HOLE` | 0 | | +| `AFATFS_FREE_SPACE_SEARCH_PHASE_GROW_HOLE` | 1 | | + +--- +## `afatfsInitializationPhase_e` + +> Source: ../../../src/main/io/asyncfatfs/asyncfatfs.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `AFATFS_INITIALIZATION_READ_MBR` | 0 | | +| `AFATFS_INITIALIZATION_READ_VOLUME_ID` | 1 | | +| `AFATFS_INITIALIZATION_FREEFILE_CREATE` | (2) | AFATFS_USE_FREEFILE | +| `AFATFS_INITIALIZATION_FREEFILE_CREATING` | (3) | AFATFS_USE_FREEFILE | +| `AFATFS_INITIALIZATION_FREEFILE_FAT_SEARCH` | (4) | AFATFS_USE_FREEFILE | +| `AFATFS_INITIALIZATION_FREEFILE_UPDATE_FAT` | (5) | AFATFS_USE_FREEFILE | +| `AFATFS_INITIALIZATION_FREEFILE_SAVE_DIR_ENTRY` | (6) | AFATFS_USE_FREEFILE | +| `AFATFS_INITIALIZATION_FREEFILE_LAST` | AFATFS_INITIALIZATION_FREEFILE_SAVE_DIR_ENTRY | AFATFS_USE_FREEFILE | +| `AFATFS_INITIALIZATION_DONE` | | | + +--- +## `afatfsOperationStatus_e` + +> Source: ../../../src/main/io/asyncfatfs/asyncfatfs.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `AFATFS_OPERATION_IN_PROGRESS` | 0 | | +| `AFATFS_OPERATION_SUCCESS` | 1 | | +| `AFATFS_OPERATION_FAILURE` | 2 | | + +--- +## `afatfsSaveDirectoryEntryMode_e` + +> Source: ../../../src/main/io/asyncfatfs/asyncfatfs.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `AFATFS_SAVE_DIRECTORY_NORMAL` | 0 | | +| `AFATFS_SAVE_DIRECTORY_FOR_CLOSE` | 1 | | +| `AFATFS_SAVE_DIRECTORY_DELETED` | 2 | | + +--- +## `afatfsSeek_e` + +> Source: ../../../src/main/io/asyncfatfs/asyncfatfs.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `AFATFS_SEEK_SET` | 0 | | +| `AFATFS_SEEK_CUR` | 1 | | +| `AFATFS_SEEK_END` | 2 | | + +--- +## `afatfsTruncateFilePhase_e` + +> Source: ../../../src/main/io/asyncfatfs/asyncfatfs.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `AFATFS_TRUNCATE_FILE_INITIAL` | 0 | | +| `AFATFS_TRUNCATE_FILE_UPDATE_DIRECTORY` | 0 | | +| `AFATFS_TRUNCATE_FILE_ERASE_FAT_CHAIN_NORMAL` | 1 | | +| `AFATFS_TRUNCATE_FILE_ERASE_FAT_CHAIN_CONTIGUOUS` | (2) | AFATFS_USE_FREEFILE | +| `AFATFS_TRUNCATE_FILE_PREPEND_TO_FREEFILE` | (3) | AFATFS_USE_FREEFILE | +| `AFATFS_TRUNCATE_FILE_SUCCESS` | 4 | | + +--- +## `airmodeHandlingType_e` + +> Source: ../../../src/main/fc/rc_controls.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `STICK_CENTER` | 0 | | +| `THROTTLE_THRESHOLD` | 1 | | +| `STICK_CENTER_ONCE` | 2 | | + +--- +## `angle_index_t` + +> Source: ../../../src/main/common/axis.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `AI_ROLL` | 0 | | +| `AI_PITCH` | 1 | | + +--- +## `armingFlag_e` + +> Source: ../../../src/main/fc/runtime_config.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `ARMED` | (1 << 2) | | +| `WAS_EVER_ARMED` | (1 << 3) | | +| `SIMULATOR_MODE_HITL` | (1 << 4) | | +| `SIMULATOR_MODE_SITL` | (1 << 5) | | +| `ARMING_DISABLED_GEOZONE` | (1 << 6) | | +| `ARMING_DISABLED_FAILSAFE_SYSTEM` | (1 << 7) | | +| `ARMING_DISABLED_NOT_LEVEL` | (1 << 8) | | +| `ARMING_DISABLED_SENSORS_CALIBRATING` | (1 << 9) | | +| `ARMING_DISABLED_SYSTEM_OVERLOADED` | (1 << 10) | | +| `ARMING_DISABLED_NAVIGATION_UNSAFE` | (1 << 11) | | +| `ARMING_DISABLED_COMPASS_NOT_CALIBRATED` | (1 << 12) | | +| `ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED` | (1 << 13) | | +| `ARMING_DISABLED_ARM_SWITCH` | (1 << 14) | | +| `ARMING_DISABLED_HARDWARE_FAILURE` | (1 << 15) | | +| `ARMING_DISABLED_BOXFAILSAFE` | (1 << 16) | | +| `ARMING_DISABLED_RC_LINK` | (1 << 18) | | +| `ARMING_DISABLED_THROTTLE` | (1 << 19) | | +| `ARMING_DISABLED_CLI` | (1 << 20) | | +| `ARMING_DISABLED_CMS_MENU` | (1 << 21) | | +| `ARMING_DISABLED_OSD_MENU` | (1 << 22) | | +| `ARMING_DISABLED_ROLLPITCH_NOT_CENTERED` | (1 << 23) | | +| `ARMING_DISABLED_SERVO_AUTOTRIM` | (1 << 24) | | +| `ARMING_DISABLED_OOM` | (1 << 25) | | +| `ARMING_DISABLED_INVALID_SETTING` | (1 << 26) | | +| `ARMING_DISABLED_PWM_OUTPUT_ERROR` | (1 << 27) | | +| `ARMING_DISABLED_NO_PREARM` | (1 << 28) | | +| `ARMING_DISABLED_DSHOT_BEEPER` | (1 << 29) | | +| `ARMING_DISABLED_LANDING_DETECTED` | (1 << 30) | | +| `ARMING_DISABLED_ALL_FLAGS` | (ARMING_DISABLED_GEOZONE | ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE | ARMING_DISABLED_COMPASS_NOT_CALIBRATED | ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED | ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_HARDWARE_FAILURE | ARMING_DISABLED_BOXFAILSAFE | ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI | ARMING_DISABLED_CMS_MENU | ARMING_DISABLED_OSD_MENU | ARMING_DISABLED_ROLLPITCH_NOT_CENTERED | ARMING_DISABLED_SERVO_AUTOTRIM | ARMING_DISABLED_OOM | ARMING_DISABLED_INVALID_SETTING | ARMING_DISABLED_PWM_OUTPUT_ERROR | ARMING_DISABLED_NO_PREARM | ARMING_DISABLED_DSHOT_BEEPER | ARMING_DISABLED_LANDING_DETECTED) | | + +--- +## `axis_e` + +> Source: ../../../src/main/common/axis.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `X` | 0 | | +| `Y` | 1 | | +| `Z` | 2 | | + +--- +## `barometerState_e` + +> Source: ../../../src/main/sensors/barometer.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `BAROMETER_NEEDS_SAMPLES` | 0 | | +| `BAROMETER_NEEDS_CALCULATION` | 1 | | + +--- +## `baroSensor_e` + +> Source: ../../../src/main/sensors/barometer.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `BARO_NONE` | 0 | | +| `BARO_AUTODETECT` | 1 | | +| `BARO_BMP085` | 2 | | +| `BARO_MS5611` | 3 | | +| `BARO_BMP280` | 4 | | +| `BARO_MS5607` | 5 | | +| `BARO_LPS25H` | 6 | | +| `BARO_SPL06` | 7 | | +| `BARO_BMP388` | 8 | | +| `BARO_DPS310` | 9 | | +| `BARO_B2SMPB` | 10 | | +| `BARO_MSP` | 11 | | +| `BARO_FAKE` | 12 | | +| `BARO_MAX` | BARO_FAKE | | + +--- +## `batCapacityUnit_e` + +> Source: ../../../src/main/sensors/battery_config_structs.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `BAT_CAPACITY_UNIT_MAH` | 0 | | +| `BAT_CAPACITY_UNIT_MWH` | 1 | | + +--- +## `batteryState_e` + +> Source: ../../../src/main/sensors/battery.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `BATTERY_OK` | 0 | | +| `BATTERY_WARNING` | 1 | | +| `BATTERY_CRITICAL` | 2 | | +| `BATTERY_NOT_PRESENT` | 3 | | + +--- +## `batVoltageSource_e` + +> Source: ../../../src/main/sensors/battery_config_structs.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `BAT_VOLTAGE_RAW` | 0 | | +| `BAT_VOLTAGE_SAG_COMP` | 1 | | + +--- +## `baudRate_e` + +> Source: ../../../src/main/io/serial.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `BAUD_AUTO` | 0 | | +| `BAUD_1200` | 1 | | +| `BAUD_2400` | 2 | | +| `BAUD_4800` | 3 | | +| `BAUD_9600` | 4 | | +| `BAUD_19200` | 5 | | +| `BAUD_38400` | 6 | | +| `BAUD_57600` | 7 | | +| `BAUD_115200` | 8 | | +| `BAUD_230400` | 9 | | +| `BAUD_250000` | 10 | | +| `BAUD_460800` | 11 | | +| `BAUD_921600` | 12 | | +| `BAUD_1000000` | 13 | | +| `BAUD_1500000` | 14 | | +| `BAUD_2000000` | 15 | | +| `BAUD_2470000` | 16 | | +| `BAUD_MIN` | BAUD_AUTO | | +| `BAUD_MAX` | BAUD_2470000 | | + +--- +## `beeperMode_e` + +> Source: ../../../src/main/io/beeper.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `BEEPER_SILENCE` | 0 | | +| `BEEPER_RUNTIME_CALIBRATION_DONE` | 1 | | +| `BEEPER_HARDWARE_FAILURE` | 2 | | +| `BEEPER_RX_LOST` | 3 | | +| `BEEPER_RX_LOST_LANDING` | 4 | | +| `BEEPER_DISARMING` | 5 | | +| `BEEPER_ARMING` | 6 | | +| `BEEPER_ARMING_GPS_FIX` | 7 | | +| `BEEPER_BAT_CRIT_LOW` | 8 | | +| `BEEPER_BAT_LOW` | 9 | | +| `BEEPER_GPS_STATUS` | 10 | | +| `BEEPER_RX_SET` | 11 | | +| `BEEPER_ACTION_SUCCESS` | 12 | | +| `BEEPER_ACTION_FAIL` | 13 | | +| `BEEPER_READY_BEEP` | 14 | | +| `BEEPER_MULTI_BEEPS` | 15 | | +| `BEEPER_DISARM_REPEAT` | 16 | | +| `BEEPER_ARMED` | 17 | | +| `BEEPER_SYSTEM_INIT` | 18 | | +| `BEEPER_USB` | 19 | | +| `BEEPER_LAUNCH_MODE_ENABLED` | 20 | | +| `BEEPER_LAUNCH_MODE_LOW_THROTTLE` | 21 | | +| `BEEPER_LAUNCH_MODE_IDLE_START` | 22 | | +| `BEEPER_CAM_CONNECTION_OPEN` | 23 | | +| `BEEPER_CAM_CONNECTION_CLOSE` | 24 | | +| `BEEPER_ALL` | 25 | | +| `BEEPER_PREFERENCE` | 26 | | + +--- +## `biquadFilterType_e` + +> Source: ../../../src/main/common/filter.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FILTER_LPF` | 0 | | +| `FILTER_NOTCH` | 1 | | + +--- +## `blackboxBufferReserveStatus_e` + +> Source: ../../../src/main/blackbox/blackbox_io.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `BLACKBOX_RESERVE_SUCCESS` | 0 | | +| `BLACKBOX_RESERVE_TEMPORARY_FAILURE` | 1 | | +| `BLACKBOX_RESERVE_PERMANENT_FAILURE` | 2 | | + +--- +## `blackboxFeatureMask_e` + +> Source: ../../../src/main/blackbox/blackbox.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `BLACKBOX_FEATURE_NAV_ACC` | 1 << 0 | | +| `BLACKBOX_FEATURE_NAV_POS` | 1 << 1 | | +| `BLACKBOX_FEATURE_NAV_PID` | 1 << 2 | | +| `BLACKBOX_FEATURE_MAG` | 1 << 3 | | +| `BLACKBOX_FEATURE_ACC` | 1 << 4 | | +| `BLACKBOX_FEATURE_ATTITUDE` | 1 << 5 | | +| `BLACKBOX_FEATURE_RC_DATA` | 1 << 6 | | +| `BLACKBOX_FEATURE_RC_COMMAND` | 1 << 7 | | +| `BLACKBOX_FEATURE_MOTORS` | 1 << 8 | | +| `BLACKBOX_FEATURE_GYRO_RAW` | 1 << 9 | | +| `BLACKBOX_FEATURE_GYRO_PEAKS_ROLL` | 1 << 10 | | +| `BLACKBOX_FEATURE_GYRO_PEAKS_PITCH` | 1 << 11 | | +| `BLACKBOX_FEATURE_GYRO_PEAKS_YAW` | 1 << 12 | | +| `BLACKBOX_FEATURE_SERVOS` | 1 << 13 | | + +--- +## `bmi270Register_e` + +> Source: ../../../src/main/drivers/accgyro/accgyro_bmi270.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `BMI270_REG_CHIP_ID` | 0 | | +| `BMI270_REG_ERR_REG` | 2 | | +| `BMI270_REG_STATUS` | 3 | | +| `BMI270_REG_ACC_DATA_X_LSB` | 12 | | +| `BMI270_REG_GYR_DATA_X_LSB` | 18 | | +| `BMI270_REG_SENSORTIME_0` | 24 | | +| `BMI270_REG_SENSORTIME_1` | 25 | | +| `BMI270_REG_SENSORTIME_2` | 26 | | +| `BMI270_REG_EVENT` | 27 | | +| `BMI270_REG_INT_STATUS_0` | 28 | | +| `BMI270_REG_INT_STATUS_1` | 29 | | +| `BMI270_REG_INTERNAL_STATUS` | 33 | | +| `BMI270_REG_TEMPERATURE_LSB` | 34 | | +| `BMI270_REG_TEMPERATURE_MSB` | 35 | | +| `BMI270_REG_FIFO_LENGTH_LSB` | 36 | | +| `BMI270_REG_FIFO_LENGTH_MSB` | 37 | | +| `BMI270_REG_FIFO_DATA` | 38 | | +| `BMI270_REG_ACC_CONF` | 64 | | +| `BMI270_REG_ACC_RANGE` | 65 | | +| `BMI270_REG_GYRO_CONF` | 66 | | +| `BMI270_REG_GYRO_RANGE` | 67 | | +| `BMI270_REG_AUX_CONF` | 68 | | +| `BMI270_REG_FIFO_DOWNS` | 69 | | +| `BMI270_REG_FIFO_WTM_0` | 70 | | +| `BMI270_REG_FIFO_WTM_1` | 71 | | +| `BMI270_REG_FIFO_CONFIG_0` | 72 | | +| `BMI270_REG_FIFO_CONFIG_1` | 73 | | +| `BMI270_REG_SATURATION` | 74 | | +| `BMI270_REG_INT1_IO_CTRL` | 83 | | +| `BMI270_REG_INT2_IO_CTRL` | 84 | | +| `BMI270_REG_INT_LATCH` | 85 | | +| `BMI270_REG_INT1_MAP_FEAT` | 86 | | +| `BMI270_REG_INT2_MAP_FEAT` | 87 | | +| `BMI270_REG_INT_MAP_DATA` | 88 | | +| `BMI270_REG_INIT_CTRL` | 89 | | +| `BMI270_REG_INIT_DATA` | 94 | | +| `BMI270_REG_ACC_SELF_TEST` | 109 | | +| `BMI270_REG_GYR_SELF_TEST_AXES` | 110 | | +| `BMI270_REG_PWR_CONF` | 124 | | +| `BMI270_REG_PWR_CTRL` | 125 | | +| `BMI270_REG_CMD` | 126 | | + +--- +## `bootLogEventCode_e` + +> Source: ../../../src/main/drivers/logging_codes.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `BOOT_EVENT_CONFIG_LOADED` | 0 | | +| `BOOT_EVENT_SYSTEM_INIT_DONE` | 1 | | +| `BOOT_EVENT_PWM_INIT_DONE` | 2 | | +| `BOOT_EVENT_EXTRA_BOOT_DELAY` | 3 | | +| `BOOT_EVENT_SENSOR_INIT_DONE` | 4 | | +| `BOOT_EVENT_GPS_INIT_DONE` | 5 | | +| `BOOT_EVENT_LEDSTRIP_INIT_DONE` | 6 | | +| `BOOT_EVENT_TELEMETRY_INIT_DONE` | 7 | | +| `BOOT_EVENT_SYSTEM_READY` | 8 | | +| `BOOT_EVENT_GYRO_DETECTION` | 9 | | +| `BOOT_EVENT_ACC_DETECTION` | 10 | | +| `BOOT_EVENT_BARO_DETECTION` | 11 | | +| `BOOT_EVENT_MAG_DETECTION` | 12 | | +| `BOOT_EVENT_RANGEFINDER_DETECTION` | 13 | | +| `BOOT_EVENT_MAG_INIT_FAILED` | 14 | | +| `BOOT_EVENT_HMC5883L_READ_OK_COUNT` | 15 | | +| `BOOT_EVENT_HMC5883L_READ_FAILED` | 16 | | +| `BOOT_EVENT_HMC5883L_SATURATION` | 17 | | +| `BOOT_EVENT_TIMER_CH_SKIPPED` | 18 | | +| `BOOT_EVENT_TIMER_CH_MAPPED` | 19 | | +| `BOOT_EVENT_PITOT_DETECTION` | 20 | | +| `BOOT_EVENT_TEMP_SENSOR_DETECTION` | 21 | | +| `BOOT_EVENT_1WIRE_DETECTION` | 22 | | +| `BOOT_EVENT_HARDWARE_IO_CONFLICT` | 23 | | +| `BOOT_EVENT_OPFLOW_DETECTION` | 24 | | +| `BOOT_EVENT_CODE_COUNT` | 25 | | + +--- +## `bootLogFlags_e` + +> Source: ../../../src/main/drivers/logging_codes.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `BOOT_EVENT_FLAGS_NONE` | 0 | | +| `BOOT_EVENT_FLAGS_WARNING` | 1 << 0 | | +| `BOOT_EVENT_FLAGS_ERROR` | 1 << 1 | | +| `BOOT_EVENT_FLAGS_PARAM16` | 1 << 14 | | +| `BOOT_EVENT_FLAGS_PARAM32` | 1 << 15 | | + +--- +## `boxId_e` + +> Source: ../../../src/main/fc/rc_modes.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `BOXARM` | 0 | | +| `BOXANGLE` | 1 | | +| `BOXHORIZON` | 2 | | +| `BOXNAVALTHOLD` | 3 | | +| `BOXHEADINGHOLD` | 4 | | +| `BOXHEADFREE` | 5 | | +| `BOXHEADADJ` | 6 | | +| `BOXCAMSTAB` | 7 | | +| `BOXNAVRTH` | 8 | | +| `BOXNAVPOSHOLD` | 9 | | +| `BOXMANUAL` | 10 | | +| `BOXBEEPERON` | 11 | | +| `BOXLEDLOW` | 12 | | +| `BOXLIGHTS` | 13 | | +| `BOXNAVLAUNCH` | 14 | | +| `BOXOSD` | 15 | | +| `BOXTELEMETRY` | 16 | | +| `BOXBLACKBOX` | 17 | | +| `BOXFAILSAFE` | 18 | | +| `BOXNAVWP` | 19 | | +| `BOXAIRMODE` | 20 | | +| `BOXHOMERESET` | 21 | | +| `BOXGCSNAV` | 22 | | +| `BOXSURFACE` | 24 | | +| `BOXFLAPERON` | 25 | | +| `BOXTURNASSIST` | 26 | | +| `BOXAUTOTRIM` | 27 | | +| `BOXAUTOTUNE` | 28 | | +| `BOXCAMERA1` | 29 | | +| `BOXCAMERA2` | 30 | | +| `BOXCAMERA3` | 31 | | +| `BOXOSDALT1` | 32 | | +| `BOXOSDALT2` | 33 | | +| `BOXOSDALT3` | 34 | | +| `BOXNAVCOURSEHOLD` | 35 | | +| `BOXBRAKING` | 36 | | +| `BOXUSER1` | 37 | | +| `BOXUSER2` | 38 | | +| `BOXFPVANGLEMIX` | 39 | | +| `BOXLOITERDIRCHN` | 40 | | +| `BOXMSPRCOVERRIDE` | 41 | | +| `BOXPREARM` | 42 | | +| `BOXTURTLE` | 43 | | +| `BOXNAVCRUISE` | 44 | | +| `BOXAUTOLEVEL` | 45 | | +| `BOXPLANWPMISSION` | 46 | | +| `BOXSOARING` | 47 | | +| `BOXUSER3` | 48 | | +| `BOXUSER4` | 49 | | +| `BOXCHANGEMISSION` | 50 | | +| `BOXBEEPERMUTE` | 51 | | +| `BOXMULTIFUNCTION` | 52 | | +| `BOXMIXERPROFILE` | 53 | | +| `BOXMIXERTRANSITION` | 54 | | +| `BOXANGLEHOLD` | 55 | | +| `BOXGIMBALTLOCK` | 56 | | +| `BOXGIMBALRLOCK` | 57 | | +| `BOXGIMBALCENTER` | 58 | | +| `BOXGIMBALHTRK` | 59 | | +| `CHECKBOX_ITEM_COUNT` | 60 | | + +--- +## `busIndex_e` + +> Source: ../../../src/main/drivers/bus.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `BUSINDEX_1` | 0 | | +| `BUSINDEX_2` | 1 | | +| `BUSINDEX_3` | 2 | | +| `BUSINDEX_4` | 3 | | + +--- +## `busSpeed_e` + +> Source: ../../../src/main/drivers/bus.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `BUS_SPEED_INITIALIZATION` | 0 | | +| `BUS_SPEED_SLOW` | 1 | | +| `BUS_SPEED_STANDARD` | 2 | | +| `BUS_SPEED_FAST` | 3 | | +| `BUS_SPEED_ULTRAFAST` | 4 | | + +--- +## `busType_e` + +> Source: ../../../src/main/drivers/bus.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `BUSTYPE_ANY` | 0 | | +| `BUSTYPE_NONE` | 0 | | +| `BUSTYPE_I2C` | 1 | | +| `BUSTYPE_SPI` | 2 | | +| `BUSTYPE_SDIO` | 3 | | + +--- +## `channelType_t` + +> Source: ../../../src/main/drivers/timer.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `TYPE_FREE` | 0 | | +| `TYPE_PWMINPUT` | 1 | | +| `TYPE_PPMINPUT` | 2 | | +| `TYPE_PWMOUTPUT_MOTOR` | 3 | | +| `TYPE_PWMOUTPUT_FAST` | 4 | | +| `TYPE_PWMOUTPUT_SERVO` | 5 | | +| `TYPE_SOFTSERIAL_RX` | 6 | | +| `TYPE_SOFTSERIAL_TX` | 7 | | +| `TYPE_SOFTSERIAL_RXTX` | 8 | | +| `TYPE_SOFTSERIAL_AUXTIMER` | 9 | | +| `TYPE_ADC` | 10 | | +| `TYPE_SERIAL_RX` | 11 | | +| `TYPE_SERIAL_TX` | 12 | | +| `TYPE_SERIAL_RXTX` | 13 | | +| `TYPE_TIMER` | 14 | | + +--- +## `climbRateToAltitudeControllerMode_e` + +> Source: ../../../src/main/navigation/navigation_private.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `ROC_TO_ALT_CURRENT` | 0 | | +| `ROC_TO_ALT_CONSTANT` | 1 | | +| `ROC_TO_ALT_TARGET` | 2 | | + +--- +## `colorComponent_e` + +> Source: ../../../src/main/common/color.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `RGB_RED` | 0 | | +| `RGB_GREEN` | 1 | | +| `RGB_BLUE` | 2 | | + +--- +## `colorId_e` + +> Source: ../../../src/main/io/ledstrip.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `COLOR_BLACK` | 0 | | +| `COLOR_WHITE` | 1 | | +| `COLOR_RED` | 2 | | +| `COLOR_ORANGE` | 3 | | +| `COLOR_YELLOW` | 4 | | +| `COLOR_LIME_GREEN` | 5 | | +| `COLOR_GREEN` | 6 | | +| `COLOR_MINT_GREEN` | 7 | | +| `COLOR_CYAN` | 8 | | +| `COLOR_LIGHT_BLUE` | 9 | | +| `COLOR_BLUE` | 10 | | +| `COLOR_DARK_VIOLET` | 11 | | +| `COLOR_MAGENTA` | 12 | | +| `COLOR_DEEP_PINK` | 13 | | + +--- +## `crsfActiveAntenna_e` + +> Source: ../../../src/main/telemetry/crsf.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `CRSF_ACTIVE_ANTENNA1` | 0 | | +| `CRSF_ACTIVE_ANTENNA2` | 1 | | + +--- +## `crsfAddress_e` + +> Source: ../../../src/main/rx/crsf.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `CRSF_ADDRESS_BROADCAST` | 0 | | +| `CRSF_ADDRESS_USB` | 16 | | +| `CRSF_ADDRESS_TBS_CORE_PNP_PRO` | 128 | | +| `CRSF_ADDRESS_RESERVED1` | 138 | | +| `CRSF_ADDRESS_CURRENT_SENSOR` | 192 | | +| `CRSF_ADDRESS_GPS` | 194 | | +| `CRSF_ADDRESS_TBS_BLACKBOX` | 196 | | +| `CRSF_ADDRESS_FLIGHT_CONTROLLER` | 200 | | +| `CRSF_ADDRESS_RESERVED2` | 202 | | +| `CRSF_ADDRESS_RACE_TAG` | 204 | | +| `CRSF_ADDRESS_RADIO_TRANSMITTER` | 234 | | +| `CRSF_ADDRESS_CRSF_RECEIVER` | 236 | | +| `CRSF_ADDRESS_CRSF_TRANSMITTER` | 238 | | + +--- +## `crsfFrameType_e` + +> Source: ../../../src/main/rx/crsf.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `CRSF_FRAMETYPE_GPS` | 2 | | +| `CRSF_FRAMETYPE_VARIO_SENSOR` | 7 | | +| `CRSF_FRAMETYPE_BATTERY_SENSOR` | 8 | | +| `CRSF_FRAMETYPE_BAROMETER_ALTITUDE` | 9 | | +| `CRSF_FRAMETYPE_LINK_STATISTICS` | 20 | | +| `CRSF_FRAMETYPE_RC_CHANNELS_PACKED` | 22 | | +| `CRSF_FRAMETYPE_ATTITUDE` | 30 | | +| `CRSF_FRAMETYPE_FLIGHT_MODE` | 33 | | +| `CRSF_FRAMETYPE_DEVICE_PING` | 40 | | +| `CRSF_FRAMETYPE_DEVICE_INFO` | 41 | | +| `CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY` | 43 | | +| `CRSF_FRAMETYPE_PARAMETER_READ` | 44 | | +| `CRSF_FRAMETYPE_PARAMETER_WRITE` | 45 | | +| `CRSF_FRAMETYPE_COMMAND` | 50 | | +| `CRSF_FRAMETYPE_MSP_REQ` | 122 | | +| `CRSF_FRAMETYPE_MSP_RESP` | 123 | | +| `CRSF_FRAMETYPE_MSP_WRITE` | 124 | | +| `CRSF_FRAMETYPE_DISPLAYPORT_CMD` | 125 | | + +--- +## `crsfFrameTypeIndex_e` + +> Source: ../../../src/main/telemetry/crsf.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `CRSF_FRAME_START_INDEX` | 0 | | +| `CRSF_FRAME_ATTITUDE_INDEX` | CRSF_FRAME_START_INDEX | | +| `CRSF_FRAME_BATTERY_SENSOR_INDEX` | | | +| `CRSF_FRAME_FLIGHT_MODE_INDEX` | | | +| `CRSF_FRAME_GPS_INDEX` | | | +| `CRSF_FRAME_VARIO_SENSOR_INDEX` | | | +| `CRSF_FRAME_BAROMETER_ALTITUDE_INDEX` | | | +| `CRSF_SCHEDULE_COUNT_MAX` | | | + +--- +## `crsrRfMode_e` + +> Source: ../../../src/main/telemetry/crsf.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `CRSF_RF_MODE_4_HZ` | 0 | | +| `CRSF_RF_MODE_50_HZ` | 1 | | +| `CRSF_RF_MODE_150_HZ` | 2 | | + +--- +## `crsrRfPower_e` + +> Source: ../../../src/main/telemetry/crsf.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `CRSF_RF_POWER_0_mW` | 0 | | +| `CRSF_RF_POWER_10_mW` | 1 | | +| `CRSF_RF_POWER_25_mW` | 2 | | +| `CRSF_RF_POWER_100_mW` | 3 | | +| `CRSF_RF_POWER_500_mW` | 4 | | +| `CRSF_RF_POWER_1000_mW` | 5 | | +| `CRSF_RF_POWER_2000_mW` | 6 | | +| `CRSF_RF_POWER_250_mW` | 7 | | + +--- +## `currentSensor_e` + +> Source: ../../../src/main/sensors/battery_config_structs.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `CURRENT_SENSOR_NONE` | 0 | | +| `CURRENT_SENSOR_ADC` | 1 | | +| `CURRENT_SENSOR_VIRTUAL` | 2 | | +| `CURRENT_SENSOR_FAKE` | 3 | | +| `CURRENT_SENSOR_ESC` | 4 | | +| `CURRENT_SENSOR_SMARTPORT` | 5 | | +| `CURRENT_SENSOR_MAX` | CURRENT_SENSOR_SMARTPORT | | + +--- +## `devHardwareType_e` + +> Source: ../../../src/main/drivers/bus.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `DEVHW_NONE` | 0 | | +| `DEVHW_MPU6000` | 1 | | +| `DEVHW_MPU6500` | 2 | | +| `DEVHW_BMI160` | 3 | | +| `DEVHW_BMI088_GYRO` | 4 | | +| `DEVHW_BMI088_ACC` | 5 | | +| `DEVHW_ICM20689` | 6 | | +| `DEVHW_ICM42605` | 7 | | +| `DEVHW_BMI270` | 8 | | +| `DEVHW_LSM6D` | 9 | | +| `DEVHW_MPU9250` | 10 | | +| `DEVHW_BMP085` | 11 | | +| `DEVHW_BMP280` | 12 | | +| `DEVHW_MS5611` | 13 | | +| `DEVHW_MS5607` | 14 | | +| `DEVHW_LPS25H` | 15 | | +| `DEVHW_SPL06` | 16 | | +| `DEVHW_BMP388` | 17 | | +| `DEVHW_DPS310` | 18 | | +| `DEVHW_B2SMPB` | 19 | | +| `DEVHW_HMC5883` | 20 | | +| `DEVHW_AK8963` | 21 | | +| `DEVHW_AK8975` | 22 | | +| `DEVHW_IST8310_0` | 23 | | +| `DEVHW_IST8310_1` | 24 | | +| `DEVHW_IST8308` | 25 | | +| `DEVHW_QMC5883` | 26 | | +| `DEVHW_QMC5883P` | 27 | | +| `DEVHW_MAG3110` | 28 | | +| `DEVHW_LIS3MDL` | 29 | | +| `DEVHW_RM3100` | 30 | | +| `DEVHW_VCM5883` | 31 | | +| `DEVHW_MLX90393` | 32 | | +| `DEVHW_LM75_0` | 33 | | +| `DEVHW_LM75_1` | 34 | | +| `DEVHW_LM75_2` | 35 | | +| `DEVHW_LM75_3` | 36 | | +| `DEVHW_LM75_4` | 37 | | +| `DEVHW_LM75_5` | 38 | | +| `DEVHW_LM75_6` | 39 | | +| `DEVHW_LM75_7` | 40 | | +| `DEVHW_DS2482` | 41 | | +| `DEVHW_MAX7456` | 42 | | +| `DEVHW_SRF10` | 43 | | +| `DEVHW_VL53L0X` | 44 | | +| `DEVHW_VL53L1X` | 45 | | +| `DEVHW_US42` | 46 | | +| `DEVHW_TOF10120_I2C` | 47 | | +| `DEVHW_TERARANGER_EVO_I2C` | 48 | | +| `DEVHW_MS4525` | 49 | | +| `DEVHW_DLVR` | 50 | | +| `DEVHW_M25P16` | 51 | | +| `DEVHW_W25N` | 52 | | +| `DEVHW_UG2864` | 53 | | +| `DEVHW_SDCARD` | 54 | | +| `DEVHW_IRLOCK` | 55 | | +| `DEVHW_PCF8574` | 56 | | + +--- +## `deviceFlags_e` + +> Source: ../../../src/main/drivers/bus.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `DEVFLAGS_NONE` | 0 | | +| `DEVFLAGS_USE_RAW_REGISTERS` | (1 << 0) | | +| `DEVFLAGS_USE_MANUAL_DEVICE_SELECT` | (1 << 1) | | +| `DEVFLAGS_SPI_MODE_0` | (1 << 2) | | + +--- +## `displayCanvasBitmapOption_t` + +> Source: ../../../src/main/drivers/display_canvas.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `DISPLAY_CANVAS_BITMAP_OPT_INVERT_COLORS` | 1 << 0 | | +| `DISPLAY_CANVAS_BITMAP_OPT_SOLID_BACKGROUND` | 1 << 1 | | +| `DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT` | 1 << 2 | | + +--- +## `displayCanvasColor_e` + +> Source: ../../../src/main/drivers/display_canvas.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `DISPLAY_CANVAS_COLOR_BLACK` | 0 | | +| `DISPLAY_CANVAS_COLOR_TRANSPARENT` | 1 | | +| `DISPLAY_CANVAS_COLOR_WHITE` | 2 | | +| `DISPLAY_CANVAS_COLOR_GRAY` | 3 | | + +--- +## `displayCanvasOutlineType_e` + +> Source: ../../../src/main/drivers/display_canvas.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `DISPLAY_CANVAS_OUTLINE_TYPE_NONE` | 0 | | +| `DISPLAY_CANVAS_OUTLINE_TYPE_TOP` | 1 << 0 | | +| `DISPLAY_CANVAS_OUTLINE_TYPE_RIGHT` | 1 << 1 | | +| `DISPLAY_CANVAS_OUTLINE_TYPE_BOTTOM` | 1 << 2 | | +| `DISPLAY_CANVAS_OUTLINE_TYPE_LEFT` | 1 << 3 | | + +--- +## `displayportMspCommand_e` + +> Source: ../../../src/main/io/displayport_msp.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MSP_DP_HEARTBEAT` | 0 | | +| `MSP_DP_RELEASE` | 1 | | +| `MSP_DP_CLEAR_SCREEN` | 2 | | +| `MSP_DP_WRITE_STRING` | 3 | | +| `MSP_DP_DRAW_SCREEN` | 4 | | +| `MSP_DP_OPTIONS` | 5 | | +| `MSP_DP_SYS` | 6 | | +| `MSP_DP_COUNT` | 7 | | + +--- +## `displayTransactionOption_e` + +> Source: ../../../src/main/drivers/display.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `DISPLAY_TRANSACTION_OPT_NONE` | 0 | | +| `DISPLAY_TRANSACTION_OPT_PROFILED` | 1 << 0 | | +| `DISPLAY_TRANSACTION_OPT_RESET_DRAWING` | 1 << 1 | | + +--- +## `displayWidgetType_e` + +> Source: ../../../src/main/drivers/display_widgets.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `DISPLAY_WIDGET_TYPE_AHI` | 0 | | +| `DISPLAY_WIDGET_TYPE_SIDEBAR` | 1 | | + +--- +## `DjiCraftNameElements_t` + +> Source: ../../../src/main/io/osd_dji_hd.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `DJI_OSD_CN_MESSAGES` | 0 | | +| `DJI_OSD_CN_THROTTLE` | 1 | | +| `DJI_OSD_CN_THROTTLE_AUTO_THR` | 2 | | +| `DJI_OSD_CN_AIR_SPEED` | 3 | | +| `DJI_OSD_CN_EFFICIENCY` | 4 | | +| `DJI_OSD_CN_DISTANCE` | 5 | | +| `DJI_OSD_CN_ADJUSTEMNTS` | 6 | | +| `DJI_OSD_CN_MAX_ELEMENTS` | 7 | | + +--- +## `dshotCommands_e` + +> Source: ../../../src/main/drivers/pwm_output.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `DSHOT_CMD_SPIN_DIRECTION_NORMAL` | 20 | | +| `DSHOT_CMD_SPIN_DIRECTION_REVERSED` | 21 | | + +--- +## `dumpFlags_e` + +> Source: ../../../src/main/fc/cli.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `DUMP_MASTER` | (1 << 0) | | +| `DUMP_CONTROL_PROFILE` | (1 << 1) | | +| `DUMP_BATTERY_PROFILE` | (1 << 2) | | +| `DUMP_MIXER_PROFILE` | (1 << 3) | | +| `DUMP_ALL` | (1 << 4) | | +| `DO_DIFF` | (1 << 5) | | +| `SHOW_DEFAULTS` | (1 << 6) | | +| `HIDE_UNUSED` | (1 << 7) | | + +--- +## `dynamicGyroNotchMode_e` + +> Source: ../../../src/main/sensors/gyro.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `DYNAMIC_NOTCH_MODE_2D` | 0 | | +| `DYNAMIC_NOTCH_MODE_3D` | 1 | | + +--- +## `emergLandState_e` + +> Source: ../../../src/main/flight/failsafe.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `EMERG_LAND_IDLE` | 0 | | +| `EMERG_LAND_IN_PROGRESS` | 1 | | +| `EMERG_LAND_HAS_LANDED` | 2 | | + +--- +## `escSensorFrameStatus_t` + +> Source: ../../../src/main/sensors/esc_sensor.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `ESC_SENSOR_FRAME_PENDING` | 0 | | +| `ESC_SENSOR_FRAME_COMPLETE` | 1 | | +| `ESC_SENSOR_FRAME_FAILED` | 2 | | + +--- +## `escSensorState_t` + +> Source: ../../../src/main/sensors/esc_sensor.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `ESC_SENSOR_WAIT_STARTUP` | 0 | | +| `ESC_SENSOR_READY` | 1 | | +| `ESC_SENSOR_WAITING` | 2 | | + +--- +## `failsafeChannelBehavior_e` + +> Source: ../../../src/main/flight/failsafe.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `FAILSAFE_CHANNEL_HOLD` | 0 | | +| `FAILSAFE_CHANNEL_NEUTRAL` | 1 | | + +--- +## `failsafePhase_e` + +> Source: ../../../src/main/flight/failsafe.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FAILSAFE_IDLE` | 0 | | +| `FAILSAFE_RX_LOSS_DETECTED` | 1 | | +| `FAILSAFE_RX_LOSS_IDLE` | 2 | | +| `FAILSAFE_RETURN_TO_HOME` | 3 | | +| `FAILSAFE_LANDING` | 4 | | +| `FAILSAFE_LANDED` | 5 | | +| `FAILSAFE_RX_LOSS_MONITORING` | 6 | | +| `FAILSAFE_RX_LOSS_RECOVERED` | 7 | | + +--- +## `failsafeProcedure_e` + +> Source: ../../../src/main/flight/failsafe.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FAILSAFE_PROCEDURE_AUTO_LANDING` | 0 | | +| `FAILSAFE_PROCEDURE_DROP_IT` | 1 | | +| `FAILSAFE_PROCEDURE_RTH` | 2 | | +| `FAILSAFE_PROCEDURE_NONE` | 3 | | + +--- +## `failsafeRxLinkState_e` + +> Source: ../../../src/main/flight/failsafe.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FAILSAFE_RXLINK_DOWN` | 0 | | +| `FAILSAFE_RXLINK_UP` | 1 | | + +--- +## `failureMode_e` + +> Source: ../../../src/main/drivers/system.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FAILURE_DEVELOPER` | 0 | | +| `FAILURE_MISSING_ACC` | 1 | | +| `FAILURE_ACC_INIT` | 2 | | +| `FAILURE_ACC_INCOMPATIBLE` | 3 | | +| `FAILURE_INVALID_EEPROM_CONTENTS` | 4 | | +| `FAILURE_FLASH_WRITE_FAILED` | 5 | | +| `FAILURE_GYRO_INIT_FAILED` | 6 | | +| `FAILURE_FLASH_READ_FAILED` | 7 | | + +--- +## `fatFilesystemType_e` + +> Source: ../../../src/main/io/asyncfatfs/fat_standard.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FAT_FILESYSTEM_TYPE_INVALID` | 0 | | +| `FAT_FILESYSTEM_TYPE_FAT12` | 1 | | +| `FAT_FILESYSTEM_TYPE_FAT16` | 2 | | +| `FAT_FILESYSTEM_TYPE_FAT32` | 3 | | + +--- +## `features_e` + +> Source: ../../../src/main/fc/config.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FEATURE_THR_VBAT_COMP` | 1 << 0 | | +| `FEATURE_VBAT` | 1 << 1 | | +| `FEATURE_TX_PROF_SEL` | 1 << 2 | | +| `FEATURE_BAT_PROFILE_AUTOSWITCH` | 1 << 3 | | +| `FEATURE_GEOZONE` | 1 << 4 | | +| `FEATURE_UNUSED_1` | 1 << 5 | | +| `FEATURE_SOFTSERIAL` | 1 << 6 | | +| `FEATURE_GPS` | 1 << 7 | | +| `FEATURE_UNUSED_3` | 1 << 8 | | +| `FEATURE_UNUSED_4` | 1 << 9 | | +| `FEATURE_TELEMETRY` | 1 << 10 | | +| `FEATURE_CURRENT_METER` | 1 << 11 | | +| `FEATURE_REVERSIBLE_MOTORS` | 1 << 12 | | +| `FEATURE_UNUSED_5` | 1 << 13 | | +| `FEATURE_UNUSED_6` | 1 << 14 | | +| `FEATURE_RSSI_ADC` | 1 << 15 | | +| `FEATURE_LED_STRIP` | 1 << 16 | | +| `FEATURE_DASHBOARD` | 1 << 17 | | +| `FEATURE_UNUSED_7` | 1 << 18 | | +| `FEATURE_BLACKBOX` | 1 << 19 | | +| `FEATURE_UNUSED_10` | 1 << 20 | | +| `FEATURE_TRANSPONDER` | 1 << 21 | | +| `FEATURE_AIRMODE` | 1 << 22 | | +| `FEATURE_SUPEREXPO_RATES` | 1 << 23 | | +| `FEATURE_VTX` | 1 << 24 | | +| `FEATURE_UNUSED_8` | 1 << 25 | | +| `FEATURE_UNUSED_9` | 1 << 26 | | +| `FEATURE_UNUSED_11` | 1 << 27 | | +| `FEATURE_PWM_OUTPUT_ENABLE` | 1 << 28 | | +| `FEATURE_OSD` | 1 << 29 | | +| `FEATURE_FW_LAUNCH` | 1 << 30 | | +| `FEATURE_FW_AUTOTRIM` | 1 << 31 | | + +--- +## `filterType_e` + +> Source: ../../../src/main/common/filter.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FILTER_PT1` | 0 | | +| `FILTER_BIQUAD` | 1 | | +| `FILTER_PT2` | 2 | | +| `FILTER_PT3` | 3 | | +| `FILTER_LULU` | 4 | | + +--- +## `fixedWingLaunchEvent_t` + +> Source: ../../../src/main/navigation/navigation_fw_launch.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `FW_LAUNCH_EVENT_NONE` | 0 | | +| `FW_LAUNCH_EVENT_SUCCESS` | 1 | | +| `FW_LAUNCH_EVENT_GOTO_DETECTION` | 2 | | +| `FW_LAUNCH_EVENT_ABORT` | 3 | | +| `FW_LAUNCH_EVENT_THROTTLE_LOW` | 4 | | +| `FW_LAUNCH_EVENT_COUNT` | 5 | | + +--- +## `fixedWingLaunchMessage_t` + +> Source: ../../../src/main/navigation/navigation_fw_launch.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `FW_LAUNCH_MESSAGE_TYPE_NONE` | 0 | | +| `FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE` | 1 | | +| `FW_LAUNCH_MESSAGE_TYPE_WAIT_IDLE` | 2 | | +| `FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION` | 3 | | +| `FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS` | 4 | | +| `FW_LAUNCH_MESSAGE_TYPE_FINISHING` | 5 | | + +--- +## `fixedWingLaunchState_t` + +> Source: ../../../src/main/navigation/navigation_fw_launch.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `FW_LAUNCH_STATE_WAIT_THROTTLE` | 0 | | +| `FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT` | 1 | | +| `FW_LAUNCH_STATE_IDLE_MOTOR_DELAY` | 2 | | +| `FW_LAUNCH_STATE_MOTOR_IDLE` | 3 | | +| `FW_LAUNCH_STATE_WAIT_DETECTION` | 4 | | +| `FW_LAUNCH_STATE_DETECTED` | 5 | | +| `FW_LAUNCH_STATE_MOTOR_DELAY` | 6 | | +| `FW_LAUNCH_STATE_MOTOR_SPINUP` | 7 | | +| `FW_LAUNCH_STATE_IN_PROGRESS` | 8 | | +| `FW_LAUNCH_STATE_FINISH` | 9 | | +| `FW_LAUNCH_STATE_ABORTED` | 10 | | +| `FW_LAUNCH_STATE_FLYING` | 11 | | +| `FW_LAUNCH_STATE_COUNT` | 12 | | + +--- +## `flashPartitionType_e` + +> Source: ../../../src/main/drivers/flash.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FLASH_PARTITION_TYPE_UNKNOWN` | 0 | | +| `FLASH_PARTITION_TYPE_PARTITION_TABLE` | 1 | | +| `FLASH_PARTITION_TYPE_FLASHFS` | 2 | | +| `FLASH_PARTITION_TYPE_BADBLOCK_MANAGEMENT` | 3 | | +| `FLASH_PARTITION_TYPE_FIRMWARE` | 4 | | +| `FLASH_PARTITION_TYPE_CONFIG` | 5 | | +| `FLASH_PARTITION_TYPE_FULL_BACKUP` | 6 | | +| `FLASH_PARTITION_TYPE_FIRMWARE_UPDATE_META` | 7 | | +| `FLASH_PARTITION_TYPE_UPDATE_FIRMWARE` | 8 | | +| `FLASH_MAX_PARTITIONS` | 9 | | + +--- +## `flashType_e` + +> Source: ../../../src/main/drivers/flash.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FLASH_TYPE_NOR` | 0 | | +| `FLASH_TYPE_NAND` | 1 | | + +--- +## `flight_dynamics_index_t` + +> Source: ../../../src/main/common/axis.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FD_ROLL` | 0 | | +| `FD_PITCH` | 1 | | +| `FD_YAW` | 2 | | + +--- +## `flightModeFlags_e` + +> Source: ../../../src/main/fc/runtime_config.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `ANGLE_MODE` | (1 << 0) | | +| `HORIZON_MODE` | (1 << 1) | | +| `HEADING_MODE` | (1 << 2) | | +| `NAV_ALTHOLD_MODE` | (1 << 3) | | +| `NAV_RTH_MODE` | (1 << 4) | | +| `NAV_POSHOLD_MODE` | (1 << 5) | | +| `HEADFREE_MODE` | (1 << 6) | | +| `NAV_LAUNCH_MODE` | (1 << 7) | | +| `MANUAL_MODE` | (1 << 8) | | +| `FAILSAFE_MODE` | (1 << 9) | | +| `AUTO_TUNE` | (1 << 10) | | +| `NAV_WP_MODE` | (1 << 11) | | +| `NAV_COURSE_HOLD_MODE` | (1 << 12) | | +| `FLAPERON` | (1 << 13) | | +| `TURN_ASSISTANT` | (1 << 14) | | +| `TURTLE_MODE` | (1 << 15) | | +| `SOARING_MODE` | (1 << 16) | | +| `ANGLEHOLD_MODE` | (1 << 17) | | +| `NAV_FW_AUTOLAND` | (1 << 18) | | +| `NAV_SEND_TO` | (1 << 19) | | + +--- +## `flightModeForTelemetry_e` + +> Source: ../../../src/main/fc/runtime_config.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FLM_MANUAL` | 0 | | +| `FLM_ACRO` | 1 | | +| `FLM_ACRO_AIR` | 2 | | +| `FLM_ANGLE` | 3 | | +| `FLM_HORIZON` | 4 | | +| `FLM_ALTITUDE_HOLD` | 5 | | +| `FLM_POSITION_HOLD` | 6 | | +| `FLM_RTH` | 7 | | +| `FLM_MISSION` | 8 | | +| `FLM_COURSE_HOLD` | 9 | | +| `FLM_CRUISE` | 10 | | +| `FLM_LAUNCH` | 11 | | +| `FLM_FAILSAFE` | 12 | | +| `FLM_ANGLEHOLD` | 13 | | +| `FLM_COUNT` | 14 | | + +--- +## `flyingPlatformType_e` + +> Source: ../../../src/main/flight/mixer.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `PLATFORM_MULTIROTOR` | 0 | | +| `PLATFORM_AIRPLANE` | 1 | | +| `PLATFORM_HELICOPTER` | 2 | | +| `PLATFORM_TRICOPTER` | 3 | | +| `PLATFORM_ROVER` | 4 | | +| `PLATFORM_BOAT` | 5 | | + +--- +## `fport2_control_frame_type_e` + +> Source: ../../../src/main/rx/fport2.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `CFT_RC` | 255 | | +| `CFT_OTA_START` | 240 | | +| `CFT_OTA_DATA` | 241 | | +| `CFT_OTA_STOP` | 242 | | + +--- +## `frame_state_e` + +> Source: ../../../src/main/rx/fport2.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `FS_CONTROL_FRAME_START` | 0 | | +| `FS_CONTROL_FRAME_TYPE` | 1 | | +| `FS_CONTROL_FRAME_DATA` | 2 | | +| `FS_DOWNLINK_FRAME_START` | 3 | | +| `FS_DOWNLINK_FRAME_DATA` | 4 | | + +--- +## `frame_type_e` + +> Source: ../../../src/main/rx/fport2.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `FT_CONTROL` | 0 | | +| `FT_DOWNLINK` | 1 | | + +--- +## `frskyOSDColor_e` + +> Source: ../../../src/main/io/frsky_osd.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FRSKY_OSD_COLOR_BLACK` | 0 | | +| `FRSKY_OSD_COLOR_TRANSPARENT` | 1 | | +| `FRSKY_OSD_COLOR_WHITE` | 2 | | +| `FRSKY_OSD_COLOR_GRAY` | 3 | | + +--- +## `frskyOSDLineOutlineType_e` + +> Source: ../../../src/main/io/frsky_osd.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FRSKY_OSD_OUTLINE_TYPE_NONE` | 0 | | +| `FRSKY_OSD_OUTLINE_TYPE_TOP` | 1 << 0 | | +| `FRSKY_OSD_OUTLINE_TYPE_RIGHT` | 1 << 1 | | +| `FRSKY_OSD_OUTLINE_TYPE_BOTTOM` | 1 << 2 | | +| `FRSKY_OSD_OUTLINE_TYPE_LEFT` | 1 << 3 | | + +--- +## `frskyOSDRecvState_e` + +> Source: ../../../src/main/io/frsky_osd.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `RECV_STATE_NONE` | 0 | | +| `RECV_STATE_SYNC` | 1 | | +| `RECV_STATE_LENGTH` | 2 | | +| `RECV_STATE_DATA` | 3 | | +| `RECV_STATE_CHECKSUM` | 4 | | +| `RECV_STATE_DONE` | 5 | | + +--- +## `frskyOSDTransactionOptions_e` + +> Source: ../../../src/main/io/frsky_osd.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FRSKY_OSD_TRANSACTION_OPT_PROFILED` | 1 << 0 | | +| `FRSKY_OSD_TRANSACTION_OPT_RESET_DRAWING` | 1 << 1 | | + +--- +## `fw_autotune_rate_adjustment_e` + +> Source: ../../../src/main/flight/pid.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FIXED` | 0 | | +| `LIMIT` | 1 | | +| `AUTO` | 2 | | + +--- +## `fwAutolandApproachDirection_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FW_AUTOLAND_APPROACH_DIRECTION_LEFT` | 0 | | +| `FW_AUTOLAND_APPROACH_DIRECTION_RIGHT` | 1 | | + +--- +## `fwAutolandState_t` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FW_AUTOLAND_STATE_IDLE` | 0 | | +| `FW_AUTOLAND_STATE_LOITER` | 1 | | +| `FW_AUTOLAND_STATE_DOWNWIND` | 2 | | +| `FW_AUTOLAND_STATE_BASE_LEG` | 3 | | +| `FW_AUTOLAND_STATE_FINAL_APPROACH` | 4 | | +| `FW_AUTOLAND_STATE_GLIDE` | 5 | | +| `FW_AUTOLAND_STATE_FLARE` | 6 | | + +--- +## `fwAutolandWaypoint_t` + +> Source: ../../../src/main/navigation/navigation_private.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FW_AUTOLAND_WP_TURN` | 0 | | +| `FW_AUTOLAND_WP_FINAL_APPROACH` | 1 | | +| `FW_AUTOLAND_WP_LAND` | 2 | | +| `FW_AUTOLAND_WP_COUNT` | 3 | | + +--- +## `geoAltitudeConversionMode_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GEO_ALT_ABSOLUTE` | 0 | | +| `GEO_ALT_RELATIVE` | 1 | | + +--- +## `geoAltitudeDatumFlag_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_WP_TAKEOFF_DATUM` | 0 | | +| `NAV_WP_MSL_DATUM` | 1 | | + +--- +## `geoOriginResetMode_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GEO_ORIGIN_SET` | 0 | | +| `GEO_ORIGIN_RESET_ALTITUDE` | 1 | | + +--- +## `geozoneActionState_e` + +> Source: ../../../src/main/navigation/navigation_geozone.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `GEOZONE_ACTION_STATE_NONE` | 0 | | +| `GEOZONE_ACTION_STATE_AVOIDING` | 1 | | +| `GEOZONE_ACTION_STATE_AVOIDING_UPWARD` | 2 | | +| `GEOZONE_ACTION_STATE_AVOIDING_ALTITUDE` | 3 | | +| `GEOZONE_ACTION_STATE_RETURN_TO_FZ` | 4 | | +| `GEOZONE_ACTION_STATE_FLYOUT_NFZ` | 5 | | +| `GEOZONE_ACTION_STATE_POSHOLD` | 6 | | +| `GEOZONE_ACTION_STATE_RTH` | 7 | | + +--- +## `geozoneMessageState_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GEOZONE_MESSAGE_STATE_NONE` | 0 | | +| `GEOZONE_MESSAGE_STATE_NFZ` | 1 | | +| `GEOZONE_MESSAGE_STATE_LEAVING_FZ` | 2 | | +| `GEOZONE_MESSAGE_STATE_OUTSIDE_FZ` | 3 | | +| `GEOZONE_MESSAGE_STATE_ENTERING_NFZ` | 4 | | +| `GEOZONE_MESSAGE_STATE_AVOIDING_FB` | 5 | | +| `GEOZONE_MESSAGE_STATE_RETURN_TO_ZONE` | 6 | | +| `GEOZONE_MESSAGE_STATE_FLYOUT_NFZ` | 7 | | +| `GEOZONE_MESSAGE_STATE_AVOIDING_ALTITUDE_BREACH` | 8 | | +| `GEOZONE_MESSAGE_STATE_POS_HOLD` | 9 | | + +--- +## `ghstAddr_e` + +> Source: ../../../src/main/rx/ghst_protocol.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GHST_ADDR_RADIO` | 128 | | +| `GHST_ADDR_TX_MODULE_SYM` | 129 | | +| `GHST_ADDR_TX_MODULE_ASYM` | 136 | | +| `GHST_ADDR_FC` | 130 | | +| `GHST_ADDR_GOGGLES` | 131 | | +| `GHST_ADDR_QUANTUM_TEE1` | 132 | | +| `GHST_ADDR_QUANTUM_TEE2` | 133 | | +| `GHST_ADDR_QUANTUM_GW1` | 134 | | +| `GHST_ADDR_5G_CLK` | 135 | | +| `GHST_ADDR_RX` | 137 | | + +--- +## `ghstDl_e` + +> Source: ../../../src/main/rx/ghst_protocol.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GHST_DL_OPENTX_SYNC` | 32 | | +| `GHST_DL_LINK_STAT` | 33 | | +| `GHST_DL_VTX_STAT` | 34 | | +| `GHST_DL_PACK_STAT` | 35 | | +| `GHST_DL_GPS_PRIMARY` | 37 | | +| `GHST_DL_GPS_SECONDARY` | 38 | | + +--- +## `ghstFrameTypeIndex_e` + +> Source: ../../../src/main/telemetry/ghst.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `GHST_FRAME_START_INDEX` | 0 | | +| `GHST_FRAME_PACK_INDEX` | GHST_FRAME_START_INDEX | | +| `GHST_FRAME_GPS_PRIMARY_INDEX` | | | +| `GHST_FRAME_GPS_SECONDARY_INDEX` | | | +| `GHST_SCHEDULE_COUNT_MAX` | | | + +--- +## `ghstUl_e` + +> Source: ../../../src/main/rx/ghst_protocol.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GHST_UL_RC_CHANS_HS4_FIRST` | 16 | | +| `GHST_UL_RC_CHANS_HS4_5TO8` | 16 | | +| `GHST_UL_RC_CHANS_HS4_9TO12` | 17 | | +| `GHST_UL_RC_CHANS_HS4_13TO16` | 18 | | +| `GHST_UL_RC_CHANS_HS4_RSSI` | 19 | | +| `GHST_UL_RC_CHANS_HS4_LAST` | 31 | | + +--- +## `gimbal_htk_mode_e` + +> Source: ../../../src/main/drivers/gimbal_common.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GIMBAL_MODE_FOLLOW` | 0 | | +| `GIMBAL_MODE_TILT_LOCK` | (1<<0) | | +| `GIMBAL_MODE_ROLL_LOCK` | (1<<1) | | +| `GIMBAL_MODE_PAN_LOCK` | (1<<2) | | + +--- +## `gimbalDevType_e` + +> Source: ../../../src/main/drivers/gimbal_common.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GIMBAL_DEV_UNSUPPORTED` | 0 | | +| `GIMBAL_DEV_SERIAL` | 1 | | +| `GIMBAL_DEV_UNKNOWN` | 255 | | + +--- +## `gimbalHeadtrackerState_e` + +> Source: ../../../src/main/io/gimbal_serial.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `WAITING_HDR1` | 0 | | +| `WAITING_HDR2` | 1 | | +| `WAITING_PAYLOAD` | 2 | | +| `WAITING_CRCH` | 3 | | +| `WAITING_CRCL` | 4 | | + +--- +## `gpsAutoBaud_e` + +> Source: ../../../src/main/io/gps.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GPS_AUTOBAUD_OFF` | 0 | | +| `GPS_AUTOBAUD_ON` | 1 | | + +--- +## `gpsAutoConfig_e` + +> Source: ../../../src/main/io/gps.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GPS_AUTOCONFIG_OFF` | 0 | | +| `GPS_AUTOCONFIG_ON` | 1 | | + +--- +## `gpsBaudRate_e` + +> Source: ../../../src/main/io/gps.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GPS_BAUDRATE_115200` | 0 | | +| `GPS_BAUDRATE_57600` | 1 | | +| `GPS_BAUDRATE_38400` | 2 | | +| `GPS_BAUDRATE_19200` | 3 | | +| `GPS_BAUDRATE_9600` | 4 | | +| `GPS_BAUDRATE_230400` | 5 | | +| `GPS_BAUDRATE_460800` | 6 | | +| `GPS_BAUDRATE_921600` | 7 | | +| `GPS_BAUDRATE_COUNT` | 8 | | + +--- +## `gpsDynModel_e` + +> Source: ../../../src/main/io/gps.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GPS_DYNMODEL_PEDESTRIAN` | 0 | | +| `GPS_DYNMODEL_AUTOMOTIVE` | 1 | | +| `GPS_DYNMODEL_AIR_1G` | 2 | | +| `GPS_DYNMODEL_AIR_2G` | 3 | | +| `GPS_DYNMODEL_AIR_4G` | 4 | | +| `GPS_DYNMODEL_SEA` | 5 | | +| `GPS_DYNMODEL_MOWER` | 6 | | + +--- +## `gpsFixChar_e` + +> Source: ../../../src/main/telemetry/hott.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `GPS_FIX_CHAR_NONE` | '-' | | +| `GPS_FIX_CHAR_2D` | '2' | | +| `GPS_FIX_CHAR_3D` | '3' | | +| `GPS_FIX_CHAR_DGPS` | 'D' | | + +--- +## `gpsFixType_e` + +> Source: ../../../src/main/io/gps.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GPS_NO_FIX` | 0 | | +| `GPS_FIX_2D` | 1 | | +| `GPS_FIX_3D` | 2 | | + +--- +## `gpsProvider_e` + +> Source: ../../../src/main/io/gps.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GPS_UBLOX` | 0 | | +| `GPS_MSP` | 1 | | +| `GPS_FAKE` | 2 | | +| `GPS_PROVIDER_COUNT` | 3 | | + +--- +## `gpsState_e` + +> Source: ../../../src/main/io/gps_private.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GPS_UNKNOWN` | 0 | | +| `GPS_INITIALIZING` | 1 | | +| `GPS_RUNNING` | 2 | | +| `GPS_LOST_COMMUNICATION` | 3 | | + +--- +## `gyroFilterMode_e` + +> Source: ../../../src/main/sensors/gyro.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GYRO_FILTER_MODE_OFF` | 0 | | +| `GYRO_FILTER_MODE_STATIC` | 1 | | +| `GYRO_FILTER_MODE_DYNAMIC` | 2 | | +| `GYRO_FILTER_MODE_ADAPTIVE` | 3 | | + +--- +## `gyroHardwareLpf_e` + +> Source: ../../../src/main/drivers/accgyro/accgyro_lsm6dxx.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GYRO_HARDWARE_LPF_NORMAL` | 0 | | +| `GYRO_HARDWARE_LPF_OPTION_1` | 1 | | +| `GYRO_HARDWARE_LPF_OPTION_2` | 2 | | +| `GYRO_HARDWARE_LPF_EXPERIMENTAL` | 3 | | +| `GYRO_HARDWARE_LPF_COUNT` | 4 | | + +--- +## `gyroSensor_e` + +> Source: ../../../src/main/sensors/gyro.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GYRO_NONE` | 0 | | +| `GYRO_AUTODETECT` | 1 | | +| `GYRO_MPU6000` | 2 | | +| `GYRO_MPU6500` | 3 | | +| `GYRO_MPU9250` | 4 | | +| `GYRO_BMI160` | 5 | | +| `GYRO_ICM20689` | 6 | | +| `GYRO_BMI088` | 7 | | +| `GYRO_ICM42605` | 8 | | +| `GYRO_BMI270` | 9 | | +| `GYRO_LSM6DXX` | 10 | | +| `GYRO_FAKE` | 11 | | + +--- +## `HardwareMotorTypes_e` + +> Source: ../../../src/main/drivers/pwm_esc_detect.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MOTOR_UNKNOWN` | 0 | | +| `MOTOR_BRUSHED` | 1 | | +| `MOTOR_BRUSHLESS` | 2 | | + +--- +## `hardwareSensorStatus_e` + +> Source: ../../../src/main/sensors/diagnostics.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `HW_SENSOR_NONE` | 0 | | +| `HW_SENSOR_OK` | 1 | | +| `HW_SENSOR_UNAVAILABLE` | 2 | | +| `HW_SENSOR_UNHEALTHY` | 3 | | + +--- +## `headTrackerDevType_e` + +> Source: ../../../src/main/drivers/headtracker_common.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `HEADTRACKER_NONE` | 0 | | +| `HEADTRACKER_SERIAL` | 1 | | +| `HEADTRACKER_MSP` | 2 | | +| `HEADTRACKER_UNKNOWN` | 255 | | + +--- +## `hottEamAlarm1Flag_e` + +> Source: ../../../src/main/telemetry/hott.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `HOTT_EAM_ALARM1_FLAG_NONE` | 0 | | +| `HOTT_EAM_ALARM1_FLAG_MAH` | (1 << 0) | | +| `HOTT_EAM_ALARM1_FLAG_BATTERY_1` | (1 << 1) | | +| `HOTT_EAM_ALARM1_FLAG_BATTERY_2` | (1 << 2) | | +| `HOTT_EAM_ALARM1_FLAG_TEMPERATURE_1` | (1 << 3) | | +| `HOTT_EAM_ALARM1_FLAG_TEMPERATURE_2` | (1 << 4) | | +| `HOTT_EAM_ALARM1_FLAG_ALTITUDE` | (1 << 5) | | +| `HOTT_EAM_ALARM1_FLAG_CURRENT` | (1 << 6) | | +| `HOTT_EAM_ALARM1_FLAG_MAIN_VOLTAGE` | (1 << 7) | | + +--- +## `hottEamAlarm2Flag_e` + +> Source: ../../../src/main/telemetry/hott.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `HOTT_EAM_ALARM2_FLAG_NONE` | 0 | | +| `HOTT_EAM_ALARM2_FLAG_MS` | (1 << 0) | | +| `HOTT_EAM_ALARM2_FLAG_M3S` | (1 << 1) | | +| `HOTT_EAM_ALARM2_FLAG_ALTITUDE_DUPLICATE` | (1 << 2) | | +| `HOTT_EAM_ALARM2_FLAG_MS_DUPLICATE` | (1 << 3) | | +| `HOTT_EAM_ALARM2_FLAG_M3S_DUPLICATE` | (1 << 4) | | +| `HOTT_EAM_ALARM2_FLAG_UNKNOWN_1` | (1 << 5) | | +| `HOTT_EAM_ALARM2_FLAG_UNKNOWN_2` | (1 << 6) | | +| `HOTT_EAM_ALARM2_FLAG_ON_SIGN_OR_TEXT_ACTIVE` | (1 << 7) | | + +--- +## `hottState_e` + +> Source: ../../../src/main/telemetry/hott.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `HOTT_WAITING_FOR_REQUEST` | 0 | | +| `HOTT_RECEIVING_REQUEST` | 1 | | +| `HOTT_WAITING_FOR_TX_WINDOW` | 2 | | +| `HOTT_TRANSMITTING` | 3 | | +| `HOTT_ENDING_TRANSMISSION` | 4 | | + +--- +## `hsvColorComponent_e` + +> Source: ../../../src/main/common/color.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `HSV_HUE` | 0 | | +| `HSV_SATURATION` | 1 | | +| `HSV_VALUE` | 2 | | + +--- +## `I2CSpeed` + +> Source: ../../../src/main/drivers/bus_i2c.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `I2C_SPEED_100KHZ` | 2 | | +| `I2C_SPEED_200KHZ` | 3 | | +| `I2C_SPEED_400KHZ` | 0 | | +| `I2C_SPEED_800KHZ` | 1 | | + +--- +## `i2cState_t` + +> Source: ../../../src/main/drivers/bus_i2c_stm32f40x.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `I2C_STATE_STOPPED` | 0 | | +| `I2C_STATE_STOPPING` | 1 | | +| `I2C_STATE_STARTING` | 2 | | +| `I2C_STATE_STARTING_WAIT` | 3 | | +| `I2C_STATE_R_ADDR` | 4 | | +| `I2C_STATE_R_ADDR_WAIT` | 5 | | +| `I2C_STATE_R_REGISTER` | 6 | | +| `I2C_STATE_R_REGISTER_WAIT` | 7 | | +| `I2C_STATE_R_RESTARTING` | 8 | | +| `I2C_STATE_R_RESTARTING_WAIT` | 9 | | +| `I2C_STATE_R_RESTART_ADDR` | 10 | | +| `I2C_STATE_R_RESTART_ADDR_WAIT` | 11 | | +| `I2C_STATE_R_TRANSFER_EQ1` | 12 | | +| `I2C_STATE_R_TRANSFER_EQ2` | 13 | | +| `I2C_STATE_R_TRANSFER_GE2` | 14 | | +| `I2C_STATE_W_ADDR` | 15 | | +| `I2C_STATE_W_ADDR_WAIT` | 16 | | +| `I2C_STATE_W_REGISTER` | 17 | | +| `I2C_STATE_W_TRANSFER_WAIT` | 18 | | +| `I2C_STATE_W_TRANSFER` | 19 | | +| `I2C_STATE_NACK` | 20 | | +| `I2C_STATE_BUS_ERROR` | 21 | | + +--- +## `i2cTransferDirection_t` + +> Source: ../../../src/main/drivers/bus_i2c_stm32f40x.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `I2C_TXN_READ` | 0 | | +| `I2C_TXN_WRITE` | 1 | | + +--- +## `ibusCommand_e` + +> Source: ../../../src/main/telemetry/ibus_shared.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `IBUS_COMMAND_DISCOVER_SENSOR` | 128 | | +| `IBUS_COMMAND_SENSOR_TYPE` | 144 | | +| `IBUS_COMMAND_MEASUREMENT` | 160 | | + +--- +## `ibusSensorType1_e` + +> Source: ../../../src/main/telemetry/ibus_shared.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `IBUS_MEAS_TYPE1_INTV` | 0 | | +| `IBUS_MEAS_TYPE1_TEM` | 1 | | +| `IBUS_MEAS_TYPE1_MOT` | 2 | | +| `IBUS_MEAS_TYPE1_EXTV` | 3 | | +| `IBUS_MEAS_TYPE1_CELL` | 4 | | +| `IBUS_MEAS_TYPE1_BAT_CURR` | 5 | | +| `IBUS_MEAS_TYPE1_FUEL` | 6 | | +| `IBUS_MEAS_TYPE1_RPM` | 7 | | +| `IBUS_MEAS_TYPE1_CMP_HEAD` | 8 | | +| `IBUS_MEAS_TYPE1_CLIMB_RATE` | 9 | | +| `IBUS_MEAS_TYPE1_COG` | 10 | | +| `IBUS_MEAS_TYPE1_GPS_STATUS` | 11 | | +| `IBUS_MEAS_TYPE1_ACC_X` | 12 | | +| `IBUS_MEAS_TYPE1_ACC_Y` | 13 | | +| `IBUS_MEAS_TYPE1_ACC_Z` | 14 | | +| `IBUS_MEAS_TYPE1_ROLL` | 15 | | +| `IBUS_MEAS_TYPE1_PITCH` | 16 | | +| `IBUS_MEAS_TYPE1_YAW` | 17 | | +| `IBUS_MEAS_TYPE1_VERTICAL_SPEED` | 18 | | +| `IBUS_MEAS_TYPE1_GROUND_SPEED` | 19 | | +| `IBUS_MEAS_TYPE1_GPS_DIST` | 20 | | +| `IBUS_MEAS_TYPE1_ARMED` | 21 | | +| `IBUS_MEAS_TYPE1_FLIGHT_MODE` | 22 | | +| `IBUS_MEAS_TYPE1_PRES` | 65 | | +| `IBUS_MEAS_TYPE1_SPE` | 126 | | +| `IBUS_MEAS_TYPE1_GPS_LAT` | 128 | | +| `IBUS_MEAS_TYPE1_GPS_LON` | 129 | | +| `IBUS_MEAS_TYPE1_GPS_ALT` | 130 | | +| `IBUS_MEAS_TYPE1_ALT` | 131 | | +| `IBUS_MEAS_TYPE1_S84` | 132 | | +| `IBUS_MEAS_TYPE1_S85` | 133 | | +| `IBUS_MEAS_TYPE1_S86` | 134 | | +| `IBUS_MEAS_TYPE1_S87` | 135 | | +| `IBUS_MEAS_TYPE1_S88` | 136 | | +| `IBUS_MEAS_TYPE1_S89` | 137 | | +| `IBUS_MEAS_TYPE1_S8a` | 138 | | + +--- +## `ibusSensorType_e` + +> Source: ../../../src/main/telemetry/ibus_shared.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `IBUS_MEAS_TYPE_INTERNAL_VOLTAGE` | 0 | | +| `IBUS_MEAS_TYPE_TEMPERATURE` | 1 | | +| `IBUS_MEAS_TYPE_RPM` | 2 | | +| `IBUS_MEAS_TYPE_EXTERNAL_VOLTAGE` | 3 | | +| `IBUS_MEAS_TYPE_HEADING` | 4 | | +| `IBUS_MEAS_TYPE_CURRENT` | 5 | | +| `IBUS_MEAS_TYPE_CLIMB` | 6 | | +| `IBUS_MEAS_TYPE_ACC_Z` | 7 | | +| `IBUS_MEAS_TYPE_ACC_Y` | 8 | | +| `IBUS_MEAS_TYPE_ACC_X` | 9 | | +| `IBUS_MEAS_TYPE_VSPEED` | 10 | | +| `IBUS_MEAS_TYPE_SPEED` | 11 | | +| `IBUS_MEAS_TYPE_DIST` | 12 | | +| `IBUS_MEAS_TYPE_ARMED` | 13 | | +| `IBUS_MEAS_TYPE_MODE` | 14 | | +| `IBUS_MEAS_TYPE_PRES` | 65 | | +| `IBUS_MEAS_TYPE_SPE` | 126 | | +| `IBUS_MEAS_TYPE_COG` | 128 | | +| `IBUS_MEAS_TYPE_GPS_STATUS` | 129 | | +| `IBUS_MEAS_TYPE_GPS_LON` | 130 | | +| `IBUS_MEAS_TYPE_GPS_LAT` | 131 | | +| `IBUS_MEAS_TYPE_ALT` | 132 | | +| `IBUS_MEAS_TYPE_S85` | 133 | | +| `IBUS_MEAS_TYPE_S86` | 134 | | +| `IBUS_MEAS_TYPE_S87` | 135 | | +| `IBUS_MEAS_TYPE_S88` | 136 | | +| `IBUS_MEAS_TYPE_S89` | 137 | | +| `IBUS_MEAS_TYPE_S8A` | 138 | | +| `IBUS_MEAS_TYPE_GALT` | 249 | | +| `IBUS_MEAS_TYPE_GPS` | 253 | | + +--- +## `ibusSensorValue_e` + +> Source: ../../../src/main/telemetry/ibus_shared.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `IBUS_MEAS_VALUE_NONE` | 0 | | +| `IBUS_MEAS_VALUE_TEMPERATURE` | 1 | | +| `IBUS_MEAS_VALUE_MOT` | 2 | | +| `IBUS_MEAS_VALUE_EXTERNAL_VOLTAGE` | 3 | | +| `IBUS_MEAS_VALUE_CELL` | 4 | | +| `IBUS_MEAS_VALUE_CURRENT` | 5 | | +| `IBUS_MEAS_VALUE_FUEL` | 6 | | +| `IBUS_MEAS_VALUE_RPM` | 7 | | +| `IBUS_MEAS_VALUE_HEADING` | 8 | | +| `IBUS_MEAS_VALUE_CLIMB` | 9 | | +| `IBUS_MEAS_VALUE_COG` | 10 | | +| `IBUS_MEAS_VALUE_GPS_STATUS` | 11 | | +| `IBUS_MEAS_VALUE_ACC_X` | 12 | | +| `IBUS_MEAS_VALUE_ACC_Y` | 13 | | +| `IBUS_MEAS_VALUE_ACC_Z` | 14 | | +| `IBUS_MEAS_VALUE_ROLL` | 15 | | +| `IBUS_MEAS_VALUE_PITCH` | 16 | | +| `IBUS_MEAS_VALUE_YAW` | 17 | | +| `IBUS_MEAS_VALUE_VSPEED` | 18 | | +| `IBUS_MEAS_VALUE_SPEED` | 19 | | +| `IBUS_MEAS_VALUE_DIST` | 20 | | +| `IBUS_MEAS_VALUE_ARMED` | 21 | | +| `IBUS_MEAS_VALUE_MODE` | 22 | | +| `IBUS_MEAS_VALUE_PRES` | 65 | | +| `IBUS_MEAS_VALUE_SPE` | 126 | | +| `IBUS_MEAS_VALUE_GPS_LAT` | 128 | | +| `IBUS_MEAS_VALUE_GPS_LON` | 129 | | +| `IBUS_MEAS_VALUE_GALT4` | 130 | | +| `IBUS_MEAS_VALUE_ALT4` | 131 | | +| `IBUS_MEAS_VALUE_GALT` | 132 | | +| `IBUS_MEAS_VALUE_ALT` | 133 | | +| `IBUS_MEAS_VALUE_STATUS` | 135 | | +| `IBUS_MEAS_VALUE_GPS_LAT1` | 136 | | +| `IBUS_MEAS_VALUE_GPS_LON1` | 137 | | +| `IBUS_MEAS_VALUE_GPS_LAT2` | 144 | | +| `IBUS_MEAS_VALUE_GPS_LON2` | 145 | | +| `IBUS_MEAS_VALUE_GPS` | 253 | | + +--- +## `inputSource_e` + +> Source: ../../../src/main/flight/servos.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `INPUT_STABILIZED_ROLL` | 0 | | +| `INPUT_STABILIZED_PITCH` | 1 | | +| `INPUT_STABILIZED_YAW` | 2 | | +| `INPUT_STABILIZED_THROTTLE` | 3 | | +| `INPUT_RC_ROLL` | 4 | | +| `INPUT_RC_PITCH` | 5 | | +| `INPUT_RC_YAW` | 6 | | +| `INPUT_RC_THROTTLE` | 7 | | +| `INPUT_RC_CH5` | 8 | | +| `INPUT_RC_CH6` | 9 | | +| `INPUT_RC_CH7` | 10 | | +| `INPUT_RC_CH8` | 11 | | +| `INPUT_GIMBAL_PITCH` | 12 | | +| `INPUT_GIMBAL_ROLL` | 13 | | +| `INPUT_FEATURE_FLAPS` | 14 | | +| `INPUT_RC_CH9` | 15 | | +| `INPUT_RC_CH10` | 16 | | +| `INPUT_RC_CH11` | 17 | | +| `INPUT_RC_CH12` | 18 | | +| `INPUT_RC_CH13` | 19 | | +| `INPUT_RC_CH14` | 20 | | +| `INPUT_RC_CH15` | 21 | | +| `INPUT_RC_CH16` | 22 | | +| `INPUT_STABILIZED_ROLL_PLUS` | 23 | | +| `INPUT_STABILIZED_ROLL_MINUS` | 24 | | +| `INPUT_STABILIZED_PITCH_PLUS` | 25 | | +| `INPUT_STABILIZED_PITCH_MINUS` | 26 | | +| `INPUT_STABILIZED_YAW_PLUS` | 27 | | +| `INPUT_STABILIZED_YAW_MINUS` | 28 | | +| `INPUT_MAX` | 29 | | +| `INPUT_GVAR_0` | 30 | | +| `INPUT_GVAR_1` | 31 | | +| `INPUT_GVAR_2` | 32 | | +| `INPUT_GVAR_3` | 33 | | +| `INPUT_GVAR_4` | 34 | | +| `INPUT_GVAR_5` | 35 | | +| `INPUT_GVAR_6` | 36 | | +| `INPUT_GVAR_7` | 37 | | +| `INPUT_MIXER_TRANSITION` | 38 | | +| `INPUT_HEADTRACKER_PAN` | 39 | | +| `INPUT_HEADTRACKER_TILT` | 40 | | +| `INPUT_HEADTRACKER_ROLL` | 41 | | +| `INPUT_RC_CH17` | 42 | | +| `INPUT_RC_CH18` | 43 | | +| `INPUT_RC_CH19` | 44 | | +| `INPUT_RC_CH20` | 45 | | +| `INPUT_RC_CH21` | 46 | | +| `INPUT_RC_CH22` | 47 | | +| `INPUT_RC_CH23` | 48 | | +| `INPUT_RC_CH24` | 49 | | +| `INPUT_RC_CH25` | 50 | | +| `INPUT_RC_CH26` | 51 | | +| `INPUT_RC_CH27` | 52 | | +| `INPUT_RC_CH28` | 53 | | +| `INPUT_RC_CH29` | 54 | | +| `INPUT_RC_CH30` | 55 | | +| `INPUT_RC_CH31` | 56 | | +| `INPUT_RC_CH32` | 57 | | +| `INPUT_RC_CH33` | 58 | | +| `INPUT_RC_CH34` | 59 | | +| `INPUT_MIXER_SWITCH_HELPER` | 60 | | +| `INPUT_SOURCE_COUNT` | 61 | | + +--- +## `itermRelax_e` + +> Source: ../../../src/main/flight/pid.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `ITERM_RELAX_OFF` | 0 | | +| `ITERM_RELAX_RP` | 1 | | +| `ITERM_RELAX_RPY` | 2 | | + +--- +## `led_pin_pwm_mode_e` + +> Source: ../../../src/main/drivers/light_ws2811strip.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LED_PIN_PWM_MODE_SHARED_LOW` | 0 | | +| `LED_PIN_PWM_MODE_SHARED_HIGH` | 1 | | +| `LED_PIN_PWM_MODE_LOW` | 2 | | +| `LED_PIN_PWM_MODE_HIGH` | 3 | | + +--- +## `ledBaseFunctionId_e` + +> Source: ../../../src/main/io/ledstrip.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LED_FUNCTION_COLOR` | 0 | | +| `LED_FUNCTION_FLIGHT_MODE` | 1 | | +| `LED_FUNCTION_ARM_STATE` | 2 | | +| `LED_FUNCTION_BATTERY` | 3 | | +| `LED_FUNCTION_RSSI` | 4 | | +| `LED_FUNCTION_GPS` | 5 | | +| `LED_FUNCTION_THRUST_RING` | 6 | | +| `LED_FUNCTION_CHANNEL` | 7 | | + +--- +## `ledDirectionId_e` + +> Source: ../../../src/main/io/ledstrip.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LED_DIRECTION_NORTH` | 0 | | +| `LED_DIRECTION_EAST` | 1 | | +| `LED_DIRECTION_SOUTH` | 2 | | +| `LED_DIRECTION_WEST` | 3 | | +| `LED_DIRECTION_UP` | 4 | | +| `LED_DIRECTION_DOWN` | 5 | | + +--- +## `ledModeIndex_e` + +> Source: ../../../src/main/io/ledstrip.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LED_MODE_ORIENTATION` | 0 | | +| `LED_MODE_HEADFREE` | 1 | | +| `LED_MODE_HORIZON` | 2 | | +| `LED_MODE_ANGLE` | 3 | | +| `LED_MODE_MAG` | 4 | | +| `LED_MODE_BARO` | 5 | | +| `LED_SPECIAL` | 6 | | + +--- +## `ledOverlayId_e` + +> Source: ../../../src/main/io/ledstrip.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LED_OVERLAY_THROTTLE` | 0 | | +| `LED_OVERLAY_LARSON_SCANNER` | 1 | | +| `LED_OVERLAY_BLINK` | 2 | | +| `LED_OVERLAY_LANDING_FLASH` | 3 | | +| `LED_OVERLAY_INDICATOR` | 4 | | +| `LED_OVERLAY_WARNING` | 5 | | +| `LED_OVERLAY_STROBE` | 6 | | + +--- +## `ledSpecialColorIds_e` + +> Source: ../../../src/main/io/ledstrip.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LED_SCOLOR_DISARMED` | 0 | | +| `LED_SCOLOR_ARMED` | 1 | | +| `LED_SCOLOR_ANIMATION` | 2 | | +| `LED_SCOLOR_BACKGROUND` | 3 | | +| `LED_SCOLOR_BLINKBACKGROUND` | 4 | | +| `LED_SCOLOR_GPSNOSATS` | 5 | | +| `LED_SCOLOR_GPSNOLOCK` | 6 | | +| `LED_SCOLOR_GPSLOCKED` | 7 | | +| `LED_SCOLOR_STROBE` | 8 | | + +--- +## `logicConditionFlags_e` + +> Source: ../../../src/main/programming/logic_condition.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LOGIC_CONDITION_FLAG_LATCH` | 1 << 0 | | +| `LOGIC_CONDITION_FLAG_TIMEOUT_SATISFIED` | 1 << 1 | | + +--- +## `logicConditionsGlobalFlags_t` + +> Source: ../../../src/main/programming/logic_condition.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY` | (1 << 0) | | +| `LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE_SCALE` | (1 << 1) | | +| `LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_SWAP_ROLL_YAW` | (1 << 2) | | +| `LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_ROLL` | (1 << 3) | | +| `LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_PITCH` | (1 << 4) | | +| `LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_INVERT_YAW` | (1 << 5) | | +| `LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE` | (1 << 6) | | +| `LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_OSD_LAYOUT` | (1 << 7) | | +| `LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_RC_CHANNEL` | (1 << 8) | | +| `LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS` | (1 << 9) | | +| `LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_FLIGHT_AXIS` | (1 << 10) | | +| `LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX` | (1 << 11) | USE_GPS_FIX_ESTIMATION | +| `LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_MIN_GROUND_SPEED` | (1 << 12) | | + +--- +## `logicFlightModeOperands_e` + +> Source: ../../../src/main/programming/logic_condition.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LOGIC_CONDITION_OPERAND_FLIGHT_MODE_FAILSAFE` | 0 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MODE_MANUAL` | 1 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MODE_RTH` | 2 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MODE_POSHOLD` | 3 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE` | 4 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD` | 5 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLE` | 6 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MODE_HORIZON` | 7 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR` | 8 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1` | 9 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER2` | 10 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MODE_COURSE_HOLD` | 11 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER3` | 12 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER4` | 13 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ACRO` | 14 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MODE_WAYPOINT_MISSION` | 15 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ANGLEHOLD` | 16 | | + +--- +## `logicFlightOperands_e` + +> Source: ../../../src/main/programming/logic_condition.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER` | 0 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE` | 1 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE` | 2 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_RSSI` | 3 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_VBAT` | 4 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE` | 5 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT` | 6 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN` | 7 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS` | 8 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED` | 9 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED` | 10 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED` | 11 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE` | 12 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED` | 13 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS` | 14 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_ROLL` | 15 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_PITCH` | 16 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_IS_ARMED` | 17 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_IS_AUTOLAUNCH` | 18 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_IS_ALTITUDE_CONTROL` | 19 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_IS_POSITION_CONTROL` | 20 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_IS_EMERGENCY_LANDING` | 21 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_IS_RTH` | 22 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING` | 23 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE` | 24 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_ROLL` | 25 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_PITCH` | 26 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_STABILIZED_YAW` | 27 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE` | 28 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_LQ_UPLINK` | 29 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_SNR` | 30 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID` | 31 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS` | 32 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE` | 33 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_BATT_CELLS` | 34 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS` | 35 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_AGL` | 36 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW` | 37 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE` | 38 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE` | 39 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW` | 40 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE` | 41 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE` | 42 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS` | 43 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_LQ_DOWNLINK` | 44 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_UPLINK_RSSI_DBM` | 45 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_MIN_GROUND_SPEED` | 46 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_HORIZONTAL_WIND_SPEED` | 47 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_WIND_DIRECTION` | 48 | | +| `LOGIC_CONDITION_OPERAND_FLIGHT_RELATIVE_WIND_OFFSET` | 49 | | + +--- +## `logicOperation_e` + +> Source: ../../../src/main/programming/logic_condition.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LOGIC_CONDITION_TRUE` | 0 | | +| `LOGIC_CONDITION_EQUAL` | 1 | | +| `LOGIC_CONDITION_GREATER_THAN` | 2 | | +| `LOGIC_CONDITION_LOWER_THAN` | 3 | | +| `LOGIC_CONDITION_LOW` | 4 | | +| `LOGIC_CONDITION_MID` | 5 | | +| `LOGIC_CONDITION_HIGH` | 6 | | +| `LOGIC_CONDITION_AND` | 7 | | +| `LOGIC_CONDITION_OR` | 8 | | +| `LOGIC_CONDITION_XOR` | 9 | | +| `LOGIC_CONDITION_NAND` | 10 | | +| `LOGIC_CONDITION_NOR` | 11 | | +| `LOGIC_CONDITION_NOT` | 12 | | +| `LOGIC_CONDITION_STICKY` | 13 | | +| `LOGIC_CONDITION_ADD` | 14 | | +| `LOGIC_CONDITION_SUB` | 15 | | +| `LOGIC_CONDITION_MUL` | 16 | | +| `LOGIC_CONDITION_DIV` | 17 | | +| `LOGIC_CONDITION_GVAR_SET` | 18 | | +| `LOGIC_CONDITION_GVAR_INC` | 19 | | +| `LOGIC_CONDITION_GVAR_DEC` | 20 | | +| `LOGIC_CONDITION_PORT_SET` | 21 | | +| `LOGIC_CONDITION_OVERRIDE_ARMING_SAFETY` | 22 | | +| `LOGIC_CONDITION_OVERRIDE_THROTTLE_SCALE` | 23 | | +| `LOGIC_CONDITION_SWAP_ROLL_YAW` | 24 | | +| `LOGIC_CONDITION_SET_VTX_POWER_LEVEL` | 25 | | +| `LOGIC_CONDITION_INVERT_ROLL` | 26 | | +| `LOGIC_CONDITION_INVERT_PITCH` | 27 | | +| `LOGIC_CONDITION_INVERT_YAW` | 28 | | +| `LOGIC_CONDITION_OVERRIDE_THROTTLE` | 29 | | +| `LOGIC_CONDITION_SET_VTX_BAND` | 30 | | +| `LOGIC_CONDITION_SET_VTX_CHANNEL` | 31 | | +| `LOGIC_CONDITION_SET_OSD_LAYOUT` | 32 | | +| `LOGIC_CONDITION_SIN` | 33 | | +| `LOGIC_CONDITION_COS` | 34 | | +| `LOGIC_CONDITION_TAN` | 35 | | +| `LOGIC_CONDITION_MAP_INPUT` | 36 | | +| `LOGIC_CONDITION_MAP_OUTPUT` | 37 | | +| `LOGIC_CONDITION_RC_CHANNEL_OVERRIDE` | 38 | | +| `LOGIC_CONDITION_SET_HEADING_TARGET` | 39 | | +| `LOGIC_CONDITION_MODULUS` | 40 | | +| `LOGIC_CONDITION_LOITER_OVERRIDE` | 41 | | +| `LOGIC_CONDITION_SET_PROFILE` | 42 | | +| `LOGIC_CONDITION_MIN` | 43 | | +| `LOGIC_CONDITION_MAX` | 44 | | +| `LOGIC_CONDITION_FLIGHT_AXIS_ANGLE_OVERRIDE` | 45 | | +| `LOGIC_CONDITION_FLIGHT_AXIS_RATE_OVERRIDE` | 46 | | +| `LOGIC_CONDITION_EDGE` | 47 | | +| `LOGIC_CONDITION_DELAY` | 48 | | +| `LOGIC_CONDITION_TIMER` | 49 | | +| `LOGIC_CONDITION_DELTA` | 50 | | +| `LOGIC_CONDITION_APPROX_EQUAL` | 51 | | +| `LOGIC_CONDITION_LED_PIN_PWM` | 52 | | +| `LOGIC_CONDITION_DISABLE_GPS_FIX` | 53 | | +| `LOGIC_CONDITION_RESET_MAG_CALIBRATION` | 54 | | +| `LOGIC_CONDITION_SET_GIMBAL_SENSITIVITY` | 55 | | +| `LOGIC_CONDITION_OVERRIDE_MIN_GROUND_SPEED` | 56 | | +| `LOGIC_CONDITION_LAST` | 57 | | + +--- +## `logicWaypointOperands_e` + +> Source: ../../../src/main/programming/logic_condition.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LOGIC_CONDITION_OPERAND_WAYPOINTS_IS_WP` | 0 | | +| `LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_INDEX` | 1 | | +| `LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_ACTION` | 2 | | +| `LOGIC_CONDITION_OPERAND_WAYPOINTS_NEXT_WAYPOINT_ACTION` | 3 | | +| `LOGIC_CONDITION_OPERAND_WAYPOINTS_WAYPOINT_DISTANCE` | 4 | | +| `LOGIC_CONDTIION_OPERAND_WAYPOINTS_DISTANCE_FROM_WAYPOINT` | 5 | | +| `LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION` | 6 | | +| `LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION` | 7 | | +| `LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION` | 8 | | +| `LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION` | 9 | | +| `LOGIC_CONDITION_OPERAND_WAYPOINTS_USER1_ACTION_NEXT_WP` | 10 | | +| `LOGIC_CONDITION_OPERAND_WAYPOINTS_USER2_ACTION_NEXT_WP` | 11 | | +| `LOGIC_CONDITION_OPERAND_WAYPOINTS_USER3_ACTION_NEXT_WP` | 12 | | +| `LOGIC_CONDITION_OPERAND_WAYPOINTS_USER4_ACTION_NEXT_WP` | 13 | | + +--- +## `logTopic_e` + +> Source: ../../../src/main/common/log.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LOG_TOPIC_SYSTEM` | 0 | | +| `LOG_TOPIC_GYRO` | 1 | | +| `LOG_TOPIC_BARO` | 2 | | +| `LOG_TOPIC_PITOT` | 3 | | +| `LOG_TOPIC_PWM` | 4 | | +| `LOG_TOPIC_TIMER` | 5 | | +| `LOG_TOPIC_IMU` | 6 | | +| `LOG_TOPIC_TEMPERATURE` | 7 | | +| `LOG_TOPIC_POS_ESTIMATOR` | 8 | | +| `LOG_TOPIC_VTX` | 9 | | +| `LOG_TOPIC_OSD` | 10 | | +| `LOG_TOPIC_COUNT` | 11 | | + +--- +## `lsm6dxxConfigMasks_e` + +> Source: ../../../src/main/drivers/accgyro/accgyro_lsm6dxx.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LSM6DXX_MASK_COUNTER_BDR1` | 128 | | +| `LSM6DXX_MASK_CTRL3_C` | 60 | | +| `LSM6DXX_MASK_CTRL3_C_RESET` | BIT(0) | | +| `LSM6DXX_MASK_CTRL4_C` | 14 | | +| `LSM6DXX_MASK_CTRL6_C` | 23 | | +| `LSM6DXX_MASK_CTRL7_G` | 112 | | +| `LSM6DXX_MASK_CTRL9_XL` | 2 | | +| `LSM6DSL_MASK_CTRL6_C` | 19 | | + +--- +## `lsm6dxxConfigValues_e` + +> Source: ../../../src/main/drivers/accgyro/accgyro_lsm6dxx.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LSM6DXX_VAL_COUNTER_BDR1_DDRY_PM` | BIT(7) | | +| `LSM6DXX_VAL_INT1_CTRL` | 2 | | +| `LSM6DXX_VAL_INT2_CTRL` | 0 | | +| `LSM6DXX_VAL_CTRL1_XL_ODR833` | 7 | | +| `LSM6DXX_VAL_CTRL1_XL_ODR1667` | 8 | | +| `LSM6DXX_VAL_CTRL1_XL_ODR3332` | 9 | | +| `LSM6DXX_VAL_CTRL1_XL_ODR3333` | 10 | | +| `LSM6DXX_VAL_CTRL1_XL_8G` | 3 | | +| `LSM6DXX_VAL_CTRL1_XL_16G` | 1 | | +| `LSM6DXX_VAL_CTRL1_XL_LPF1` | 0 | | +| `LSM6DXX_VAL_CTRL1_XL_LPF2` | 1 | | +| `LSM6DXX_VAL_CTRL2_G_ODR6664` | 10 | | +| `LSM6DXX_VAL_CTRL2_G_2000DPS` | 3 | | +| `LSM6DXX_VAL_CTRL3_C_H_LACTIVE` | 0 | | +| `LSM6DXX_VAL_CTRL3_C_PP_OD` | 0 | | +| `LSM6DXX_VAL_CTRL3_C_SIM` | 0 | | +| `LSM6DXX_VAL_CTRL3_C_IF_INC` | BIT(2) | | +| `LSM6DXX_VAL_CTRL4_C_DRDY_MASK` | BIT(3) | | +| `LSM6DXX_VAL_CTRL4_C_I2C_DISABLE` | BIT(2) | | +| `LSM6DXX_VAL_CTRL4_C_LPF1_SEL_G` | BIT(1) | | +| `LSM6DXX_VAL_CTRL6_C_XL_HM_MODE` | 0 | | +| `LSM6DXX_VAL_CTRL6_C_FTYPE_300HZ` | 0 | | +| `LSM6DXX_VAL_CTRL6_C_FTYPE_201HZ` | 1 | | +| `LSM6DXX_VAL_CTRL6_C_FTYPE_102HZ` | 2 | | +| `LSM6DXX_VAL_CTRL6_C_FTYPE_603HZ` | 3 | | +| `LSM6DXX_VAL_CTRL7_G_HP_EN_G` | BIT(6) | | +| `LSM6DXX_VAL_CTRL7_G_HPM_G_16` | 0 | | +| `LSM6DXX_VAL_CTRL7_G_HPM_G_65` | 1 | | +| `LSM6DXX_VAL_CTRL7_G_HPM_G_260` | 2 | | +| `LSM6DXX_VAL_CTRL7_G_HPM_G_1040` | 3 | | +| `LSM6DXX_VAL_CTRL9_XL_I3C_DISABLE` | BIT(1) | | + +--- +## `lsm6dxxRegister_e` + +> Source: ../../../src/main/drivers/accgyro/accgyro_lsm6dxx.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LSM6DXX_REG_COUNTER_BDR1` | 11 | | +| `LSM6DXX_REG_INT1_CTRL` | 13 | | +| `LSM6DXX_REG_INT2_CTRL` | 14 | | +| `LSM6DXX_REG_WHO_AM_I` | 15 | | +| `LSM6DXX_REG_CTRL1_XL` | 16 | | +| `LSM6DXX_REG_CTRL2_G` | 17 | | +| `LSM6DXX_REG_CTRL3_C` | 18 | | +| `LSM6DXX_REG_CTRL4_C` | 19 | | +| `LSM6DXX_REG_CTRL5_C` | 20 | | +| `LSM6DXX_REG_CTRL6_C` | 21 | | +| `LSM6DXX_REG_CTRL7_G` | 22 | | +| `LSM6DXX_REG_CTRL8_XL` | 23 | | +| `LSM6DXX_REG_CTRL9_XL` | 24 | | +| `LSM6DXX_REG_CTRL10_C` | 25 | | +| `LSM6DXX_REG_STATUS` | 30 | | +| `LSM6DXX_REG_OUT_TEMP_L` | 32 | | +| `LSM6DXX_REG_OUT_TEMP_H` | 33 | | +| `LSM6DXX_REG_OUTX_L_G` | 34 | | +| `LSM6DXX_REG_OUTX_H_G` | 35 | | +| `LSM6DXX_REG_OUTY_L_G` | 36 | | +| `LSM6DXX_REG_OUTY_H_G` | 37 | | +| `LSM6DXX_REG_OUTZ_L_G` | 38 | | +| `LSM6DXX_REG_OUTZ_H_G` | 39 | | +| `LSM6DXX_REG_OUTX_L_A` | 40 | | +| `LSM6DXX_REG_OUTX_H_A` | 41 | | +| `LSM6DXX_REG_OUTY_L_A` | 42 | | +| `LSM6DXX_REG_OUTY_H_A` | 43 | | +| `LSM6DXX_REG_OUTZ_L_A` | 44 | | +| `LSM6DXX_REG_OUTZ_H_A` | 45 | | + +--- +## `ltm_frame_e` + +> Source: ../../../src/main/telemetry/ltm.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LTM_FRAME_START` | 0 | | +| `LTM_AFRAME` | LTM_FRAME_START | | +| `LTM_SFRAME` | | | +| `LTM_GFRAME` | | USE_GPS | +| `LTM_OFRAME` | | USE_GPS | +| `LTM_XFRAME` | | USE_GPS | +| `LTM_NFRAME` | | | +| `LTM_FRAME_COUNT` | | | + +--- +## `ltm_modes_e` + +> Source: ../../../src/main/telemetry/ltm.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LTM_MODE_MANUAL` | 0 | | +| `LTM_MODE_RATE` | 1 | | +| `LTM_MODE_ANGLE` | 2 | | +| `LTM_MODE_HORIZON` | 3 | | +| `LTM_MODE_ACRO` | 4 | | +| `LTM_MODE_STABALIZED1` | 5 | | +| `LTM_MODE_STABALIZED2` | 6 | | +| `LTM_MODE_STABILIZED3` | 7 | | +| `LTM_MODE_ALTHOLD` | 8 | | +| `LTM_MODE_GPSHOLD` | 9 | | +| `LTM_MODE_WAYPOINTS` | 10 | | +| `LTM_MODE_HEADHOLD` | 11 | | +| `LTM_MODE_CIRCLE` | 12 | | +| `LTM_MODE_RTH` | 13 | | +| `LTM_MODE_FOLLOWWME` | 14 | | +| `LTM_MODE_LAND` | 15 | | +| `LTM_MODE_FLYBYWIRE1` | 16 | | +| `LTM_MODE_FLYBYWIRE2` | 17 | | +| `LTM_MODE_CRUISE` | 18 | | +| `LTM_MODE_UNKNOWN` | 19 | | +| `LTM_MODE_LAUNCH` | 20 | | +| `LTM_MODE_AUTOTUNE` | 21 | | + +--- +## `ltmUpdateRate_e` + +> Source: ../../../src/main/telemetry/telemetry.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `LTM_RATE_NORMAL` | 0 | | +| `LTM_RATE_MEDIUM` | 1 | | +| `LTM_RATE_SLOW` | 2 | | + +--- +## `magSensor_e` + +> Source: ../../../src/main/sensors/compass.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MAG_NONE` | 0 | | +| `MAG_AUTODETECT` | 1 | | +| `MAG_HMC5883` | 2 | | +| `MAG_AK8975` | 3 | | +| `MAG_MAG3110` | 4 | | +| `MAG_AK8963` | 5 | | +| `MAG_IST8310` | 6 | | +| `MAG_QMC5883` | 7 | | +| `MAG_QMC5883P` | 8 | | +| `MAG_MPU9250` | 9 | | +| `MAG_IST8308` | 10 | | +| `MAG_LIS3MDL` | 11 | | +| `MAG_MSP` | 12 | | +| `MAG_RM3100` | 13 | | +| `MAG_VCM5883` | 14 | | +| `MAG_MLX90393` | 15 | | +| `MAG_FAKE` | 16 | | +| `MAG_MAX` | MAG_FAKE | | + +--- +## `mavlinkAutopilotType_e` + +> Source: ../../../src/main/telemetry/telemetry.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MAVLINK_AUTOPILOT_GENERIC` | 0 | | +| `MAVLINK_AUTOPILOT_ARDUPILOT` | 1 | | + +--- +## `mavlinkRadio_e` + +> Source: ../../../src/main/telemetry/telemetry.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MAVLINK_RADIO_GENERIC` | 0 | | +| `MAVLINK_RADIO_ELRS` | 1 | | +| `MAVLINK_RADIO_SIK` | 2 | | + +--- +## `measurementSteps_e` + +> Source: ../../../src/main/drivers/rangefinder/rangefinder_vl53l0x.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `MEASUREMENT_START` | 0 | | +| `MEASUREMENT_WAIT` | 1 | | +| `MEASUREMENT_READ` | 2 | | + +--- +## `mixerProfileATRequest_e` + +> Source: ../../../src/main/flight/mixer_profile.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MIXERAT_REQUEST_NONE` | 0 | | +| `MIXERAT_REQUEST_RTH` | 1 | | +| `MIXERAT_REQUEST_LAND` | 2 | | +| `MIXERAT_REQUEST_ABORT` | 3 | | + +--- +## `mixerProfileATState_e` + +> Source: ../../../src/main/flight/mixer_profile.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MIXERAT_PHASE_IDLE` | 0 | | +| `MIXERAT_PHASE_TRANSITION_INITIALIZE` | 1 | | +| `MIXERAT_PHASE_TRANSITIONING` | 2 | | +| `MIXERAT_PHASE_DONE` | 3 | | + +--- +## `modeActivationOperator_e` + +> Source: ../../../src/main/fc/rc_modes.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MODE_OPERATOR_OR` | 0 | | +| `MODE_OPERATOR_AND` | 1 | | + +--- +## `motorPwmProtocolTypes_e` + +> Source: ../../../src/main/drivers/pwm_mapping.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `PWM_TYPE_STANDARD` | 0 | | +| `PWM_TYPE_ONESHOT125` | 1 | | +| `PWM_TYPE_MULTISHOT` | 2 | | +| `PWM_TYPE_BRUSHED` | 3 | | +| `PWM_TYPE_DSHOT150` | 4 | | +| `PWM_TYPE_DSHOT300` | 5 | | +| `PWM_TYPE_DSHOT600` | 6 | | + +--- +## `motorStatus_e` + +> Source: ../../../src/main/flight/mixer.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MOTOR_STOPPED_USER` | 0 | | +| `MOTOR_STOPPED_AUTO` | 1 | | +| `MOTOR_RUNNING` | 2 | | + +--- +## `mpu9250CompassReadState_e` + +> Source: ../../../src/main/drivers/compass/compass_mpu9250.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `CHECK_STATUS` | 0 | | +| `WAITING_FOR_STATUS` | 1 | | +| `WAITING_FOR_DATA` | 2 | | + +--- +## `mspFlashfsFlags_e` + +> Source: ../../../src/main/fc/fc_msp.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `MSP_FLASHFS_BIT_READY` | 1 | | +| `MSP_FLASHFS_BIT_SUPPORTED` | 2 | | + +--- +## `mspPassthroughType_e` + +> Source: ../../../src/main/fc/fc_msp.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `MSP_PASSTHROUGH_SERIAL_ID` | 253 | | +| `MSP_PASSTHROUGH_SERIAL_FUNCTION_ID` | 254 | | +| `MSP_PASSTHROUGH_ESC_4WAY` | 255 | | + +--- +## `mspSDCardFlags_e` + +> Source: ../../../src/main/fc/fc_msp.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `MSP_SDCARD_FLAG_SUPPORTTED` | 1 | | + +--- +## `mspSDCardState_e` + +> Source: ../../../src/main/fc/fc_msp.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `MSP_SDCARD_STATE_NOT_PRESENT` | 0 | | +| `MSP_SDCARD_STATE_FATAL` | 1 | | +| `MSP_SDCARD_STATE_CARD_INIT` | 2 | | +| `MSP_SDCARD_STATE_FS_INIT` | 3 | | +| `MSP_SDCARD_STATE_READY` | 4 | | + +--- +## `multi_function_e` + +> Source: ../../../src/main/fc/multifunction.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MULTI_FUNC_NONE` | 0 | | +| `MULTI_FUNC_1` | 1 | | +| `MULTI_FUNC_2` | 2 | | +| `MULTI_FUNC_3` | 3 | | +| `MULTI_FUNC_4` | 4 | | +| `MULTI_FUNC_5` | 5 | | +| `MULTI_FUNC_6` | 6 | | +| `MULTI_FUNC_END` | 7 | | + +--- +## `multiFunctionFlags_e` + +> Source: ../../../src/main/fc/multifunction.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MF_SUSPEND_SAFEHOMES` | (1 << 0) | | +| `MF_SUSPEND_TRACKBACK` | (1 << 1) | | +| `MF_TURTLE_MODE` | (1 << 2) | | + +--- +## `nav_reset_type_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_RESET_NEVER` | 0 | | +| `NAV_RESET_ON_FIRST_ARM` | 1 | | +| `NAV_RESET_ON_EACH_ARM` | 2 | | + +--- +## `navAGLEstimateQuality_e` + +> Source: ../../../src/main/navigation/navigation_pos_estimator_private.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SURFACE_QUAL_LOW` | 0 | | +| `SURFACE_QUAL_MID` | 1 | | +| `SURFACE_QUAL_HIGH` | 2 | | + +--- +## `navArmingBlocker_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_ARMING_BLOCKER_NONE` | 0 | | +| `NAV_ARMING_BLOCKER_MISSING_GPS_FIX` | 1 | | +| `NAV_ARMING_BLOCKER_NAV_IS_ALREADY_ACTIVE` | 2 | | +| `NAV_ARMING_BLOCKER_FIRST_WAYPOINT_TOO_FAR` | 3 | | +| `NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR` | 4 | | + +--- +## `navDefaultAltitudeSensor_e` + +> Source: ../../../src/main/navigation/navigation_pos_estimator_private.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `ALTITUDE_SOURCE_GPS` | 0 | | +| `ALTITUDE_SOURCE_BARO` | 1 | | +| `ALTITUDE_SOURCE_GPS_ONLY` | 2 | | +| `ALTITUDE_SOURCE_BARO_ONLY` | 3 | | + +--- +## `navExtraArmingSafety_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_EXTRA_ARMING_SAFETY_ON` | 0 | | +| `NAV_EXTRA_ARMING_SAFETY_ALLOW_BYPASS` | 1 | | + +--- +## `navFwLaunchStatus_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FW_LAUNCH_DETECTED` | 5 | | +| `FW_LAUNCH_ABORTED` | 10 | | +| `FW_LAUNCH_FLYING` | 11 | | + +--- +## `navigationEstimateStatus_e` + +> Source: ../../../src/main/navigation/navigation_private.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `EST_NONE` | 0 | | +| `EST_USABLE` | 1 | | +| `EST_TRUSTED` | 2 | | + +--- +## `navigationFSMEvent_t` + +> Source: ../../../src/main/navigation/navigation_private.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_FSM_EVENT_NONE` | 0 | | +| `NAV_FSM_EVENT_TIMEOUT` | 1 | | +| `NAV_FSM_EVENT_SUCCESS` | 2 | | +| `NAV_FSM_EVENT_ERROR` | 3 | | +| `NAV_FSM_EVENT_SWITCH_TO_IDLE` | 4 | | +| `NAV_FSM_EVENT_SWITCH_TO_ALTHOLD` | 5 | | +| `NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D` | 6 | | +| `NAV_FSM_EVENT_SWITCH_TO_RTH` | 7 | | +| `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT` | 8 | | +| `NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING` | 9 | | +| `NAV_FSM_EVENT_SWITCH_TO_LAUNCH` | 10 | | +| `NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD` | 11 | | +| `NAV_FSM_EVENT_SWITCH_TO_CRUISE` | 12 | | +| `NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ` | 13 | | +| `NAV_FSM_EVENT_SWITCH_TO_MIXERAT` | 14 | | +| `NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING` | 15 | | +| `NAV_FSM_EVENT_SWITCH_TO_SEND_TO` | 16 | | +| `NAV_FSM_EVENT_STATE_SPECIFIC_1` | 17 | | +| `NAV_FSM_EVENT_STATE_SPECIFIC_2` | 18 | | +| `NAV_FSM_EVENT_STATE_SPECIFIC_3` | 19 | | +| `NAV_FSM_EVENT_STATE_SPECIFIC_4` | 20 | | +| `NAV_FSM_EVENT_STATE_SPECIFIC_5` | 21 | | +| `NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT` | NAV_FSM_EVENT_STATE_SPECIFIC_1 | | +| `NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_FINISHED` | NAV_FSM_EVENT_STATE_SPECIFIC_2 | | +| `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME` | NAV_FSM_EVENT_STATE_SPECIFIC_1 | | +| `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND` | NAV_FSM_EVENT_STATE_SPECIFIC_2 | | +| `NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED` | NAV_FSM_EVENT_STATE_SPECIFIC_3 | | +| `NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE` | NAV_FSM_EVENT_STATE_SPECIFIC_1 | | +| `NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK` | NAV_FSM_EVENT_STATE_SPECIFIC_2 | | +| `NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME` | NAV_FSM_EVENT_STATE_SPECIFIC_3 | | +| `NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME` | NAV_FSM_EVENT_STATE_SPECIFIC_4 | | +| `NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING` | NAV_FSM_EVENT_STATE_SPECIFIC_5 | | +| `NAV_FSM_EVENT_COUNT` | | | + +--- +## `navigationFSMState_t` + +> Source: ../../../src/main/navigation/navigation_private.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_STATE_UNDEFINED` | 0 | | +| `NAV_STATE_IDLE` | 1 | | +| `NAV_STATE_ALTHOLD_INITIALIZE` | 2 | | +| `NAV_STATE_ALTHOLD_IN_PROGRESS` | 3 | | +| `NAV_STATE_POSHOLD_3D_INITIALIZE` | 4 | | +| `NAV_STATE_POSHOLD_3D_IN_PROGRESS` | 5 | | +| `NAV_STATE_RTH_INITIALIZE` | 6 | | +| `NAV_STATE_RTH_CLIMB_TO_SAFE_ALT` | 7 | | +| `NAV_STATE_RTH_TRACKBACK` | 8 | | +| `NAV_STATE_RTH_HEAD_HOME` | 9 | | +| `NAV_STATE_RTH_LOITER_PRIOR_TO_LANDING` | 10 | | +| `NAV_STATE_RTH_LOITER_ABOVE_HOME` | 11 | | +| `NAV_STATE_RTH_LANDING` | 12 | | +| `NAV_STATE_RTH_FINISHING` | 13 | | +| `NAV_STATE_RTH_FINISHED` | 14 | | +| `NAV_STATE_WAYPOINT_INITIALIZE` | 15 | | +| `NAV_STATE_WAYPOINT_PRE_ACTION` | 16 | | +| `NAV_STATE_WAYPOINT_IN_PROGRESS` | 17 | | +| `NAV_STATE_WAYPOINT_REACHED` | 18 | | +| `NAV_STATE_WAYPOINT_HOLD_TIME` | 19 | | +| `NAV_STATE_WAYPOINT_NEXT` | 20 | | +| `NAV_STATE_WAYPOINT_FINISHED` | 21 | | +| `NAV_STATE_WAYPOINT_RTH_LAND` | 22 | | +| `NAV_STATE_EMERGENCY_LANDING_INITIALIZE` | 23 | | +| `NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS` | 24 | | +| `NAV_STATE_EMERGENCY_LANDING_FINISHED` | 25 | | +| `NAV_STATE_LAUNCH_INITIALIZE` | 26 | | +| `NAV_STATE_LAUNCH_WAIT` | 27 | | +| `NAV_STATE_LAUNCH_IN_PROGRESS` | 28 | | +| `NAV_STATE_COURSE_HOLD_INITIALIZE` | 29 | | +| `NAV_STATE_COURSE_HOLD_IN_PROGRESS` | 30 | | +| `NAV_STATE_COURSE_HOLD_ADJUSTING` | 31 | | +| `NAV_STATE_CRUISE_INITIALIZE` | 32 | | +| `NAV_STATE_CRUISE_IN_PROGRESS` | 33 | | +| `NAV_STATE_CRUISE_ADJUSTING` | 34 | | +| `NAV_STATE_FW_LANDING_CLIMB_TO_LOITER` | 35 | | +| `NAV_STATE_FW_LANDING_LOITER` | 36 | | +| `NAV_STATE_FW_LANDING_APPROACH` | 37 | | +| `NAV_STATE_FW_LANDING_GLIDE` | 38 | | +| `NAV_STATE_FW_LANDING_FLARE` | 39 | | +| `NAV_STATE_FW_LANDING_FINISHED` | 40 | | +| `NAV_STATE_FW_LANDING_ABORT` | 41 | | +| `NAV_STATE_MIXERAT_INITIALIZE` | 42 | | +| `NAV_STATE_MIXERAT_IN_PROGRESS` | 43 | | +| `NAV_STATE_MIXERAT_ABORT` | 44 | | +| `NAV_STATE_SEND_TO_INITALIZE` | 45 | | +| `NAV_STATE_SEND_TO_IN_PROGESS` | 46 | | +| `NAV_STATE_SEND_TO_FINISHED` | 47 | | +| `NAV_STATE_COUNT` | 48 | | + +--- +## `navigationFSMStateFlags_t` + +> Source: ../../../src/main/navigation/navigation_private.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_CTL_ALT` | (1 << 0) | | +| `NAV_CTL_POS` | (1 << 1) | | +| `NAV_CTL_YAW` | (1 << 2) | | +| `NAV_CTL_EMERG` | (1 << 3) | | +| `NAV_CTL_LAUNCH` | (1 << 4) | | +| `NAV_REQUIRE_ANGLE` | (1 << 5) | | +| `NAV_REQUIRE_ANGLE_FW` | (1 << 6) | | +| `NAV_REQUIRE_MAGHOLD` | (1 << 7) | | +| `NAV_REQUIRE_THRTILT` | (1 << 8) | | +| `NAV_AUTO_RTH` | (1 << 9) | | +| `NAV_AUTO_WP` | (1 << 10) | | +| `NAV_RC_ALT` | (1 << 11) | | +| `NAV_RC_POS` | (1 << 12) | | +| `NAV_RC_YAW` | (1 << 13) | | +| `NAV_CTL_LAND` | (1 << 14) | | +| `NAV_AUTO_WP_DONE` | (1 << 15) | | +| `NAV_MIXERAT` | (1 << 16) | | +| `NAV_CTL_HOLD` | (1 << 17) | | + +--- +## `navigationHomeFlags_t` + +> Source: ../../../src/main/navigation/navigation_private.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_HOME_INVALID` | 0 | | +| `NAV_HOME_VALID_XY` | 1 << 0 | | +| `NAV_HOME_VALID_Z` | 1 << 1 | | +| `NAV_HOME_VALID_HEADING` | 1 << 2 | | +| `NAV_HOME_VALID_ALL` | NAV_HOME_VALID_XY | NAV_HOME_VALID_Z | NAV_HOME_VALID_HEADING | | + +--- +## `navigationPersistentId_e` + +> Source: ../../../src/main/navigation/navigation_private.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_PERSISTENT_ID_UNDEFINED` | 0 | | +| `NAV_PERSISTENT_ID_IDLE` | 1 | | +| `NAV_PERSISTENT_ID_ALTHOLD_INITIALIZE` | 2 | | +| `NAV_PERSISTENT_ID_ALTHOLD_IN_PROGRESS` | 3 | | +| `NAV_PERSISTENT_ID_UNUSED_1` | 4 | | +| `NAV_PERSISTENT_ID_UNUSED_2` | 5 | | +| `NAV_PERSISTENT_ID_POSHOLD_3D_INITIALIZE` | 6 | | +| `NAV_PERSISTENT_ID_POSHOLD_3D_IN_PROGRESS` | 7 | | +| `NAV_PERSISTENT_ID_RTH_INITIALIZE` | 8 | | +| `NAV_PERSISTENT_ID_RTH_CLIMB_TO_SAFE_ALT` | 9 | | +| `NAV_PERSISTENT_ID_RTH_HEAD_HOME` | 10 | | +| `NAV_PERSISTENT_ID_RTH_LOITER_PRIOR_TO_LANDING` | 11 | | +| `NAV_PERSISTENT_ID_RTH_LANDING` | 12 | | +| `NAV_PERSISTENT_ID_RTH_FINISHING` | 13 | | +| `NAV_PERSISTENT_ID_RTH_FINISHED` | 14 | | +| `NAV_PERSISTENT_ID_WAYPOINT_INITIALIZE` | 15 | | +| `NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION` | 16 | | +| `NAV_PERSISTENT_ID_WAYPOINT_IN_PROGRESS` | 17 | | +| `NAV_PERSISTENT_ID_WAYPOINT_REACHED` | 18 | | +| `NAV_PERSISTENT_ID_WAYPOINT_NEXT` | 19 | | +| `NAV_PERSISTENT_ID_WAYPOINT_FINISHED` | 20 | | +| `NAV_PERSISTENT_ID_WAYPOINT_RTH_LAND` | 21 | | +| `NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE` | 22 | | +| `NAV_PERSISTENT_ID_EMERGENCY_LANDING_IN_PROGRESS` | 23 | | +| `NAV_PERSISTENT_ID_EMERGENCY_LANDING_FINISHED` | 24 | | +| `NAV_PERSISTENT_ID_LAUNCH_INITIALIZE` | 25 | | +| `NAV_PERSISTENT_ID_LAUNCH_WAIT` | 26 | | +| `NAV_PERSISTENT_ID_UNUSED_3` | 27 | | +| `NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS` | 28 | | +| `NAV_PERSISTENT_ID_COURSE_HOLD_INITIALIZE` | 29 | | +| `NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS` | 30 | | +| `NAV_PERSISTENT_ID_COURSE_HOLD_ADJUSTING` | 31 | | +| `NAV_PERSISTENT_ID_CRUISE_INITIALIZE` | 32 | | +| `NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS` | 33 | | +| `NAV_PERSISTENT_ID_CRUISE_ADJUSTING` | 34 | | +| `NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME` | 35 | | +| `NAV_PERSISTENT_ID_RTH_LOITER_ABOVE_HOME` | 36 | | +| `NAV_PERSISTENT_ID_UNUSED_4` | 37 | | +| `NAV_PERSISTENT_ID_RTH_TRACKBACK` | 38 | | +| `NAV_PERSISTENT_ID_MIXERAT_INITIALIZE` | 39 | | +| `NAV_PERSISTENT_ID_MIXERAT_IN_PROGRESS` | 40 | | +| `NAV_PERSISTENT_ID_MIXERAT_ABORT` | 41 | | +| `NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER` | 42 | | +| `NAV_PERSISTENT_ID_FW_LANDING_LOITER` | 43 | | +| `NAV_PERSISTENT_ID_FW_LANDING_APPROACH` | 44 | | +| `NAV_PERSISTENT_ID_FW_LANDING_GLIDE` | 45 | | +| `NAV_PERSISTENT_ID_FW_LANDING_FLARE` | 46 | | +| `NAV_PERSISTENT_ID_FW_LANDING_ABORT` | 47 | | +| `NAV_PERSISTENT_ID_FW_LANDING_FINISHED` | 48 | | +| `NAV_PERSISTENT_ID_SEND_TO_INITALIZE` | 49 | | +| `NAV_PERSISTENT_ID_SEND_TO_IN_PROGRES` | 50 | | +| `NAV_PERSISTENT_ID_SEND_TO_FINISHED` | 51 | | + +--- +## `navMcAltHoldThrottle_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MC_ALT_HOLD_STICK` | 0 | | +| `MC_ALT_HOLD_MID` | 1 | | +| `MC_ALT_HOLD_HOVER` | 2 | | + +--- +## `navMissionRestart_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `WP_MISSION_START` | 0 | | +| `WP_MISSION_RESUME` | 1 | | +| `WP_MISSION_SWITCH` | 2 | | + +--- +## `navOverridesMotorStop_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NOMS_OFF_ALWAYS` | 0 | | +| `NOMS_OFF` | 1 | | +| `NOMS_AUTO_ONLY` | 2 | | +| `NOMS_ALL_NAV` | 3 | | + +--- +## `navPositionEstimationFlags_e` + +> Source: ../../../src/main/navigation/navigation_pos_estimator_private.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `EST_GPS_XY_VALID` | (1 << 0) | | +| `EST_GPS_Z_VALID` | (1 << 1) | | +| `EST_BARO_VALID` | (1 << 2) | | +| `EST_SURFACE_VALID` | (1 << 3) | | +| `EST_FLOW_VALID` | (1 << 4) | | +| `EST_XY_VALID` | (1 << 5) | | +| `EST_Z_VALID` | (1 << 6) | | + +--- +## `navRTHAllowLanding_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_RTH_ALLOW_LANDING_NEVER` | 0 | | +| `NAV_RTH_ALLOW_LANDING_ALWAYS` | 1 | | +| `NAV_RTH_ALLOW_LANDING_FS_ONLY` | 2 | | + +--- +## `navRTHClimbFirst_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `RTH_CLIMB_OFF` | 0 | | +| `RTH_CLIMB_ON` | 1 | | +| `RTH_CLIMB_ON_FW_SPIRAL` | 2 | | + +--- +## `navSetWaypointFlags_t` + +> Source: ../../../src/main/navigation/navigation_private.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_POS_UPDATE_NONE` | 0 | | +| `NAV_POS_UPDATE_Z` | 1 << 1 | | +| `NAV_POS_UPDATE_XY` | 1 << 0 | | +| `NAV_POS_UPDATE_HEADING` | 1 << 2 | | +| `NAV_POS_UPDATE_BEARING` | 1 << 3 | | +| `NAV_POS_UPDATE_BEARING_TAIL_FIRST` | 1 << 4 | | + +--- +## `navSystemStatus_Error_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MW_NAV_ERROR_NONE` | 0 | | +| `MW_NAV_ERROR_TOOFAR` | 1 | | +| `MW_NAV_ERROR_SPOILED_GPS` | 2 | | +| `MW_NAV_ERROR_WP_CRC` | 3 | | +| `MW_NAV_ERROR_FINISH` | 4 | | +| `MW_NAV_ERROR_TIMEWAIT` | 5 | | +| `MW_NAV_ERROR_INVALID_JUMP` | 6 | | +| `MW_NAV_ERROR_INVALID_DATA` | 7 | | +| `MW_NAV_ERROR_WAIT_FOR_RTH_ALT` | 8 | | +| `MW_NAV_ERROR_GPS_FIX_LOST` | 9 | | +| `MW_NAV_ERROR_DISARMED` | 10 | | +| `MW_NAV_ERROR_LANDING` | 11 | | + +--- +## `navSystemStatus_Flags_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MW_NAV_FLAG_ADJUSTING_POSITION` | 1 << 0 | | +| `MW_NAV_FLAG_ADJUSTING_ALTITUDE` | 1 << 1 | | + +--- +## `navSystemStatus_Mode_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MW_GPS_MODE_NONE` | 0 | | +| `MW_GPS_MODE_HOLD` | 1 | | +| `MW_GPS_MODE_RTH` | 2 | | +| `MW_GPS_MODE_NAV` | 3 | | +| `MW_GPS_MODE_EMERG` | 15 | | + +--- +## `navSystemStatus_State_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MW_NAV_STATE_NONE` | 0 | | +| `MW_NAV_STATE_RTH_START` | 1 | | +| `MW_NAV_STATE_RTH_ENROUTE` | 2 | | +| `MW_NAV_STATE_HOLD_INFINIT` | 3 | | +| `MW_NAV_STATE_HOLD_TIMED` | 4 | | +| `MW_NAV_STATE_WP_ENROUTE` | 5 | | +| `MW_NAV_STATE_PROCESS_NEXT` | 6 | | +| `MW_NAV_STATE_DO_JUMP` | 7 | | +| `MW_NAV_STATE_LAND_START` | 8 | | +| `MW_NAV_STATE_LAND_IN_PROGRESS` | 9 | | +| `MW_NAV_STATE_LANDED` | 10 | | +| `MW_NAV_STATE_LAND_SETTLE` | 11 | | +| `MW_NAV_STATE_LAND_START_DESCENT` | 12 | | +| `MW_NAV_STATE_HOVER_ABOVE_HOME` | 13 | | +| `MW_NAV_STATE_EMERGENCY_LANDING` | 14 | | +| `MW_NAV_STATE_RTH_CLIMB` | 15 | | + +--- +## `navWaypointActions_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_WP_ACTION_WAYPOINT` | 1 | | +| `NAV_WP_ACTION_HOLD_TIME` | 3 | | +| `NAV_WP_ACTION_RTH` | 4 | | +| `NAV_WP_ACTION_SET_POI` | 5 | | +| `NAV_WP_ACTION_JUMP` | 6 | | +| `NAV_WP_ACTION_SET_HEAD` | 7 | | +| `NAV_WP_ACTION_LAND` | 8 | | + +--- +## `navWaypointFlags_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_WP_FLAG_HOME` | 72 | | +| `NAV_WP_FLAG_LAST` | 165 | | + +--- +## `navWaypointHeadings_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_WP_HEAD_MODE_NONE` | 0 | | +| `NAV_WP_HEAD_MODE_POI` | 1 | | +| `NAV_WP_HEAD_MODE_FIXED` | 2 | | + +--- +## `navWaypointP3Flags_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_WP_ALTMODE` | (1<<0) | | +| `NAV_WP_USER1` | (1<<1) | | +| `NAV_WP_USER2` | (1<<2) | | +| `NAV_WP_USER3` | (1<<3) | | +| `NAV_WP_USER4` | (1<<4) | | + +--- +## `opflowQuality_e` + +> Source: ../../../src/main/sensors/opflow.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OPFLOW_QUALITY_INVALID` | 0 | | +| `OPFLOW_QUALITY_VALID` | 1 | | + +--- +## `opticalFlowSensor_e` + +> Source: ../../../src/main/sensors/opflow.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OPFLOW_NONE` | 0 | | +| `OPFLOW_CXOF` | 1 | | +| `OPFLOW_MSP` | 2 | | +| `OPFLOW_FAKE` | 3 | | + +--- +## `osd_adsb_warning_style_e` + +> Source: ../../../src/main/io/osd.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OSD_ADSB_WARNING_STYLE_COMPACT` | 0 | | +| `OSD_ADSB_WARNING_STYLE_EXTENDED` | 1 | | + +--- +## `osd_ahi_style_e` + +> Source: ../../../src/main/io/osd.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OSD_AHI_STYLE_DEFAULT` | 0 | | +| `OSD_AHI_STYLE_LINE` | 1 | | + +--- +## `osd_alignment_e` + +> Source: ../../../src/main/io/osd.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OSD_ALIGN_LEFT` | 0 | | +| `OSD_ALIGN_RIGHT` | 1 | | + +--- +## `osd_crosshairs_style_e` + +> Source: ../../../src/main/io/osd.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OSD_CROSSHAIRS_STYLE_DEFAULT` | 0 | | +| `OSD_CROSSHAIRS_STYLE_AIRCRAFT` | 1 | | +| `OSD_CROSSHAIRS_STYLE_TYPE3` | 2 | | +| `OSD_CROSSHAIRS_STYLE_TYPE4` | 3 | | +| `OSD_CROSSHAIRS_STYLE_TYPE5` | 4 | | +| `OSD_CROSSHAIRS_STYLE_TYPE6` | 5 | | +| `OSD_CROSSHAIRS_STYLE_TYPE7` | 6 | | + +--- +## `osd_crsf_lq_format_e` + +> Source: ../../../src/main/io/osd.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OSD_CRSF_LQ_TYPE1` | 0 | | +| `OSD_CRSF_LQ_TYPE2` | 1 | | +| `OSD_CRSF_LQ_TYPE3` | 2 | | + +--- +## `osd_items_e` + +> Source: ../../../src/main/io/osd.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OSD_RSSI_VALUE` | 0 | | +| `OSD_MAIN_BATT_VOLTAGE` | 1 | | +| `OSD_CROSSHAIRS` | 2 | | +| `OSD_ARTIFICIAL_HORIZON` | 3 | | +| `OSD_HORIZON_SIDEBARS` | 4 | | +| `OSD_ONTIME` | 5 | | +| `OSD_FLYTIME` | 6 | | +| `OSD_FLYMODE` | 7 | | +| `OSD_CRAFT_NAME` | 8 | | +| `OSD_THROTTLE_POS` | 9 | | +| `OSD_VTX_CHANNEL` | 10 | | +| `OSD_CURRENT_DRAW` | 11 | | +| `OSD_MAH_DRAWN` | 12 | | +| `OSD_GPS_SPEED` | 13 | | +| `OSD_GPS_SATS` | 14 | | +| `OSD_ALTITUDE` | 15 | | +| `OSD_ROLL_PIDS` | 16 | | +| `OSD_PITCH_PIDS` | 17 | | +| `OSD_YAW_PIDS` | 18 | | +| `OSD_POWER` | 19 | | +| `OSD_GPS_LON` | 20 | | +| `OSD_GPS_LAT` | 21 | | +| `OSD_HOME_DIR` | 22 | | +| `OSD_HOME_DIST` | 23 | | +| `OSD_HEADING` | 24 | | +| `OSD_VARIO` | 25 | | +| `OSD_VERTICAL_SPEED_INDICATOR` | 26 | | +| `OSD_AIR_SPEED` | 27 | | +| `OSD_ONTIME_FLYTIME` | 28 | | +| `OSD_RTC_TIME` | 29 | | +| `OSD_MESSAGES` | 30 | | +| `OSD_GPS_HDOP` | 31 | | +| `OSD_MAIN_BATT_CELL_VOLTAGE` | 32 | | +| `OSD_SCALED_THROTTLE_POS` | 33 | | +| `OSD_HEADING_GRAPH` | 34 | | +| `OSD_EFFICIENCY_MAH_PER_KM` | 35 | | +| `OSD_WH_DRAWN` | 36 | | +| `OSD_BATTERY_REMAINING_CAPACITY` | 37 | | +| `OSD_BATTERY_REMAINING_PERCENT` | 38 | | +| `OSD_EFFICIENCY_WH_PER_KM` | 39 | | +| `OSD_TRIP_DIST` | 40 | | +| `OSD_ATTITUDE_PITCH` | 41 | | +| `OSD_ATTITUDE_ROLL` | 42 | | +| `OSD_MAP_NORTH` | 43 | | +| `OSD_MAP_TAKEOFF` | 44 | | +| `OSD_RADAR` | 45 | | +| `OSD_WIND_SPEED_HORIZONTAL` | 46 | | +| `OSD_WIND_SPEED_VERTICAL` | 47 | | +| `OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH` | 48 | | +| `OSD_REMAINING_DISTANCE_BEFORE_RTH` | 49 | | +| `OSD_HOME_HEADING_ERROR` | 50 | | +| `OSD_COURSE_HOLD_ERROR` | 51 | | +| `OSD_COURSE_HOLD_ADJUSTMENT` | 52 | | +| `OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE` | 53 | | +| `OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE` | 54 | | +| `OSD_POWER_SUPPLY_IMPEDANCE` | 55 | | +| `OSD_LEVEL_PIDS` | 56 | | +| `OSD_POS_XY_PIDS` | 57 | | +| `OSD_POS_Z_PIDS` | 58 | | +| `OSD_VEL_XY_PIDS` | 59 | | +| `OSD_VEL_Z_PIDS` | 60 | | +| `OSD_HEADING_P` | 61 | | +| `OSD_BOARD_ALIGN_ROLL` | 62 | | +| `OSD_BOARD_ALIGN_PITCH` | 63 | | +| `OSD_RC_EXPO` | 64 | | +| `OSD_RC_YAW_EXPO` | 65 | | +| `OSD_THROTTLE_EXPO` | 66 | | +| `OSD_PITCH_RATE` | 67 | | +| `OSD_ROLL_RATE` | 68 | | +| `OSD_YAW_RATE` | 69 | | +| `OSD_MANUAL_RC_EXPO` | 70 | | +| `OSD_MANUAL_RC_YAW_EXPO` | 71 | | +| `OSD_MANUAL_PITCH_RATE` | 72 | | +| `OSD_MANUAL_ROLL_RATE` | 73 | | +| `OSD_MANUAL_YAW_RATE` | 74 | | +| `OSD_NAV_FW_CRUISE_THR` | 75 | | +| `OSD_NAV_FW_PITCH2THR` | 76 | | +| `OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE` | 77 | | +| `OSD_DEBUG` | 78 | | +| `OSD_FW_ALT_PID_OUTPUTS` | 79 | | +| `OSD_FW_POS_PID_OUTPUTS` | 80 | | +| `OSD_MC_VEL_X_PID_OUTPUTS` | 81 | | +| `OSD_MC_VEL_Y_PID_OUTPUTS` | 82 | | +| `OSD_MC_VEL_Z_PID_OUTPUTS` | 83 | | +| `OSD_MC_POS_XYZ_P_OUTPUTS` | 84 | | +| `OSD_3D_SPEED` | 85 | | +| `OSD_IMU_TEMPERATURE` | 86 | | +| `OSD_BARO_TEMPERATURE` | 87 | | +| `OSD_TEMP_SENSOR_0_TEMPERATURE` | 88 | | +| `OSD_TEMP_SENSOR_1_TEMPERATURE` | 89 | | +| `OSD_TEMP_SENSOR_2_TEMPERATURE` | 90 | | +| `OSD_TEMP_SENSOR_3_TEMPERATURE` | 91 | | +| `OSD_TEMP_SENSOR_4_TEMPERATURE` | 92 | | +| `OSD_TEMP_SENSOR_5_TEMPERATURE` | 93 | | +| `OSD_TEMP_SENSOR_6_TEMPERATURE` | 94 | | +| `OSD_TEMP_SENSOR_7_TEMPERATURE` | 95 | | +| `OSD_ALTITUDE_MSL` | 96 | | +| `OSD_PLUS_CODE` | 97 | | +| `OSD_MAP_SCALE` | 98 | | +| `OSD_MAP_REFERENCE` | 99 | | +| `OSD_GFORCE` | 100 | | +| `OSD_GFORCE_X` | 101 | | +| `OSD_GFORCE_Y` | 102 | | +| `OSD_GFORCE_Z` | 103 | | +| `OSD_RC_SOURCE` | 104 | | +| `OSD_VTX_POWER` | 105 | | +| `OSD_ESC_RPM` | 106 | | +| `OSD_ESC_TEMPERATURE` | 107 | | +| `OSD_AZIMUTH` | 108 | | +| `OSD_RSSI_DBM` | 109 | | +| `OSD_LQ_UPLINK` | 110 | | +| `OSD_SNR_DB` | 111 | | +| `OSD_TX_POWER_UPLINK` | 112 | | +| `OSD_GVAR_0` | 113 | | +| `OSD_GVAR_1` | 114 | | +| `OSD_GVAR_2` | 115 | | +| `OSD_GVAR_3` | 116 | | +| `OSD_TPA` | 117 | | +| `OSD_NAV_FW_CONTROL_SMOOTHNESS` | 118 | | +| `OSD_VERSION` | 119 | | +| `OSD_RANGEFINDER` | 120 | | +| `OSD_PLIMIT_REMAINING_BURST_TIME` | 121 | | +| `OSD_PLIMIT_ACTIVE_CURRENT_LIMIT` | 122 | | +| `OSD_PLIMIT_ACTIVE_POWER_LIMIT` | 123 | | +| `OSD_GLIDESLOPE` | 124 | | +| `OSD_GPS_MAX_SPEED` | 125 | | +| `OSD_3D_MAX_SPEED` | 126 | | +| `OSD_AIR_MAX_SPEED` | 127 | | +| `OSD_ACTIVE_PROFILE` | 128 | | +| `OSD_MISSION` | 129 | | +| `OSD_SWITCH_INDICATOR_0` | 130 | | +| `OSD_SWITCH_INDICATOR_1` | 131 | | +| `OSD_SWITCH_INDICATOR_2` | 132 | | +| `OSD_SWITCH_INDICATOR_3` | 133 | | +| `OSD_TPA_TIME_CONSTANT` | 134 | | +| `OSD_FW_LEVEL_TRIM` | 135 | | +| `OSD_GLIDE_TIME_REMAINING` | 136 | | +| `OSD_GLIDE_RANGE` | 137 | | +| `OSD_CLIMB_EFFICIENCY` | 138 | | +| `OSD_NAV_WP_MULTI_MISSION_INDEX` | 139 | | +| `OSD_GROUND_COURSE` | 140 | | +| `OSD_CROSS_TRACK_ERROR` | 141 | | +| `OSD_PILOT_NAME` | 142 | | +| `OSD_PAN_SERVO_CENTRED` | 143 | | +| `OSD_MULTI_FUNCTION` | 144 | | +| `OSD_ODOMETER` | 145 | | +| `OSD_PILOT_LOGO` | 146 | | +| `OSD_CUSTOM_ELEMENT_1` | 147 | | +| `OSD_CUSTOM_ELEMENT_2` | 148 | | +| `OSD_CUSTOM_ELEMENT_3` | 149 | | +| `OSD_ADSB_WARNING` | 150 | | +| `OSD_ADSB_INFO` | 151 | | +| `OSD_BLACKBOX` | 152 | | +| `OSD_FORMATION_FLIGHT` | 153 | | +| `OSD_CUSTOM_ELEMENT_4` | 154 | | +| `OSD_CUSTOM_ELEMENT_5` | 155 | | +| `OSD_CUSTOM_ELEMENT_6` | 156 | | +| `OSD_CUSTOM_ELEMENT_7` | 157 | | +| `OSD_CUSTOM_ELEMENT_8` | 158 | | +| `OSD_LQ_DOWNLINK` | 159 | | +| `OSD_RX_POWER_DOWNLINK` | 160 | | +| `OSD_RX_BAND` | 161 | | +| `OSD_RX_MODE` | 162 | | +| `OSD_COURSE_TO_FENCE` | 163 | | +| `OSD_H_DIST_TO_FENCE` | 164 | | +| `OSD_V_DIST_TO_FENCE` | 165 | | +| `OSD_NAV_FW_ALT_CONTROL_RESPONSE` | 166 | | +| `OSD_NAV_MIN_GROUND_SPEED` | 167 | | +| `OSD_THROTTLE_GAUGE` | 168 | | +| `OSD_ITEM_COUNT` | 169 | | + +--- +## `osd_sidebar_arrow_e` + +> Source: ../../../src/main/io/osd_grid.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `OSD_SIDEBAR_ARROW_NONE` | 0 | | +| `OSD_SIDEBAR_ARROW_UP` | 1 | | +| `OSD_SIDEBAR_ARROW_DOWN` | 2 | | + +--- +## `osd_sidebar_scroll_e` + +> Source: ../../../src/main/io/osd.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OSD_SIDEBAR_SCROLL_NONE` | 0 | | +| `OSD_SIDEBAR_SCROLL_ALTITUDE` | 1 | | +| `OSD_SIDEBAR_SCROLL_SPEED` | 2 | | +| `OSD_SIDEBAR_SCROLL_HOME_DISTANCE` | 3 | | +| `OSD_SIDEBAR_SCROLL_MAX` | OSD_SIDEBAR_SCROLL_HOME_DISTANCE | | + +--- +## `osd_SpeedTypes_e` + +> Source: ../../../src/main/io/osd.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OSD_SPEED_TYPE_GROUND` | 0 | | +| `OSD_SPEED_TYPE_AIR` | 1 | | +| `OSD_SPEED_TYPE_3D` | 2 | | +| `OSD_SPEED_TYPE_MIN_GROUND` | 3 | | + +--- +## `osd_stats_energy_unit_e` + +> Source: ../../../src/main/io/osd.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OSD_STATS_ENERGY_UNIT_MAH` | 0 | | +| `OSD_STATS_ENERGY_UNIT_WH` | 1 | | + +--- +## `osd_unit_e` + +> Source: ../../../src/main/io/osd.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OSD_UNIT_IMPERIAL` | 0 | | +| `OSD_UNIT_METRIC` | 1 | | +| `OSD_UNIT_METRIC_MPH` | 2 | | +| `OSD_UNIT_UK` | 3 | | +| `OSD_UNIT_GA` | 4 | | +| `OSD_UNIT_MAX` | OSD_UNIT_GA | | + +--- +## `osdCustomElementType_e` + +> Source: ../../../src/main/io/osd/custom_elements.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `CUSTOM_ELEMENT_TYPE_NONE` | 0 | | +| `CUSTOM_ELEMENT_TYPE_TEXT` | 1 | | +| `CUSTOM_ELEMENT_TYPE_ICON_STATIC` | 2 | | +| `CUSTOM_ELEMENT_TYPE_ICON_GV` | 3 | | +| `CUSTOM_ELEMENT_TYPE_ICON_LC` | 4 | | +| `CUSTOM_ELEMENT_TYPE_GV_1` | 5 | | +| `CUSTOM_ELEMENT_TYPE_GV_2` | 6 | | +| `CUSTOM_ELEMENT_TYPE_GV_3` | 7 | | +| `CUSTOM_ELEMENT_TYPE_GV_4` | 8 | | +| `CUSTOM_ELEMENT_TYPE_GV_5` | 9 | | +| `CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_1` | 10 | | +| `CUSTOM_ELEMENT_TYPE_GV_FLOAT_1_2` | 11 | | +| `CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_1` | 12 | | +| `CUSTOM_ELEMENT_TYPE_GV_FLOAT_2_2` | 13 | | +| `CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_1` | 14 | | +| `CUSTOM_ELEMENT_TYPE_GV_FLOAT_3_2` | 15 | | +| `CUSTOM_ELEMENT_TYPE_GV_FLOAT_4_1` | 16 | | +| `CUSTOM_ELEMENT_TYPE_LC_1` | 17 | | +| `CUSTOM_ELEMENT_TYPE_LC_2` | 18 | | +| `CUSTOM_ELEMENT_TYPE_LC_3` | 19 | | +| `CUSTOM_ELEMENT_TYPE_LC_4` | 20 | | +| `CUSTOM_ELEMENT_TYPE_LC_5` | 21 | | +| `CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_1` | 22 | | +| `CUSTOM_ELEMENT_TYPE_LC_FLOAT_1_2` | 23 | | +| `CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_1` | 24 | | +| `CUSTOM_ELEMENT_TYPE_LC_FLOAT_2_2` | 25 | | +| `CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_1` | 26 | | +| `CUSTOM_ELEMENT_TYPE_LC_FLOAT_3_2` | 27 | | +| `CUSTOM_ELEMENT_TYPE_LC_FLOAT_4_1` | 28 | | +| `CUSTOM_ELEMENT_TYPE_END` | 29 | | + +--- +## `osdCustomElementTypeVisibility_e` + +> Source: ../../../src/main/io/osd/custom_elements.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `CUSTOM_ELEMENT_VISIBILITY_ALWAYS` | 0 | | +| `CUSTOM_ELEMENT_VISIBILITY_GV` | 1 | | +| `CUSTOM_ELEMENT_VISIBILITY_LOGIC_CON` | 2 | | + +--- +## `osdDrawPointType_e` + +> Source: ../../../src/main/io/osd_common.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OSD_DRAW_POINT_TYPE_GRID` | 0 | | +| `OSD_DRAW_POINT_TYPE_PIXEL` | 1 | | + +--- +## `osdDriver_e` + +> Source: ../../../src/main/drivers/osd.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OSD_DRIVER_NONE` | 0 | | +| `OSD_DRIVER_MAX7456` | 1 | | + +--- +## `osdSpeedSource_e` + +> Source: ../../../src/main/io/osd_common.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OSD_SPEED_SOURCE_GROUND` | 0 | | +| `OSD_SPEED_SOURCE_3D` | 1 | | +| `OSD_SPEED_SOURCE_AIR` | 2 | | + +--- +## `outputMode_e` + +> Source: ../../../src/main/flight/mixer.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OUTPUT_MODE_AUTO` | 0 | | +| `OUTPUT_MODE_MOTORS` | 1 | | +| `OUTPUT_MODE_SERVOS` | 2 | | +| `OUTPUT_MODE_LED` | 3 | | + +--- +## `pageId_e` + +> Source: ../../../src/main/io/dashboard.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `PAGE_WELCOME` | 0 | | +| `PAGE_ARMED` | 1 | | +| `PAGE_STATUS` | 2 | | + +--- +## `persistentObjectId_e` + +> Source: ../../../src/main/drivers/persistent.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `PERSISTENT_OBJECT_MAGIC` | 0 | | +| `PERSISTENT_OBJECT_RESET_REASON` | 1 | | +| `PERSISTENT_OBJECT_COUNT` | 2 | | + +--- +## `pidAutotuneState_e` + +> Source: ../../../src/main/flight/pid_autotune.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `DEMAND_TOO_LOW` | 0 | | +| `DEMAND_UNDERSHOOT` | 1 | | +| `DEMAND_OVERSHOOT` | 2 | | +| `TUNE_UPDATED` | 3 | | + +--- +## `pidControllerFlags_e` + +> Source: ../../../src/main/common/fp_pid.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `PID_DTERM_FROM_ERROR` | 1 << 0 | | +| `PID_ZERO_INTEGRATOR` | 1 << 1 | | +| `PID_SHRINK_INTEGRATOR` | 1 << 2 | | +| `PID_LIMIT_INTEGRATOR` | 1 << 3 | | +| `PID_FREEZE_INTEGRATOR` | 1 << 4 | | + +--- +## `pidIndex_e` + +> Source: ../../../src/main/flight/pid.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `PID_ROLL` | 0 | | +| `PID_PITCH` | 1 | | +| `PID_YAW` | 2 | | +| `PID_POS_Z` | 3 | | +| `PID_POS_XY` | 4 | | +| `PID_VEL_XY` | 5 | | +| `PID_SURFACE` | 6 | | +| `PID_LEVEL` | 7 | | +| `PID_HEADING` | 8 | | +| `PID_VEL_Z` | 9 | | +| `PID_POS_HEADING` | 10 | | +| `PID_ITEM_COUNT` | 11 | | + +--- +## `pidType_e` + +> Source: ../../../src/main/flight/pid.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `PID_TYPE_NONE` | 0 | | +| `PID_TYPE_PID` | 1 | | +| `PID_TYPE_PIFF` | 2 | | +| `PID_TYPE_AUTO` | 3 | | + +--- +## `pinLabel_e` + +> Source: ../../../src/main/drivers/pwm_mapping.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `PIN_LABEL_NONE` | 0 | | +| `PIN_LABEL_LED` | 1 | | + +--- +## `pitotSensor_e` + +> Source: ../../../src/main/sensors/pitotmeter.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `PITOT_NONE` | 0 | | +| `PITOT_AUTODETECT` | 1 | | +| `PITOT_MS4525` | 2 | | +| `PITOT_ADC` | 3 | | +| `PITOT_VIRTUAL` | 4 | | +| `PITOT_FAKE` | 5 | | +| `PITOT_MSP` | 6 | | +| `PITOT_DLVR` | 7 | | + +--- +## `pollType_e` + +> Source: ../../../src/main/io/smartport_master.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `PT_ACTIVE_ID` | 0 | | +| `PT_INACTIVE_ID` | 1 | | + +--- +## `portSharing_e` + +> Source: ../../../src/main/io/serial.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `PORTSHARING_UNUSED` | 0 | | +| `PORTSHARING_NOT_SHARED` | 1 | | +| `PORTSHARING_SHARED` | 2 | | + +--- +## `pwmInitError_e` + +> Source: ../../../src/main/drivers/pwm_mapping.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `PWM_INIT_ERROR_NONE` | 0 | | +| `PWM_INIT_ERROR_TOO_MANY_MOTORS` | 1 | | +| `PWM_INIT_ERROR_TOO_MANY_SERVOS` | 2 | | +| `PWM_INIT_ERROR_NOT_ENOUGH_MOTOR_OUTPUTS` | 3 | | +| `PWM_INIT_ERROR_NOT_ENOUGH_SERVO_OUTPUTS` | 4 | | +| `PWM_INIT_ERROR_TIMER_INIT_FAILED` | 5 | | + +--- +## `quadrant_e` + +> Source: ../../../src/main/io/ledstrip.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `QUADRANT_NORTH` | 1 << 0 | | +| `QUADRANT_SOUTH` | 1 << 1 | | +| `QUADRANT_EAST` | 1 << 2 | | +| `QUADRANT_WEST` | 1 << 3 | | +| `QUADRANT_NORTH_EAST` | 1 << 4 | | +| `QUADRANT_SOUTH_EAST` | 1 << 5 | | +| `QUADRANT_NORTH_WEST` | 1 << 6 | | +| `QUADRANT_SOUTH_WEST` | 1 << 7 | | +| `QUADRANT_NONE` | 1 << 8 | | +| `QUADRANT_NOTDIAG` | 1 << 9 | | +| `QUADRANT_ANY` | QUADRANT_NORTH | QUADRANT_SOUTH | QUADRANT_EAST | QUADRANT_WEST | QUADRANT_NONE | | + +--- +## `QUADSPIClockDivider_e` + +> Source: ../../../src/main/drivers/bus_quadspi.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `QUADSPI_CLOCK_INITIALISATION` | 255 | | +| `QUADSPI_CLOCK_SLOW` | 19 | | +| `QUADSPI_CLOCK_STANDARD` | 9 | | +| `QUADSPI_CLOCK_FAST` | 3 | | +| `QUADSPI_CLOCK_ULTRAFAST` | 1 | | + +--- +## `quadSpiMode_e` + +> Source: ../../../src/main/drivers/bus_quadspi.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `QUADSPI_MODE_BK1_ONLY` | 0 | | +| `QUADSPI_MODE_BK2_ONLY` | 1 | | +| `QUADSPI_MODE_DUAL_FLASH` | 2 | | + +--- +## `rangefinderType_e` + +> Source: ../../../src/main/sensors/rangefinder.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `RANGEFINDER_NONE` | 0 | | +| `RANGEFINDER_SRF10` | 1 | | +| `RANGEFINDER_VL53L0X` | 2 | | +| `RANGEFINDER_MSP` | 3 | | +| `RANGEFINDER_BENEWAKE` | 4 | | +| `RANGEFINDER_VL53L1X` | 5 | | +| `RANGEFINDER_US42` | 6 | | +| `RANGEFINDER_TOF10102I2C` | 7 | | +| `RANGEFINDER_FAKE` | 8 | | +| `RANGEFINDER_TERARANGER_EVO` | 9 | | +| `RANGEFINDER_USD1_V0` | 10 | | +| `RANGEFINDER_NANORADAR` | 11 | | + +--- +## `RCDEVICE_5key_connection_event_e` + +> Source: ../../../src/main/io/rcdevice.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `RCDEVICE_PROTOCOL_5KEY_CONNECTION_OPEN` | 1 | | +| `RCDEVICE_PROTOCOL_5KEY_CONNECTION_CLOSE` | 2 | | + +--- +## `rcdevice_5key_simulation_operation_e` + +> Source: ../../../src/main/io/rcdevice.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `RCDEVICE_PROTOCOL_5KEY_SIMULATION_NONE` | 0 | | +| `RCDEVICE_PROTOCOL_5KEY_SIMULATION_SET` | 1 | | +| `RCDEVICE_PROTOCOL_5KEY_SIMULATION_LEFT` | 2 | | +| `RCDEVICE_PROTOCOL_5KEY_SIMULATION_RIGHT` | 3 | | +| `RCDEVICE_PROTOCOL_5KEY_SIMULATION_UP` | 4 | | +| `RCDEVICE_PROTOCOL_5KEY_SIMULATION_DOWN` | 5 | | + +--- +## `rcdevice_camera_control_opeation_e` + +> Source: ../../../src/main/io/rcdevice.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN` | 0 | | +| `RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_POWER_BTN` | 1 | | +| `RCDEVICE_PROTOCOL_CAM_CTRL_CHANGE_MODE` | 2 | | +| `RCDEVICE_PROTOCOL_CAM_CTRL_START_RECORDING` | 3 | | +| `RCDEVICE_PROTOCOL_CAM_CTRL_STOP_RECORDING` | 4 | | +| `RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION` | 255 | | + +--- +## `rcdevice_features_e` + +> Source: ../../../src/main/io/rcdevice.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON` | (1 << 0) | | +| `RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON` | (1 << 1) | | +| `RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE` | (1 << 2) | | +| `RCDEVICE_PROTOCOL_FEATURE_SIMULATE_5_KEY_OSD_CABLE` | (1 << 3) | | +| `RCDEVICE_PROTOCOL_FEATURE_START_RECORDING` | (1 << 6) | | +| `RCDEVICE_PROTOCOL_FEATURE_STOP_RECORDING` | (1 << 7) | | +| `RCDEVICE_PROTOCOL_FEATURE_CMS_MENU` | (1 << 8) | | + +--- +## `rcdevice_protocol_version_e` + +> Source: ../../../src/main/io/rcdevice.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `RCDEVICE_PROTOCOL_RCSPLIT_VERSION` | 0 | | +| `RCDEVICE_PROTOCOL_VERSION_1_0` | 1 | | +| `RCDEVICE_PROTOCOL_UNKNOWN` | 2 | | + +--- +## `rcdeviceCamSimulationKeyEvent_e` + +> Source: ../../../src/main/io/rcdevice.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `RCDEVICE_CAM_KEY_NONE` | 0 | | +| `RCDEVICE_CAM_KEY_ENTER` | 1 | | +| `RCDEVICE_CAM_KEY_LEFT` | 2 | | +| `RCDEVICE_CAM_KEY_UP` | 3 | | +| `RCDEVICE_CAM_KEY_RIGHT` | 4 | | +| `RCDEVICE_CAM_KEY_DOWN` | 5 | | +| `RCDEVICE_CAM_KEY_CONNECTION_CLOSE` | 6 | | +| `RCDEVICE_CAM_KEY_CONNECTION_OPEN` | 7 | | +| `RCDEVICE_CAM_KEY_RELEASE` | 8 | | + +--- +## `rcdeviceResponseStatus_e` + +> Source: ../../../src/main/io/rcdevice.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `RCDEVICE_RESP_SUCCESS` | 0 | | +| `RCDEVICE_RESP_INCORRECT_CRC` | 1 | | +| `RCDEVICE_RESP_TIMEOUT` | 2 | | + +--- +## `resolutionType_e` + +> Source: ../../../src/main/io/displayport_msp_osd.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `SD_3016` | 0 | | +| `HD_5018` | 1 | | +| `HD_3016` | 2 | | +| `HD_6022` | 3 | | +| `HD_5320` | 4 | | + +--- +## `resourceOwner_e` + +> Source: ../../../src/main/drivers/resource.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `OWNER_FREE` | 0 | | +| `OWNER_PWMIO` | 1 | | +| `OWNER_MOTOR` | 2 | | +| `OWNER_SERVO` | 3 | | +| `OWNER_SOFTSERIAL` | 4 | | +| `OWNER_ADC` | 5 | | +| `OWNER_SERIAL` | 6 | | +| `OWNER_TIMER` | 7 | | +| `OWNER_RANGEFINDER` | 8 | | +| `OWNER_SYSTEM` | 9 | | +| `OWNER_SPI` | 10 | | +| `OWNER_QUADSPI` | 11 | | +| `OWNER_I2C` | 12 | | +| `OWNER_SDCARD` | 13 | | +| `OWNER_FLASH` | 14 | | +| `OWNER_USB` | 15 | | +| `OWNER_BEEPER` | 16 | | +| `OWNER_OSD` | 17 | | +| `OWNER_BARO` | 18 | | +| `OWNER_MPU` | 19 | | +| `OWNER_INVERTER` | 20 | | +| `OWNER_LED_STRIP` | 21 | | +| `OWNER_LED` | 22 | | +| `OWNER_RX` | 23 | | +| `OWNER_TX` | 24 | | +| `OWNER_VTX` | 25 | | +| `OWNER_SPI_PREINIT` | 26 | | +| `OWNER_COMPASS` | 27 | | +| `OWNER_TEMPERATURE` | 28 | | +| `OWNER_1WIRE` | 29 | | +| `OWNER_AIRSPEED` | 30 | | +| `OWNER_OLED_DISPLAY` | 31 | | +| `OWNER_PINIO` | 32 | | +| `OWNER_IRLOCK` | 33 | | +| `OWNER_TOTAL_COUNT` | 34 | | + +--- +## `resourceType_e` + +> Source: ../../../src/main/drivers/resource.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `RESOURCE_NONE` | 0 | | +| `RESOURCE_INPUT` | 1 | | +| `RESOURCE_TIMER` | 2 | | +| `RESOURCE_UART_TX` | 3 | | +| `RESOURCE_EXTI` | 4 | | +| `RESOURCE_I2C_SCL` | 5 | | +| `RESOURCE_SPI_SCK` | 6 | | +| `RESOURCE_QUADSPI_CLK` | 7 | | +| `RESOURCE_QUADSPI_BK1IO2` | 8 | | +| `RESOURCE_QUADSPI_BK2IO0` | 9 | | +| `RESOURCE_QUADSPI_BK2IO3` | 10 | | +| `RESOURCE_ADC_CH1` | 11 | | +| `RESOURCE_RX_CE` | 12 | | +| `RESOURCE_TOTAL_COUNT` | 13 | | + +--- +## `reversibleMotorsThrottleState_e` + +> Source: ../../../src/main/flight/mixer.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MOTOR_DIRECTION_FORWARD` | 0 | | +| `MOTOR_DIRECTION_BACKWARD` | 1 | | +| `MOTOR_DIRECTION_DEADBAND` | 2 | | + +--- +## `rollPitchStatus_e` + +> Source: ../../../src/main/fc/rc_controls.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NOT_CENTERED` | 0 | | +| `CENTERED` | 1 | | + +--- +## `rssiSource_e` + +> Source: ../../../src/main/rx/rx.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `RSSI_SOURCE_NONE` | 0 | | +| `RSSI_SOURCE_AUTO` | 1 | | +| `RSSI_SOURCE_ADC` | 2 | | +| `RSSI_SOURCE_RX_CHANNEL` | 3 | | +| `RSSI_SOURCE_RX_PROTOCOL` | 4 | | +| `RSSI_SOURCE_MSP` | 5 | | + +--- +## `rthState_e` + +> Source: ../../../src/main/flight/failsafe.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `RTH_IDLE` | 0 | | +| `RTH_IN_PROGRESS` | 1 | | +| `RTH_HAS_LANDED` | 2 | | + +--- +## `rthTargetMode_e` + +> Source: ../../../src/main/navigation/navigation_private.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `RTH_HOME_ENROUTE_INITIAL` | 0 | | +| `RTH_HOME_ENROUTE_PROPORTIONAL` | 1 | | +| `RTH_HOME_ENROUTE_FINAL` | 2 | | +| `RTH_HOME_FINAL_LOITER` | 3 | | +| `RTH_HOME_FINAL_LAND` | 4 | | + +--- +## `rthTrackbackMode_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `RTH_TRACKBACK_OFF` | 0 | | +| `RTH_TRACKBACK_ON` | 1 | | +| `RTH_TRACKBACK_FS` | 2 | | + +--- +## `rxFrameState_e` + +> Source: ../../../src/main/rx/rx.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `RX_FRAME_PENDING` | 0 | | +| `RX_FRAME_COMPLETE` | (1 << 0) | | +| `RX_FRAME_FAILSAFE` | (1 << 1) | | +| `RX_FRAME_PROCESSING_REQUIRED` | (1 << 2) | | +| `RX_FRAME_DROPPED` | (1 << 3) | | + +--- +## `rxReceiverType_e` + +> Source: ../../../src/main/rx/rx.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `RX_TYPE_NONE` | 0 | | +| `RX_TYPE_SERIAL` | 1 | | +| `RX_TYPE_MSP` | 2 | | +| `RX_TYPE_SIM` | 3 | | + +--- +## `rxSerialReceiverType_e` + +> Source: ../../../src/main/rx/rx.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SERIALRX_SPEKTRUM1024` | 0 | | +| `SERIALRX_SPEKTRUM2048` | 1 | | +| `SERIALRX_SBUS` | 2 | | +| `SERIALRX_SUMD` | 3 | | +| `SERIALRX_IBUS` | 4 | | +| `SERIALRX_JETIEXBUS` | 5 | | +| `SERIALRX_CRSF` | 6 | | +| `SERIALRX_FPORT` | 7 | | +| `SERIALRX_SBUS_FAST` | 8 | | +| `SERIALRX_FPORT2` | 9 | | +| `SERIALRX_SRXL2` | 10 | | +| `SERIALRX_GHST` | 11 | | +| `SERIALRX_MAVLINK` | 12 | | +| `SERIALRX_FBUS` | 13 | | +| `SERIALRX_SBUS2` | 14 | | + +--- +## `safehomeUsageMode_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SAFEHOME_USAGE_OFF` | 0 | | +| `SAFEHOME_USAGE_RTH` | 1 | | +| `SAFEHOME_USAGE_RTH_FS` | 2 | | + +--- +## `sbasMode_e` + +> Source: ../../../src/main/io/gps.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SBAS_AUTO` | 0 | | +| `SBAS_EGNOS` | 1 | | +| `SBAS_WAAS` | 2 | | +| `SBAS_MSAS` | 3 | | +| `SBAS_GAGAN` | 4 | | +| `SBAS_SPAN` | 5 | | +| `SBAS_NONE` | 6 | | + +--- +## `sbusDecoderState_e` + +> Source: ../../../src/main/rx/sbus.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `STATE_SBUS_SYNC` | 0 | | +| `STATE_SBUS_PAYLOAD` | 1 | | +| `STATE_SBUS26_PAYLOAD` | 2 | | +| `STATE_SBUS_WAIT_SYNC` | 3 | | + +--- +## `sdcardBlockOperation_e` + +> Source: ../../../src/main/drivers/sdcard/sdcard.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SDCARD_BLOCK_OPERATION_READ` | 0 | | +| `SDCARD_BLOCK_OPERATION_WRITE` | 1 | | +| `SDCARD_BLOCK_OPERATION_ERASE` | 2 | | + +--- +## `sdcardOperationStatus_e` + +> Source: ../../../src/main/drivers/sdcard/sdcard.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SDCARD_OPERATION_IN_PROGRESS` | 0 | | +| `SDCARD_OPERATION_BUSY` | 1 | | +| `SDCARD_OPERATION_SUCCESS` | 2 | | +| `SDCARD_OPERATION_FAILURE` | 3 | | + +--- +## `sdcardReceiveBlockStatus_e` + +> Source: ../../../src/main/drivers/sdcard/sdcard_sdio.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `SDCARD_RECEIVE_SUCCESS` | 0 | | +| `SDCARD_RECEIVE_BLOCK_IN_PROGRESS` | 1 | | +| `SDCARD_RECEIVE_ERROR` | 2 | | + +--- +## `sdcardReceiveBlockStatus_e` + +> Source: ../../../src/main/drivers/sdcard/sdcard_spi.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `SDCARD_RECEIVE_SUCCESS` | 0 | | +| `SDCARD_RECEIVE_BLOCK_IN_PROGRESS` | 1 | | +| `SDCARD_RECEIVE_ERROR` | 2 | | + +--- +## `sdcardState_e` + +> Source: ../../../src/main/drivers/sdcard/sdcard_impl.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SDCARD_STATE_NOT_PRESENT` | 0 | | +| `SDCARD_STATE_RESET` | 1 | | +| `SDCARD_STATE_CARD_INIT_IN_PROGRESS` | 2 | | +| `SDCARD_STATE_INITIALIZATION_RECEIVE_CID` | 3 | | +| `SDCARD_STATE_READY` | 4 | | +| `SDCARD_STATE_READING` | 5 | | +| `SDCARD_STATE_SENDING_WRITE` | 6 | | +| `SDCARD_STATE_WAITING_FOR_WRITE` | 7 | | +| `SDCARD_STATE_WRITING_MULTIPLE_BLOCKS` | 8 | | +| `SDCARD_STATE_STOPPING_MULTIPLE_BLOCK_WRITE` | 9 | | + +--- +## `SDIODevice` + +> Source: ../../../src/main/drivers/sdio.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SDIOINVALID` | -1 | | +| `SDIODEV_1` | 0 | | +| `SDIODEV_2` | 1 | | + +--- +## `sensor_align_e` + +> Source: ../../../src/main/drivers/sensor.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `ALIGN_DEFAULT` | 0 | | +| `CW0_DEG` | 1 | | +| `CW90_DEG` | 2 | | +| `CW180_DEG` | 3 | | +| `CW270_DEG` | 4 | | +| `CW0_DEG_FLIP` | 5 | | +| `CW90_DEG_FLIP` | 6 | | +| `CW180_DEG_FLIP` | 7 | | +| `CW270_DEG_FLIP` | 8 | | + +--- +## `sensorIndex_e` + +> Source: ../../../src/main/sensors/sensors.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SENSOR_INDEX_GYRO` | 0 | | +| `SENSOR_INDEX_ACC` | 1 | | +| `SENSOR_INDEX_BARO` | 2 | | +| `SENSOR_INDEX_MAG` | 3 | | +| `SENSOR_INDEX_RANGEFINDER` | 4 | | +| `SENSOR_INDEX_PITOT` | 5 | | +| `SENSOR_INDEX_OPFLOW` | 6 | | +| `SENSOR_INDEX_COUNT` | 7 | | + +--- +## `sensors_e` + +> Source: ../../../src/main/sensors/sensors.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SENSOR_GYRO` | 1 << 0 | | +| `SENSOR_ACC` | 1 << 1 | | +| `SENSOR_BARO` | 1 << 2 | | +| `SENSOR_MAG` | 1 << 3 | | +| `SENSOR_RANGEFINDER` | 1 << 4 | | +| `SENSOR_PITOT` | 1 << 5 | | +| `SENSOR_OPFLOW` | 1 << 6 | | +| `SENSOR_GPS` | 1 << 7 | | +| `SENSOR_GPSMAG` | 1 << 8 | | +| `SENSOR_TEMP` | 1 << 9 | | + +--- +## `sensorTempCalState_e` + +> Source: ../../../src/main/sensors/sensors.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SENSOR_TEMP_CAL_INITIALISE` | 0 | | +| `SENSOR_TEMP_CAL_IN_PROGRESS` | 1 | | +| `SENSOR_TEMP_CAL_COMPLETE` | 2 | | + +--- +## `serialPortFunction_e` + +> Source: ../../../src/main/io/serial.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FUNCTION_NONE` | 0 | | +| `FUNCTION_MSP` | (1 << 0) | | +| `FUNCTION_GPS` | (1 << 1) | | +| `FUNCTION_UNUSED_3` | (1 << 2) | | +| `FUNCTION_TELEMETRY_HOTT` | (1 << 3) | | +| `FUNCTION_TELEMETRY_LTM` | (1 << 4) | | +| `FUNCTION_TELEMETRY_SMARTPORT` | (1 << 5) | | +| `FUNCTION_RX_SERIAL` | (1 << 6) | | +| `FUNCTION_BLACKBOX` | (1 << 7) | | +| `FUNCTION_TELEMETRY_MAVLINK` | (1 << 8) | | +| `FUNCTION_TELEMETRY_IBUS` | (1 << 9) | | +| `FUNCTION_RCDEVICE` | (1 << 10) | | +| `FUNCTION_VTX_SMARTAUDIO` | (1 << 11) | | +| `FUNCTION_VTX_TRAMP` | (1 << 12) | | +| `FUNCTION_UNUSED_1` | (1 << 13) | | +| `FUNCTION_OPTICAL_FLOW` | (1 << 14) | | +| `FUNCTION_LOG` | (1 << 15) | | +| `FUNCTION_RANGEFINDER` | (1 << 16) | | +| `FUNCTION_VTX_FFPV` | (1 << 17) | | +| `FUNCTION_ESCSERIAL` | (1 << 18) | | +| `FUNCTION_TELEMETRY_SIM` | (1 << 19) | | +| `FUNCTION_FRSKY_OSD` | (1 << 20) | | +| `FUNCTION_DJI_HD_OSD` | (1 << 21) | | +| `FUNCTION_SERVO_SERIAL` | (1 << 22) | | +| `FUNCTION_TELEMETRY_SMARTPORT_MASTER` | (1 << 23) | | +| `FUNCTION_UNUSED_2` | (1 << 24) | | +| `FUNCTION_MSP_OSD` | (1 << 25) | | +| `FUNCTION_GIMBAL` | (1 << 26) | | +| `FUNCTION_GIMBAL_HEADTRACKER` | (1 << 27) | | + +--- +## `serialPortIdentifier_e` + +> Source: ../../../src/main/io/serial.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SERIAL_PORT_NONE` | -1 | | +| `SERIAL_PORT_USART1` | 0 | | +| `SERIAL_PORT_USART2` | 1 | | +| `SERIAL_PORT_USART3` | 2 | | +| `SERIAL_PORT_USART4` | 3 | | +| `SERIAL_PORT_USART5` | 4 | | +| `SERIAL_PORT_USART6` | 5 | | +| `SERIAL_PORT_USART7` | 6 | | +| `SERIAL_PORT_USART8` | 7 | | +| `SERIAL_PORT_USB_VCP` | 20 | | +| `SERIAL_PORT_SOFTSERIAL1` | 30 | | +| `SERIAL_PORT_SOFTSERIAL2` | 31 | | +| `SERIAL_PORT_IDENTIFIER_MAX` | SERIAL_PORT_SOFTSERIAL2 | | + +--- +## `servoAutotrimState_e` + +> Source: ../../../src/main/flight/servos.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `AUTOTRIM_IDLE` | 0 | | +| `AUTOTRIM_COLLECTING` | 1 | | +| `AUTOTRIM_SAVE_PENDING` | 2 | | +| `AUTOTRIM_DONE` | 3 | | + +--- +## `servoIndex_e` + +> Source: ../../../src/main/flight/servos.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SERVO_GIMBAL_PITCH` | 0 | | +| `SERVO_GIMBAL_ROLL` | 1 | | +| `SERVO_ELEVATOR` | 2 | | +| `SERVO_FLAPPERON_1` | 3 | | +| `SERVO_FLAPPERON_2` | 4 | | +| `SERVO_RUDDER` | 5 | | +| `SERVO_BICOPTER_LEFT` | 4 | | +| `SERVO_BICOPTER_RIGHT` | 5 | | +| `SERVO_DUALCOPTER_LEFT` | 4 | | +| `SERVO_DUALCOPTER_RIGHT` | 5 | | +| `SERVO_SINGLECOPTER_1` | 3 | | +| `SERVO_SINGLECOPTER_2` | 4 | | +| `SERVO_SINGLECOPTER_3` | 5 | | +| `SERVO_SINGLECOPTER_4` | 6 | | + +--- +## `servoProtocolType_e` + +> Source: ../../../src/main/drivers/pwm_mapping.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SERVO_TYPE_PWM` | 0 | | +| `SERVO_TYPE_SBUS` | 1 | | +| `SERVO_TYPE_SBUS_PWM` | 2 | | + +--- +## `setting_mode_e` + +> Source: ../../../src/main/fc/settings.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MODE_DIRECT` | (0 << SETTING_MODE_OFFSET) | | +| `MODE_LOOKUP` | (1 << SETTING_MODE_OFFSET) | | + +--- +## `setting_section_e` + +> Source: ../../../src/main/fc/settings.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `MASTER_VALUE` | (0 << SETTING_SECTION_OFFSET) | | +| `PROFILE_VALUE` | (1 << SETTING_SECTION_OFFSET) | | +| `CONTROL_VALUE` | (2 << SETTING_SECTION_OFFSET) | | +| `BATTERY_CONFIG_VALUE` | (3 << SETTING_SECTION_OFFSET) | | +| `MIXER_CONFIG_VALUE` | (4 << SETTING_SECTION_OFFSET) | | +| `EZ_TUNE_VALUE` | (5 << SETTING_SECTION_OFFSET) | | + +--- +## `setting_type_e` + +> Source: ../../../src/main/fc/settings.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `VAR_UINT8` | (0 << SETTING_TYPE_OFFSET) | | +| `VAR_INT8` | (1 << SETTING_TYPE_OFFSET) | | +| `VAR_UINT16` | (2 << SETTING_TYPE_OFFSET) | | +| `VAR_INT16` | (3 << SETTING_TYPE_OFFSET) | | +| `VAR_UINT32` | (4 << SETTING_TYPE_OFFSET) | | +| `VAR_FLOAT` | (5 << SETTING_TYPE_OFFSET) | | +| `VAR_STRING` | (6 << SETTING_TYPE_OFFSET) | | + +--- +## `simATCommandState_e` + +> Source: ../../../src/main/telemetry/sim.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `SIM_AT_OK` | 0 | | +| `SIM_AT_ERROR` | 1 | | +| `SIM_AT_WAITING_FOR_RESPONSE` | 2 | | + +--- +## `simModuleState_e` + +> Source: ../../../src/main/telemetry/sim.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `SIM_MODULE_NOT_DETECTED` | 0 | | +| `SIM_MODULE_NOT_REGISTERED` | 1 | | +| `SIM_MODULE_REGISTERED` | 2 | | + +--- +## `simReadState_e` + +> Source: ../../../src/main/telemetry/sim.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `SIM_READSTATE_RESPONSE` | 0 | | +| `SIM_READSTATE_SMS` | 1 | | +| `SIM_READSTATE_SKIP` | 2 | | + +--- +## `simTelemetryState_e` + +> Source: ../../../src/main/telemetry/sim.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `SIM_STATE_INIT` | 0 | | +| `SIM_STATE_INIT2` | 1 | | +| `SIM_STATE_INIT_ENTER_PIN` | 2 | | +| `SIM_STATE_SET_MODES` | 3 | | +| `SIM_STATE_SEND_SMS` | 4 | | +| `SIM_STATE_SEND_SMS_ENTER_MESSAGE` | 5 | | + +--- +## `simTransmissionState_e` + +> Source: ../../../src/main/telemetry/sim.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `SIM_TX_NO` | 0 | | +| `SIM_TX_FS` | 1 | | +| `SIM_TX` | 2 | | + +--- +## `simTxFlags_e` + +> Source: ../../../src/main/telemetry/sim.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SIM_TX_FLAG` | (1 << 0) | | +| `SIM_TX_FLAG_FAILSAFE` | (1 << 1) | | +| `SIM_TX_FLAG_GPS` | (1 << 2) | | +| `SIM_TX_FLAG_ACC` | (1 << 3) | | +| `SIM_TX_FLAG_LOW_ALT` | (1 << 4) | | +| `SIM_TX_FLAG_RESPONSE` | (1 << 5) | | + +--- +## `simulatorFlags_t` + +> Source: ../../../src/main/fc/runtime_config.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `HITL_RESET_FLAGS` | (0 << 0) | | +| `HITL_ENABLE` | (1 << 0) | | +| `HITL_SIMULATE_BATTERY` | (1 << 1) | | +| `HITL_MUTE_BEEPER` | (1 << 2) | | +| `HITL_USE_IMU` | (1 << 3) | | +| `HITL_HAS_NEW_GPS_DATA` | (1 << 4) | | +| `HITL_EXT_BATTERY_VOLTAGE` | (1 << 5) | | +| `HITL_AIRSPEED` | (1 << 6) | | +| `HITL_EXTENDED_FLAGS` | (1 << 7) | | +| `HITL_GPS_TIMEOUT` | (1 << 8) | | +| `HITL_PITOT_FAILURE` | (1 << 9) | | + +--- +## `smartAudioVersion_e` + +> Source: ../../../src/main/io/vtx_smartaudio.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SA_UNKNOWN` | 0 | | +| `SA_1_0` | 1 | | +| `SA_2_0` | 2 | | +| `SA_2_1` | 3 | | + +--- +## `smartportFuelUnit_e` + +> Source: ../../../src/main/telemetry/telemetry.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SMARTPORT_FUEL_UNIT_PERCENT` | 0 | | +| `SMARTPORT_FUEL_UNIT_MAH` | 1 | | +| `SMARTPORT_FUEL_UNIT_MWH` | 2 | | + +--- +## `softSerialPortIndex_e` + +> Source: ../../../src/main/drivers/serial_softserial.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SOFTSERIAL1` | 0 | | +| `SOFTSERIAL2` | 1 | | + +--- +## `SPIClockSpeed_e` + +> Source: ../../../src/main/drivers/bus_spi.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SPI_CLOCK_INITIALIZATON` | 0 | | +| `SPI_CLOCK_SLOW` | 1 | | +| `SPI_CLOCK_STANDARD` | 2 | | +| `SPI_CLOCK_FAST` | 3 | | +| `SPI_CLOCK_ULTRAFAST` | 4 | | + +--- +## `Srxl2BindRequest` + +> Source: ../../../src/main/rx/srxl2_types.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `EnterBindMode` | 235 | | +| `RequestBindStatus` | 181 | | +| `BoundDataReport` | 219 | | +| `SetBindInfo` | 91 | | + +--- +## `Srxl2BindType` + +> Source: ../../../src/main/rx/srxl2_types.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NotBound` | 0 | | +| `DSM2_1024_22ms` | 1 | | +| `DSM2_1024_MC24` | 2 | | +| `DMS2_2048_11ms` | 18 | | +| `DMSX_22ms` | 162 | | +| `DMSX_11ms` | 178 | | +| `Surface_DSM2_16_5ms` | 99 | | +| `DSMR_11ms_22ms` | 226 | | +| `DSMR_5_5ms` | 228 | | + +--- +## `Srxl2ControlDataCommand` + +> Source: ../../../src/main/rx/srxl2_types.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `ChannelData` | 0 | | +| `FailsafeChannelData` | 1 | | +| `VTXData` | 2 | | + +--- +## `Srxl2DeviceId` + +> Source: ../../../src/main/rx/srxl2_types.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FlightControllerDefault` | 48 | | +| `FlightControllerMax` | 63 | | +| `Broadcast` | 255 | | + +--- +## `Srxl2DeviceType` + +> Source: ../../../src/main/rx/srxl2_types.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NoDevice` | 0 | | +| `RemoteReceiver` | 1 | | +| `Receiver` | 2 | | +| `FlightController` | 3 | | +| `ESC` | 4 | | +| `Reserved` | 5 | | +| `SRXLServo` | 6 | | +| `SRXLServo_2` | 7 | | +| `VTX` | 8 | | + +--- +## `Srxl2PacketType` + +> Source: ../../../src/main/rx/srxl2_types.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `Handshake` | 33 | | +| `BindInfo` | 65 | | +| `ParameterConfiguration` | 80 | | +| `SignalQuality` | 85 | | +| `TelemetrySensorData` | 128 | | +| `ControlData` | 205 | | + +--- +## `Srxl2State` + +> Source: ../../../src/main/rx/srxl2_types.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `Disabled` | 0 | | +| `ListenForActivity` | 1 | | +| `SendHandshake` | 2 | | +| `ListenForHandshake` | 3 | | +| `Running` | 4 | | + +--- +## `stateFlags_t` + +> Source: ../../../src/main/fc/runtime_config.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `GPS_FIX_HOME` | (1 << 0) | | +| `GPS_FIX` | (1 << 1) | | +| `CALIBRATE_MAG` | (1 << 2) | | +| `SMALL_ANGLE` | (1 << 3) | | +| `FIXED_WING_LEGACY` | (1 << 4) | | +| `ANTI_WINDUP` | (1 << 5) | | +| `FLAPERON_AVAILABLE` | (1 << 6) | | +| `NAV_MOTOR_STOP_OR_IDLE` | (1 << 7) | | +| `COMPASS_CALIBRATED` | (1 << 8) | | +| `ACCELEROMETER_CALIBRATED` | (1 << 9) | | +| `GPS_ESTIMATED_FIX` | (1 << 10) | USE_GPS_FIX_ESTIMATION | +| `NAV_CRUISE_BRAKING` | (1 << 11) | | +| `NAV_CRUISE_BRAKING_BOOST` | (1 << 12) | | +| `NAV_CRUISE_BRAKING_LOCKED` | (1 << 13) | | +| `NAV_EXTRA_ARMING_SAFETY_BYPASSED` | (1 << 14) | | +| `AIRMODE_ACTIVE` | (1 << 15) | | +| `ESC_SENSOR_ENABLED` | (1 << 16) | | +| `AIRPLANE` | (1 << 17) | | +| `MULTIROTOR` | (1 << 18) | | +| `ROVER` | (1 << 19) | | +| `BOAT` | (1 << 20) | | +| `ALTITUDE_CONTROL` | (1 << 21) | | +| `MOVE_FORWARD_ONLY` | (1 << 22) | | +| `SET_REVERSIBLE_MOTORS_FORWARD` | (1 << 23) | | +| `FW_HEADING_USE_YAW` | (1 << 24) | | +| `ANTI_WINDUP_DEACTIVATED` | (1 << 25) | | +| `LANDING_DETECTED` | (1 << 26) | | +| `IN_FLIGHT_EMERG_REARM` | (1 << 27) | | +| `TAILSITTER` | (1 << 28) | | + +--- +## `stickPositions_e` + +> Source: ../../../src/main/fc/rc_controls.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `ROL_LO` | (1 << (2 * ROLL)) | | +| `ROL_CE` | (3 << (2 * ROLL)) | | +| `ROL_HI` | (2 << (2 * ROLL)) | | +| `PIT_LO` | (1 << (2 * PITCH)) | | +| `PIT_CE` | (3 << (2 * PITCH)) | | +| `PIT_HI` | (2 << (2 * PITCH)) | | +| `YAW_LO` | (1 << (2 * YAW)) | | +| `YAW_CE` | (3 << (2 * YAW)) | | +| `YAW_HI` | (2 << (2 * YAW)) | | +| `THR_LO` | (1 << (2 * THROTTLE)) | | +| `THR_CE` | (3 << (2 * THROTTLE)) | | +| `THR_HI` | (2 << (2 * THROTTLE)) | | + +--- +## `systemState_e` + +> Source: ../../../src/main/fc/fc_init.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `SYSTEM_STATE_INITIALISING` | 0 | | +| `SYSTEM_STATE_CONFIG_LOADED` | (1 << 0) | | +| `SYSTEM_STATE_SENSORS_READY` | (1 << 1) | | +| `SYSTEM_STATE_MOTORS_READY` | (1 << 2) | | +| `SYSTEM_STATE_TRANSPONDER_ENABLED` | (1 << 3) | | +| `SYSTEM_STATE_READY` | (1 << 7) | | + +--- +## `systemState_e` + +> Source: ../../../src/main/fc/fc_init.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `SYSTEM_STATE_INITIALISING` | 0 | | +| `SYSTEM_STATE_CONFIG_LOADED` | (1 << 0) | | +| `SYSTEM_STATE_SENSORS_READY` | (1 << 1) | | +| `SYSTEM_STATE_MOTORS_READY` | (1 << 2) | | +| `SYSTEM_STATE_TRANSPONDER_ENABLED` | (1 << 3) | | +| `SYSTEM_STATE_READY` | (1 << 7) | | + +--- +## `tchDmaState_e` + +> Source: ../../../src/main/drivers/timer.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `TCH_DMA_IDLE` | 0 | | +| `TCH_DMA_READY` | 1 | | +| `TCH_DMA_ACTIVE` | 2 | | + +--- +## `tempSensorType_e` + +> Source: ../../../src/main/sensors/temperature.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `TEMP_SENSOR_NONE` | 0 | | +| `TEMP_SENSOR_LM75` | 1 | | +| `TEMP_SENSOR_DS18B20` | 2 | | + +--- +## `throttleStatus_e` + +> Source: ../../../src/main/fc/rc_controls.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `THROTTLE_LOW` | 0 | | +| `THROTTLE_HIGH` | 1 | | + +--- +## `throttleStatusType_e` + +> Source: ../../../src/main/fc/rc_controls.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `THROTTLE_STATUS_TYPE_RC` | 0 | | +| `THROTTLE_STATUS_TYPE_COMMAND` | 1 | | + +--- +## `timerMode_e` + +> Source: ../../../src/main/drivers/serial_softserial.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `TIMER_MODE_SINGLE` | 0 | | +| `TIMER_MODE_DUAL` | 1 | | + +--- +## `timerUsageFlag_e` + +> Source: ../../../src/main/drivers/timer.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `TIM_USE_ANY` | 0 | | +| `TIM_USE_PPM` | (1 << 0) | | +| `TIM_USE_PWM` | (1 << 1) | | +| `TIM_USE_MOTOR` | (1 << 2) | | +| `TIM_USE_SERVO` | (1 << 3) | | +| `TIM_USE_MC_CHNFW` | (1 << 4) | | +| `TIM_USE_LED` | (1 << 24) | | +| `TIM_USE_BEEPER` | (1 << 25) | | + +--- +## `timId_e` + +> Source: ../../../src/main/io/ledstrip.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `timBlink` | 0 | | +| `timLarson` | 1 | | +| `timBattery` | 2 | | +| `timRssi` | 3 | | +| `timGps` | (4) | USE_GPS | +| `timWarning` | 5 | | +| `timIndicator` | 6 | | +| `timAnimation` | (7) | USE_LED_ANIMATION | +| `timRing` | 8 | | +| `timTimerCount` | 9 | | + +--- +## `tristate_e` + +> Source: ../../../src/main/common/tristate.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `TRISTATE_AUTO` | 0 | | +| `TRISTATE_ON` | 1 | | +| `TRISTATE_OFF` | 2 | | + +--- +## `tz_automatic_dst_e` + +> Source: ../../../src/main/common/time.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `TZ_AUTO_DST_OFF` | 0 | | +| `TZ_AUTO_DST_EU` | 1 | | +| `TZ_AUTO_DST_USA` | 2 | | + +--- +## `UARTDevice_e` + +> Source: ../../../src/main/drivers/serial_uart.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `UARTDEV_1` | 0 | | +| `UARTDEV_2` | 1 | | +| `UARTDEV_3` | 2 | | +| `UARTDEV_4` | 3 | | +| `UARTDEV_5` | 4 | | +| `UARTDEV_6` | 5 | | +| `UARTDEV_7` | 6 | | +| `UARTDEV_8` | 7 | | +| `UARTDEV_MAX` | 8 | | + +--- +## `uartInverterLine_e` + +> Source: ../../../src/main/drivers/uart_inverter.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `UART_INVERTER_LINE_NONE` | 0 | | +| `UART_INVERTER_LINE_RX` | 1 << 0 | | +| `UART_INVERTER_LINE_TX` | 1 << 1 | | + +--- +## `ublox_nav_sig_health_e` + +> Source: ../../../src/main/io/gps_ublox.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `UBLOX_SIG_HEALTH_UNKNOWN` | 0 | | +| `UBLOX_SIG_HEALTH_HEALTHY` | 1 | | +| `UBLOX_SIG_HEALTH_UNHEALTHY` | 2 | | + +--- +## `ublox_nav_sig_quality` + +> Source: ../../../src/main/io/gps_ublox.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `UBLOX_SIG_QUALITY_NOSIGNAL` | 0 | | +| `UBLOX_SIG_QUALITY_SEARCHING` | 1 | | +| `UBLOX_SIG_QUALITY_ACQUIRED` | 2 | | +| `UBLOX_SIG_QUALITY_UNUSABLE` | 3 | | +| `UBLOX_SIG_QUALITY_CODE_LOCK_TIME_SYNC` | 4 | | +| `UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC` | 5 | | +| `UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC2` | 6 | | +| `UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC3` | 7 | | + +--- +## `ubs_nav_fix_type_t` + +> Source: ../../../src/main/io/gps_ublox.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FIX_NONE` | 0 | | +| `FIX_DEAD_RECKONING` | 1 | | +| `FIX_2D` | 2 | | +| `FIX_3D` | 3 | | +| `FIX_GPS_DEAD_RECKONING` | 4 | | +| `FIX_TIME` | 5 | | + +--- +## `ubx_ack_state_t` + +> Source: ../../../src/main/io/gps_ublox.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `UBX_ACK_WAITING` | 0 | | +| `UBX_ACK_GOT_ACK` | 1 | | +| `UBX_ACK_GOT_NAK` | 2 | | + +--- +## `ubx_nav_status_bits_t` + +> Source: ../../../src/main/io/gps_ublox.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `NAV_STATUS_FIX_VALID` | 1 | | + +--- +## `ubx_protocol_bytes_t` + +> Source: ../../../src/main/io/gps_ublox.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `PREAMBLE1` | 181 | | +| `PREAMBLE2` | 98 | | +| `CLASS_NAV` | 1 | | +| `CLASS_ACK` | 5 | | +| `CLASS_CFG` | 6 | | +| `CLASS_MON` | 10 | | +| `MSG_CLASS_UBX` | 1 | | +| `MSG_CLASS_NMEA` | 240 | | +| `MSG_VER` | 4 | | +| `MSG_ACK_NACK` | 0 | | +| `MSG_ACK_ACK` | 1 | | +| `MSG_NMEA_GGA` | 0 | | +| `MSG_NMEA_GLL` | 1 | | +| `MSG_NMEA_GSA` | 2 | | +| `MSG_NMEA_GSV` | 3 | | +| `MSG_NMEA_RMC` | 4 | | +| `MSG_NMEA_VGS` | 5 | | +| `MSG_POSLLH` | 2 | | +| `MSG_STATUS` | 3 | | +| `MSG_SOL` | 6 | | +| `MSG_PVT` | 7 | | +| `MSG_VELNED` | 18 | | +| `MSG_TIMEUTC` | 33 | | +| `MSG_SVINFO` | 48 | | +| `MSG_NAV_SAT` | 53 | | +| `MSG_CFG_PRT` | 0 | | +| `MSG_CFG_RATE` | 8 | | +| `MSG_CFG_SET_RATE` | 1 | | +| `MSG_CFG_NAV_SETTINGS` | 36 | | +| `MSG_CFG_SBAS` | 22 | | +| `MSG_CFG_GNSS` | 62 | | +| `MSG_MON_GNSS` | 40 | | +| `MSG_NAV_SIG` | 67 | | + +--- +## `vcselPeriodType_e` + +> Source: ../../../src/main/drivers/rangefinder/rangefinder_vl53l0x.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `VcselPeriodPreRange` | 0 | | +| `VcselPeriodFinalRange` | 1 | | + +--- +## `videoSystem_e` + +> Source: ../../../src/main/drivers/osd.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `VIDEO_SYSTEM_AUTO` | 0 | | +| `VIDEO_SYSTEM_PAL` | 1 | | +| `VIDEO_SYSTEM_NTSC` | 2 | | +| `VIDEO_SYSTEM_HDZERO` | 3 | | +| `VIDEO_SYSTEM_DJIWTF` | 4 | | +| `VIDEO_SYSTEM_AVATAR` | 5 | | +| `VIDEO_SYSTEM_DJICOMPAT` | 6 | | +| `VIDEO_SYSTEM_DJICOMPAT_HD` | 7 | | +| `VIDEO_SYSTEM_DJI_NATIVE` | 8 | | + +--- +## `voltageSensor_e` + +> Source: ../../../src/main/sensors/battery_config_structs.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `VOLTAGE_SENSOR_NONE` | 0 | | +| `VOLTAGE_SENSOR_ADC` | 1 | | +| `VOLTAGE_SENSOR_ESC` | 2 | | +| `VOLTAGE_SENSOR_FAKE` | 3 | | +| `VOLTAGE_SENSOR_SMARTPORT` | 4 | | +| `VOLTAGE_SENSOR_MAX` | VOLTAGE_SENSOR_SMARTPORT | | + +--- +## `vs600Band_e` + +> Source: ../../../src/main/io/smartport_master.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `VS600_BAND_A` | 0 | | +| `VS600_BAND_B` | 1 | | +| `VS600_BAND_C` | 2 | | +| `VS600_BAND_D` | 3 | | +| `VS600_BAND_E` | 4 | | +| `VS600_BAND_F` | 5 | | + +--- +## `vs600Power_e` + +> Source: ../../../src/main/io/smartport_master.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `VS600_POWER_PIT` | 0 | | +| `VS600_POWER_25MW` | 1 | | +| `VS600_POWER_200MW` | 2 | | +| `VS600_POWER_600MW` | 3 | | + +--- +## `vtxDevType_e` + +> Source: ../../../src/main/drivers/vtx_common.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `VTXDEV_UNSUPPORTED` | 0 | | +| `VTXDEV_RTC6705` | 1 | | +| `VTXDEV_SMARTAUDIO` | 3 | | +| `VTXDEV_TRAMP` | 4 | | +| `VTXDEV_FFPV` | 5 | | +| `VTXDEV_MSP` | 6 | | +| `VTXDEV_UNKNOWN` | 255 | | + +--- +## `vtxFrequencyGroups_e` + +> Source: ../../../src/main/drivers/vtx_common.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `FREQUENCYGROUP_5G8` | 0 | | +| `FREQUENCYGROUP_2G4` | 1 | | +| `FREQUENCYGROUP_1G3` | 2 | | + +--- +## `vtxLowerPowerDisarm_e` + +> Source: ../../../src/main/io/vtx.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `VTX_LOW_POWER_DISARM_OFF` | 0 | | +| `VTX_LOW_POWER_DISARM_ALWAYS` | 1 | | +| `VTX_LOW_POWER_DISARM_UNTIL_FIRST_ARM` | 2 | | + +--- +## `vtxProtoResponseType_e` + +> Source: ../../../src/main/io/vtx_tramp.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `VTX_RESPONSE_TYPE_NONE` | 0 | | +| `VTX_RESPONSE_TYPE_CAPABILITIES` | 1 | | +| `VTX_RESPONSE_TYPE_STATUS` | 2 | | + +--- +## `vtxProtoState_e` + +> Source: ../../../src/main/io/vtx_tramp.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `VTX_STATE_RESET` | 0 | | +| `VTX_STATE_OFFILE` | 1 | | +| `VTX_STATE_DETECTING` | 2 | | +| `VTX_STATE_IDLE` | 3 | | +| `VTX_STATE_QUERY_DELAY` | 4 | | +| `VTX_STATE_QUERY_STATUS` | 5 | | +| `VTX_STATE_WAIT_STATUS` | 6 | | + +--- +## `vtxScheduleParams_e` + +> Source: ../../../src/main/io/vtx.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `VTX_PARAM_POWER` | 0 | | +| `VTX_PARAM_BANDCHAN` | 1 | | +| `VTX_PARAM_PITMODE` | 2 | | +| `VTX_PARAM_COUNT` | 3 | | + +--- +## `warningFlags_e` + +> Source: ../../../src/main/io/ledstrip.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `WARNING_ARMING_DISABLED` | 0 | | +| `WARNING_LOW_BATTERY` | 1 | | +| `WARNING_FAILSAFE` | 2 | | +| `WARNING_HW_ERROR` | 3 | | + +--- +## `warningLedState_e` + +> Source: ../../../src/main/io/statusindicator.c + +| Enumerator | Value | Condition | +|---|---:|---| +| `WARNING_LED_OFF` | 0 | | +| `WARNING_LED_ON` | 1 | | +| `WARNING_LED_FLASH` | 2 | | + +--- +## `widgetAHIOptions_t` + +> Source: ../../../src/main/drivers/display_widgets.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `DISPLAY_WIDGET_AHI_OPTION_SHOW_CORNERS` | 1 << 0 | | + +--- +## `widgetAHIStyle_e` + +> Source: ../../../src/main/drivers/display_widgets.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `DISPLAY_WIDGET_AHI_STYLE_STAIRCASE` | 0 | | +| `DISPLAY_WIDGET_AHI_STYLE_LINE` | 1 | | + +--- +## `wpFwTurnSmoothing_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `WP_TURN_SMOOTHING_OFF` | 0 | | +| `WP_TURN_SMOOTHING_ON` | 1 | | +| `WP_TURN_SMOOTHING_CUT` | 2 | | + +--- +## `wpMissionPlannerStatus_e` + +> Source: ../../../src/main/navigation/navigation.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `WP_PLAN_WAIT` | 0 | | +| `WP_PLAN_SAVE` | 1 | | +| `WP_PLAN_OK` | 2 | | +| `WP_PLAN_FULL` | 3 | | + +--- +## `zeroCalibrationState_e` + +> Source: ../../../src/main/common/calibration.h + +| Enumerator | Value | Condition | +|---|---:|---| +| `ZERO_CALIBRATION_NONE` | 0 | | +| `ZERO_CALIBRATION_IN_PROGRESS` | 1 | | +| `ZERO_CALIBRATION_DONE` | 2 | | +| `ZERO_CALIBRATION_FAIL` | 3 | | diff --git a/docs/development/msp/msp_messages.checksum b/docs/development/msp/msp_messages.checksum new file mode 100644 index 00000000000..98dc134e990 --- /dev/null +++ b/docs/development/msp/msp_messages.checksum @@ -0,0 +1 @@ +7db80f38dda2265704e7852630a02a83 diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json new file mode 100644 index 00000000000..db8dbe22833 --- /dev/null +++ b/docs/development/msp/msp_messages.json @@ -0,0 +1,10928 @@ +{ + "MSP_API_VERSION": { + "code": 1, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "mspProtocolVersion", + "ctype": "uint8_t", + "desc": "MSP Protocol version (`MSP_PROTOCOL_VERSION`, typically 0)", + "units": "" + }, + { + "name": "apiVersionMajor", + "ctype": "uint8_t", + "desc": "INAV API Major version (`API_VERSION_MAJOR`)", + "units": "" + }, + { + "name": "apiVersionMinor", + "ctype": "uint8_t", + "desc": "INAV API Minor version (`API_VERSION_MINOR`)", + "units": "" + } + ] + }, + "notes": "Used by configurators to check compatibility.", + "description": "Provides the MSP protocol version and the INAV API version." + }, + "MSP_FC_VARIANT": { + "code": 2, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "fcVariantIdentifier", + "desc": "4-character identifier string (e.g., \"INAV\"). Defined by `flightControllerIdentifier`.", + "ctype": "char", + "array": true, + "array_size": 4, + "units": "" + } + ] + }, + "notes": "See `FLIGHT_CONTROLLER_IDENTIFIER_LENGTH`.", + "description": "Identifies the flight controller firmware variant (e.g., INAV, Betaflight)." + }, + "MSP_FC_VERSION": { + "code": 3, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "fcVersionMajor", + "ctype": "uint8_t", + "desc": "Firmware Major version (`FC_VERSION_MAJOR`)", + "units": "" + }, + { + "name": "fcVersionMinor", + "ctype": "uint8_t", + "desc": "Firmware Minor version (`FC_VERSION_MINOR`)", + "units": "" + }, + { + "name": "fcVersionPatch", + "ctype": "uint8_t", + "desc": "Firmware Patch level (`FC_VERSION_PATCH_LEVEL`)", + "units": "" + } + ] + }, + "notes": "", + "description": "Provides the specific version number of the flight controller firmware." + }, + "MSP_BOARD_INFO": { + "code": 4, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "boardIdentifier", + "desc": "4-character UPPER CASE board identifier (`TARGET_BOARD_IDENTIFIER`)", + "ctype": "char", + "array": true, + "array_size": 4, + "units": "" + }, + { + "name": "hardwareRevision", + "ctype": "uint16_t", + "desc": "Hardware revision number. 0 if not detected (`USE_HARDWARE_REVISION_DETECTION`)", + "units": "" + }, + { + "name": "osdSupport", + "ctype": "uint8_t", + "desc": "OSD chip type: 0=None, 2=Onboard (`USE_OSD`). INAV does not support slave OSD (1)", + "units": "" + }, + { + "name": "commCapabilities", + "ctype": "uint8_t", + "desc": "Bitmask: Communication capabilities: Bit 0=VCP support (`USE_VCP`), Bit 1=SoftSerial support (`USE_SOFTSERIAL1`/`2`)", + "units": "Bitmask", + "bitmask": true + }, + { + "name": "targetNameLength", + "ctype": "uint8_t", + "desc": "Length of the target name string that follows", + "units": "" + }, + { + "name": "targetName", + "desc": "Target name string (e.g., \"MATEKF405\"). Length given by previous field", + "ctype": "char", + "array": true, + "array_size": 0, + "units": "" + } + ] + }, + "variable_len": true, + "notes": "`BOARD_IDENTIFIER_LENGTH` is 4.", + "description": "Provides information about the specific hardware board and its capabilities." + }, + "MSP_BUILD_INFO": { + "code": 5, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "buildDate", + "desc": "Build date string (e.g., \"Dec 31 2023\"). `BUILD_DATE_LENGTH`.", + "ctype": "char", + "array": true, + "array_size": 11, + "array_size_define": "BUILD_DATE_LENGTH", + "units": "" + }, + { + "name": "buildTime", + "desc": "Build time string (e.g., \"23:59:59\"). `BUILD_TIME_LENGTH`.", + "ctype": "char", + "array": true, + "array_size": 8, + "array_size_define": "BUILD_TIME_LENGTH", + "units": "" + }, + { + "name": "gitRevision", + "desc": "Short Git revision string. `GIT_SHORT_REVISION_LENGTH`.", + "ctype": "char", + "array": true, + "array_size": 8, + "array_size_define": "GIT_SHORT_REVISION_LENGTH", + "units": "" + } + ] + }, + "notes": "", + "description": "Provides build date, time, and Git revision of the firmware." + }, + "MSP_INAV_PID": { + "code": 6, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "legacyAsyncProcessing", + "ctype": "uint8_t", + "desc": "Legacy, unused. Always 0", + "value": 0 + }, + { + "name": "legacyAsyncValue1", + "ctype": "uint16_t", + "desc": "Legacy, unused. Always 0", + "value": 0 + }, + { + "name": "legacyAsyncValue2", + "ctype": "int16_t", + "desc": "Legacy, unused. Always 0", + "value": 0 + }, + { + "name": "headingHoldRateLimit", + "ctype": "uint8_t", + "desc": "Max rate for heading hold P term (`pidProfile()->heading_hold_rate_limit`)", + "units": "deg/s" + }, + { + "name": "headingHoldLpfFreq", + "ctype": "uint8_t", + "desc": "Fixed LPF frequency for heading hold error (`HEADING_HOLD_ERROR_LPF_FREQ`)", + "units": "Hz" + }, + { + "name": "legacyYawJumpLimit", + "ctype": "int16_t", + "desc": "Legacy, unused. Always 0", + "value": 0 + }, + { + "name": "legacyGyroLpf", + "ctype": "uint8_t", + "desc": "Fixed value `GYRO_LPF_256HZ`", + "units": "Hz" + }, + { + "name": "accLpfHz", + "ctype": "uint8_t", + "desc": "Accelerometer LPF frequency (`accelerometerConfig()->acc_lpf_hz`) cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz", + "units": "Hz" + }, + { + "name": "reserved1", + "ctype": "uint8_t", + "desc": "Reserved. Always 0", + "value": 0 + }, + { + "name": "reserved2", + "ctype": "uint8_t", + "desc": "Reserved. Always 0", + "value": 0 + }, + { + "name": "reserved3", + "ctype": "uint8_t", + "desc": "Reserved. Always 0", + "value": 0 + }, + { + "name": "reserved4", + "ctype": "uint8_t", + "desc": "Reserved. Always 0", + "value": 0 + } + ] + }, + "notes": "Superseded by `MSP2_PID` for core PIDs and other specific messages for filter settings.", + "description": "Retrieves legacy INAV-specific PID controller related settings. Many fields are now obsolete or placeholders." + }, + "MSP_SET_INAV_PID": { + "code": 7, + "mspv": 1, + "request": { + "payload": [ + { + "name": "legacyAsyncProcessing", + "ctype": "uint8_t", + "desc": "Legacy, ignored" + }, + { + "name": "legacyAsyncValue1", + "ctype": "int16_t", + "desc": "Legacy, ignored" + }, + { + "name": "legacyAsyncValue2", + "ctype": "int16_t", + "desc": "Legacy, ignored" + }, + { + "name": "headingHoldRateLimit", + "ctype": "uint8_t", + "desc": "Sets `pidProfileMutable()->heading_hold_rate_limit`.", + "units": "deg/s" + }, + { + "name": "headingHoldLpfFreq", + "ctype": "uint8_t", + "desc": "Ignored (fixed value `HEADING_HOLD_ERROR_LPF_FREQ` used)", + "units": "Hz" + }, + { + "name": "legacyYawJumpLimit", + "ctype": "int16_t", + "desc": "Legacy, ignored" + }, + { + "name": "legacyGyroLpf", + "ctype": "uint8_t", + "desc": "Ignored (historically mapped to `gyro_lpf_e` values).", + "units": "" + }, + { + "name": "accLpfHz", + "ctype": "uint8_t", + "desc": "Sets `accelerometerConfigMutable()->acc_lpf_hz`.", + "units": "Hz" + }, + { + "name": "reserved1", + "ctype": "uint8_t", + "desc": "Ignored" + }, + { + "name": "reserved2", + "ctype": "uint8_t", + "desc": "Ignored" + }, + { + "name": "reserved3", + "ctype": "uint8_t", + "desc": "Ignored" + }, + { + "name": "reserved4", + "ctype": "uint8_t", + "desc": "Ignored" + } + ] + }, + "reply": null, + "notes": "Expects 15 bytes.", + "description": "Sets legacy INAV-specific PID controller related settings." + }, + "MSP_NAME": { + "code": 10, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "craftName", + "desc": "The craft name string (`systemConfig()->craftName`). Null termination is *not* explicitly sent, the length is determined by the payload size", + "ctype": "char", + "array": true, + "array_size": 0, + "units": "" + } + ] + }, + "variable_len": true, + "notes": "", + "description": "Returns the user-defined craft name." + }, + "MSP_SET_NAME": { + "code": 11, + "mspv": 1, + "request": { + "payload": [ + { + "name": "craftName", + "desc": "The new craft name string. Automatically null-terminated by the FC", + "ctype": "char", + "array": true, + "array_size": 0, + "units": "" + } + ] + }, + "reply": null, + "variable_len": true, + "notes": "Maximum length is `MAX_NAME_LENGTH`.", + "description": "Sets the user-defined craft name." + }, + "MSP_NAV_POSHOLD": { + "code": 12, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "userControlMode", + "ctype": "uint8_t", + "desc": "Navigation user control mode NAV_GPS_ATTI (0) or NAV_GPS_CRUISE (1)" + }, + { + "name": "maxAutoSpeed", + "ctype": "uint16_t", + "desc": "Max speed in autonomous modes (`navConfig()->general.max_auto_speed`)", + "units": "cm/s" + }, + { + "name": "maxAutoClimbRate", + "ctype": "uint16_t", + "desc": "Max climb rate in autonomous modes (uses `fw.max_auto_climb_rate` or `mc.max_auto_climb_rate` based on platform)", + "units": "cm/s" + }, + { + "name": "maxManualSpeed", + "ctype": "uint16_t", + "desc": "Max speed in manual modes with GPS aiding (`navConfig()->general.max_manual_speed`)", + "units": "cm/s" + }, + { + "name": "maxManualClimbRate", + "ctype": "uint16_t", + "desc": "Max climb rate in manual modes with GPS aiding (uses `fw.max_manual_climb_rate` or `mc.max_manual_climb_rate`)", + "units": "cm/s" + }, + { + "name": "mcMaxBankAngle", + "ctype": "uint8_t", + "desc": "Max bank angle for multirotor position hold (`navConfig()->mc.max_bank_angle`)", + "units": "degrees" + }, + { + "name": "mcAltHoldThrottleType", + "ctype": "uint8_t", + "desc": "Enum `navMcAltHoldThrottle_e` mirrored from `navConfig()->mc.althold_throttle_type`.", + "units": "Enum", + "enum": "navMcAltHoldThrottle_e" + }, + { + "name": "mcHoverThrottle", + "ctype": "uint16_t", + "desc": "Multirotor hover throttle PWM value (`currentBatteryProfile->nav.mc.hover_throttle`).", + "units": "PWM" + } + ] + }, + "notes": "", + "description": "Retrieves navigation position hold and general manual/auto flight parameters. Some parameters depend on the platform type (Multirotor vs Fixed Wing)." + }, + "MSP_SET_NAV_POSHOLD": { + "code": 13, + "mspv": 1, + "request": { + "payload": [ + { + "name": "userControlMode", + "ctype": "uint8_t", + "desc": "Sets `navConfigMutable()->general.flags.user_control_mode`. WARNING: uses unnamed enum in navigation.h 'NAV_GPS_ATTI/NAV_GPS_CRUISE'", + "units": "Enum", + "enum": "navUserControlMode_e" + }, + { + "name": "maxAutoSpeed", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->general.max_auto_speed`.", + "units": "cm/s" + }, + { + "name": "maxAutoClimbRate", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->fw.max_auto_climb_rate` or `navConfigMutable()->mc.max_auto_climb_rate` based on `mixerConfig()->platformType`.", + "units": "cm/s" + }, + { + "name": "maxManualSpeed", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->general.max_manual_speed`.", + "units": "cm/s" + }, + { + "name": "maxManualClimbRate", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->fw.max_manual_climb_rate` or `navConfigMutable()->mc.max_manual_climb_rate`.", + "units": "cm/s" + }, + { + "name": "mcMaxBankAngle", + "ctype": "uint8_t", + "desc": "Sets `navConfigMutable()->mc.max_bank_angle`.", + "units": "degrees" + }, + { + "name": "mcAltHoldThrottleType", + "ctype": "uint8_t", + "desc": "Enum `navMcAltHoldThrottle_e`; updates `navConfigMutable()->mc.althold_throttle_type`.", + "units": "Enum", + "enum": "navMcAltHoldThrottle_e" + }, + { + "name": "mcHoverThrottle", + "ctype": "uint16_t", + "desc": "Sets `currentBatteryProfileMutable->nav.mc.hover_throttle`.", + "units": "PWM" + } + ] + }, + "reply": null, + "notes": "Expects 13 bytes.", + "description": "Sets navigation position hold and general manual/auto flight parameters." + }, + "MSP_CALIBRATION_DATA": { + "code": 14, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "accCalibAxisFlags", + "ctype": "uint8_t", + "desc": "Bitmask: Flags indicating which axes of the accelerometer have been calibrated (`accGetCalibrationAxisFlags()`)", + "units": "Bitmask", + "bitmask": true + }, + { + "name": "accZeroX", + "ctype": "int16_t", + "desc": "Accelerometer zero offset for X-axis (`accelerometerConfig()->accZero.raw[X]`)", + "units": "Raw ADC" + }, + { + "name": "accZeroY", + "ctype": "int16_t", + "desc": "Accelerometer zero offset for Y-axis (`accelerometerConfig()->accZero.raw[Y]`)", + "units": "Raw ADC" + }, + { + "name": "accZeroZ", + "ctype": "int16_t", + "desc": "Accelerometer zero offset for Z-axis (`accelerometerConfig()->accZero.raw[Z]`)", + "units": "Raw ADC" + }, + { + "name": "accGainX", + "ctype": "int16_t", + "desc": "Accelerometer gain/scale for X-axis (`accelerometerConfig()->accGain.raw[X]`)", + "units": "Raw ADC" + }, + { + "name": "accGainY", + "ctype": "int16_t", + "desc": "Accelerometer gain/scale for Y-axis (`accelerometerConfig()->accGain.raw[Y]`)", + "units": "Raw ADC" + }, + { + "name": "accGainZ", + "ctype": "int16_t", + "desc": "Accelerometer gain/scale for Z-axis (`accelerometerConfig()->accGain.raw[Z]`)", + "units": "Raw ADC" + }, + { + "name": "magZeroX", + "ctype": "int16_t", + "desc": "Magnetometer zero offset for X-axis (`compassConfig()->magZero.raw[X]`). 0 if `USE_MAG` disabled", + "units": "Raw ADC" + }, + { + "name": "magZeroY", + "ctype": "int16_t", + "desc": "Magnetometer zero offset for Y-axis (`compassConfig()->magZero.raw[Y]`). 0 if `USE_MAG` disabled", + "units": "Raw ADC" + }, + { + "name": "magZeroZ", + "ctype": "int16_t", + "desc": "Magnetometer zero offset for Z-axis (`compassConfig()->magZero.raw[Z]`). 0 if `USE_MAG` disabled", + "units": "Raw ADC" + }, + { + "name": "opflowScale", + "ctype": "uint16_t", + "desc": "Optical flow scale factor (`opticalFlowConfig()->opflow_scale * 256`). 0 if `USE_OPFLOW` disabled", + "units": "Scale * 256" + }, + { + "name": "magGainX", + "ctype": "int16_t", + "desc": "Magnetometer gain/scale for X-axis (`compassConfig()->magGain[X]`). 0 if `USE_MAG` disabled", + "units": "Raw ADC" + }, + { + "name": "magGainY", + "ctype": "int16_t", + "desc": "Magnetometer gain/scale for Y-axis (`compassConfig()->magGain[Y]`). 0 if `USE_MAG` disabled", + "units": "Raw ADC" + }, + { + "name": "magGainZ", + "ctype": "int16_t", + "desc": "Magnetometer gain/scale for Z-axis (`compassConfig()->magGain[Z]`). 0 if `USE_MAG` disabled", + "units": "Raw ADC" + } + ] + }, + "notes": "Total size 27 bytes. Fields related to optional sensors are zero if the sensor is not used.", + "description": "Retrieves sensor calibration data (Accelerometer zero/gain, Magnetometer zero/gain, Optical Flow scale)." + }, + "MSP_SET_CALIBRATION_DATA": { + "code": 15, + "mspv": 1, + "request": { + "payload": [ + { + "name": "accZeroX", + "ctype": "int16_t", + "desc": "Sets `accelerometerConfigMutable()->accZero.raw[X]`.", + "units": "Raw ADC" + }, + { + "name": "accZeroY", + "ctype": "int16_t", + "desc": "Sets `accelerometerConfigMutable()->accZero.raw[Y]`.", + "units": "Raw ADC" + }, + { + "name": "accZeroZ", + "ctype": "int16_t", + "desc": "Sets `accelerometerConfigMutable()->accZero.raw[Z]`.", + "units": "Raw ADC" + }, + { + "name": "accGainX", + "ctype": "int16_t", + "desc": "Sets `accelerometerConfigMutable()->accGain.raw[X]`.", + "units": "Raw ADC" + }, + { + "name": "accGainY", + "ctype": "int16_t", + "desc": "Sets `accelerometerConfigMutable()->accGain.raw[Y]`.", + "units": "Raw ADC" + }, + { + "name": "accGainZ", + "ctype": "int16_t", + "desc": "Sets `accelerometerConfigMutable()->accGain.raw[Z]`.", + "units": "Raw ADC" + }, + { + "name": "magZeroX", + "ctype": "int16_t", + "desc": "Sets `compassConfigMutable()->magZero.raw[X]` (if `USE_MAG`)", + "units": "Raw ADC" + }, + { + "name": "magZeroY", + "ctype": "int16_t", + "desc": "Sets `compassConfigMutable()->magZero.raw[Y]` (if `USE_MAG`)", + "units": "Raw ADC" + }, + { + "name": "magZeroZ", + "ctype": "int16_t", + "desc": "Sets `compassConfigMutable()->magZero.raw[Z]` (if `USE_MAG`)", + "units": "Raw ADC" + }, + { + "name": "opflowScale", + "ctype": "uint16_t", + "desc": "Sets `opticalFlowConfigMutable()->opflow_scale = value / 256.0f` (if `USE_OPFLOW`)", + "units": "Scale * 256" + }, + { + "name": "magGainX", + "ctype": "int16_t", + "desc": "Sets `compassConfigMutable()->magGain[X]` (if `USE_MAG`)", + "units": "Raw ADC" + }, + { + "name": "magGainY", + "ctype": "int16_t", + "desc": "Sets `compassConfigMutable()->magGain[Y]` (if `USE_MAG`)", + "units": "Raw ADC" + }, + { + "name": "magGainZ", + "ctype": "int16_t", + "desc": "Sets `compassConfigMutable()->magGain[Z]` (if `USE_MAG`)", + "units": "Raw ADC" + } + ] + }, + "reply": null, + "notes": "Minimum payload 18 bytes. Adds +6 bytes for magnetometer zeros, +2 for optical flow scale, and +6 for magnetometer gains when those features (`USE_MAG`, `USE_OPFLOW`) are compiled in.", + "description": "Sets sensor calibration data." + }, + "MSP_POSITION_ESTIMATION_CONFIG": { + "code": 16, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "weightZBaroP", + "ctype": "uint16_t", + "desc": "Barometer Z position fusion weight (`positionEstimationConfig()->w_z_baro_p * 100`)", + "units": "Weight * 100" + }, + { + "name": "weightZGPSP", + "ctype": "uint16_t", + "desc": "GPS Z position fusion weight (`positionEstimationConfig()->w_z_gps_p * 100`)", + "units": "Weight * 100" + }, + { + "name": "weightZGPSV", + "ctype": "uint16_t", + "desc": "GPS Z velocity fusion weight (`positionEstimationConfig()->w_z_gps_v * 100`)", + "units": "Weight * 100" + }, + { + "name": "weightXYGPSP", + "ctype": "uint16_t", + "desc": "GPS XY position fusion weight (`positionEstimationConfig()->w_xy_gps_p * 100`)", + "units": "Weight * 100" + }, + { + "name": "weightXYGPSV", + "ctype": "uint16_t", + "desc": "GPS XY velocity fusion weight (`positionEstimationConfig()->w_xy_gps_v * 100`)", + "units": "Weight * 100" + }, + { + "name": "minSats", + "ctype": "uint8_t", + "desc": "Minimum satellites required for GPS use (`gpsConfigMutable()->gpsMinSats`)", + "units": "Count" + }, + { + "name": "useGPSVelNED", + "ctype": "uint8_t", + "desc": "Legacy flag, always 1 (GPS velocity is always used if available)", + "units": "Boolean", + "value": 1 + } + ] + }, + "notes": "", + "description": "Retrieves parameters related to the INAV position estimation fusion weights and GPS minimum satellite count." + }, + "MSP_SET_POSITION_ESTIMATION_CONFIG": { + "code": 17, + "mspv": 1, + "request": { + "payload": [ + { + "name": "weightZBaroP", + "ctype": "uint16_t", + "desc": "Sets `positionEstimationConfigMutable()->w_z_baro_p = value / 100.0f` (constrained 0.0-10.0)", + "units": "Weight * 100" + }, + { + "name": "weightZGPSP", + "ctype": "uint16_t", + "desc": "Sets `positionEstimationConfigMutable()->w_z_gps_p = value / 100.0f` (constrained 0.0-10.0)", + "units": "Weight * 100" + }, + { + "name": "weightZGPSV", + "ctype": "uint16_t", + "desc": "Sets `positionEstimationConfigMutable()->w_z_gps_v = value / 100.0f` (constrained 0.0-10.0)", + "units": "Weight * 100" + }, + { + "name": "weightXYGPSP", + "ctype": "uint16_t", + "desc": "Sets `positionEstimationConfigMutable()->w_xy_gps_p = value / 100.0f` (constrained 0.0-10.0)", + "units": "Weight * 100" + }, + { + "name": "weightXYGPSV", + "ctype": "uint16_t", + "desc": "Sets `positionEstimationConfigMutable()->w_xy_gps_v = value / 100.0f` (constrained 0.0-10.0)", + "units": "Weight * 100" + }, + { + "name": "minSats", + "ctype": "uint8_t", + "desc": "Sets `gpsConfigMutable()->gpsMinSats` (constrained 5-10)", + "units": "Count" + }, + { + "name": "useGPSVelNED", + "ctype": "uint8_t", + "desc": "Legacy flag, ignored", + "units": "Boolean" + } + ] + }, + "reply": null, + "notes": "Expects 12 bytes.", + "description": "Sets parameters related to the INAV position estimation fusion weights and GPS minimum satellite count." + }, + "MSP_WP_MISSION_LOAD": { + "code": 18, + "mspv": 1, + "request": { + "payload": [ + { + "name": "missionID", + "ctype": "uint8_t", + "desc": "Reserved for future use, currently ignored", + "units": "" + } + ] + }, + "reply": null, + "notes": "Only functional if `NAV_NON_VOLATILE_WAYPOINT_STORAGE` is defined. Requires 1 byte payload. Returns error if loading fails.", + "description": "Commands the FC to load the waypoint mission stored in non-volatile memory (e.g., EEPROM or FlashFS) into the active mission buffer." + }, + "MSP_WP_MISSION_SAVE": { + "code": 19, + "mspv": 1, + "request": { + "payload": [ + { + "name": "missionID", + "ctype": "uint8_t", + "desc": "Reserved for future use, currently ignored", + "units": "" + } + ] + }, + "reply": null, + "notes": "Only functional if `NAV_NON_VOLATILE_WAYPOINT_STORAGE` is defined. Requires 1 byte payload. Returns error if saving fails.", + "description": "Commands the FC to save the currently active waypoint mission from RAM to non-volatile memory (e.g., EEPROM or FlashFS)." + }, + "MSP_WP_GETINFO": { + "code": 20, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "wpCapabilities", + "ctype": "uint8_t", + "desc": "Reserved for future waypoint capabilities flags. Currently always 0", + "units": "", + "value": 0 + }, + { + "name": "maxWaypoints", + "ctype": "uint8_t", + "desc": "Maximum number of waypoints supported (`NAV_MAX_WAYPOINTS`)", + "units": "" + }, + { + "name": "missionValid", + "ctype": "uint8_t", + "desc": "Boolean flag indicating if the current mission in RAM is valid (`isWaypointListValid()`)", + "units": "" + }, + { + "name": "waypointCount", + "ctype": "uint8_t", + "desc": "Number of waypoints currently defined in the mission (`getWaypointCount()`)", + "units": "" + } + ] + }, + "notes": "", + "description": "Retrieves information about the waypoint mission capabilities and the status of the currently loaded mission." + }, + "MSP_RTH_AND_LAND_CONFIG": { + "code": 21, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "minRthDistance", + "ctype": "uint16_t", + "desc": "Minimum distance from home required for RTH to engage (`navConfig()->general.min_rth_distance`)", + "units": "cm" + }, + { + "name": "rthClimbFirst", + "ctype": "uint8_t", + "desc": "Flag: Climb to RTH altitude before returning (`navConfig()->general.flags.rth_climb_first`)", + "units": "Boolean" + }, + { + "name": "rthClimbIgnoreEmerg", + "ctype": "uint8_t", + "desc": "Flag: Climb even in emergency RTH (`navConfig()->general.flags.rth_climb_ignore_emerg`)", + "units": "Boolean" + }, + { + "name": "rthTailFirst", + "ctype": "uint8_t", + "desc": "Flag: Return tail-first during RTH (`navConfig()->general.flags.rth_tail_first`)", + "units": "Boolean" + }, + { + "name": "rthAllowLanding", + "ctype": "uint8_t", + "desc": "Flag: Allow automatic landing after RTH (`navConfig()->general.flags.rth_allow_landing`)", + "units": "Boolean" + }, + { + "name": "rthAltControlMode", + "ctype": "uint8_t", + "desc": "RTH altitude control mode (`navConfig()->general.flags.rth_alt_control_mode`). WARNING: uses unnamed enum in navigation.h:253 'NAV_RTH_NO_ALT...'", + "units": "Enum", + "enum": "navRthAltControlMode_e" + }, + { + "name": "rthAbortThreshold", + "ctype": "uint16_t", + "desc": "Distance increase threshold to abort RTH (`navConfig()->general.rth_abort_threshold`)", + "units": "cm" + }, + { + "name": "rthAltitude", + "ctype": "uint16_t", + "desc": "Target RTH altitude (`navConfig()->general.rth_altitude`)", + "units": "cm" + }, + { + "name": "landMinAltVspd", + "ctype": "uint16_t", + "desc": "Landing vertical speed at minimum slowdown altitude (`navConfig()->general.land_minalt_vspd`)", + "units": "cm/s" + }, + { + "name": "landMaxAltVspd", + "ctype": "uint16_t", + "desc": "Landing vertical speed at maximum slowdown altitude (`navConfig()->general.land_maxalt_vspd`)", + "units": "cm/s" + }, + { + "name": "landSlowdownMinAlt", + "ctype": "uint16_t", + "desc": "Altitude below which `landMinAltVspd` applies (`navConfig()->general.land_slowdown_minalt`)", + "units": "cm" + }, + { + "name": "landSlowdownMaxAlt", + "ctype": "uint16_t", + "desc": "Altitude above which `landMaxAltVspd` applies (`navConfig()->general.land_slowdown_maxalt`)", + "units": "cm" + }, + { + "name": "emergDescentRate", + "ctype": "uint16_t", + "desc": "Vertical speed during emergency landing descent (`navConfig()->general.emerg_descent_rate`)", + "units": "cm/s" + } + ] + }, + "notes": "", + "description": "Retrieves configuration parameters related to Return-to-Home (RTH) and automatic landing behaviors." + }, + "MSP_SET_RTH_AND_LAND_CONFIG": { + "code": 22, + "mspv": 1, + "request": { + "payload": [ + { + "name": "minRthDistance", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->general.min_rth_distance`.", + "units": "cm" + }, + { + "name": "rthClimbFirst", + "ctype": "uint8_t", + "desc": "Sets `navConfigMutable()->general.flags.rth_climb_first`.", + "units": "Boolean" + }, + { + "name": "rthClimbIgnoreEmerg", + "ctype": "uint8_t", + "desc": "Sets `navConfigMutable()->general.flags.rth_climb_ignore_emerg`.", + "units": "Boolean" + }, + { + "name": "rthTailFirst", + "ctype": "uint8_t", + "desc": "Sets `navConfigMutable()->general.flags.rth_tail_first`.", + "units": "Boolean" + }, + { + "name": "rthAllowLanding", + "ctype": "uint8_t", + "desc": "Sets `navConfigMutable()->general.flags.rth_allow_landing`.", + "units": "Boolean" + }, + { + "name": "rthAltControlMode", + "ctype": "uint8_t", + "desc": "Sets `navConfigMutable()->general.flags.rth_alt_control_mode`. WARNING: uses unnamed enum in navigation.h:253", + "units": "Enum", + "enum": "navRthAltControlMode_e" + }, + { + "name": "rthAbortThreshold", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->general.rth_abort_threshold`.", + "units": "cm" + }, + { + "name": "rthAltitude", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->general.rth_altitude`.", + "units": "cm" + }, + { + "name": "landMinAltVspd", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->general.land_minalt_vspd`.", + "units": "cm/s" + }, + { + "name": "landMaxAltVspd", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->general.land_maxalt_vspd`.", + "units": "cm/s" + }, + { + "name": "landSlowdownMinAlt", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->general.land_slowdown_minalt`.", + "units": "cm" + }, + { + "name": "landSlowdownMaxAlt", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->general.land_slowdown_maxalt`.", + "units": "cm" + }, + { + "name": "emergDescentRate", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->general.emerg_descent_rate`.", + "units": "cm/s" + } + ] + }, + "reply": null, + "notes": "Expects 21 bytes.", + "description": "Sets configuration parameters related to Return-to-Home (RTH) and automatic landing behaviors." + }, + "MSP_FW_CONFIG": { + "code": 23, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "cruiseThrottle", + "ctype": "uint16_t", + "desc": "Cruise throttle command (`currentBatteryProfile->nav.fw.cruise_throttle`).", + "units": "PWM" + }, + { + "name": "minThrottle", + "ctype": "uint16_t", + "desc": "Minimum throttle during autonomous flight (`currentBatteryProfile->nav.fw.min_throttle`).", + "units": "PWM" + }, + { + "name": "maxThrottle", + "ctype": "uint16_t", + "desc": "Maximum throttle during autonomous flight (`currentBatteryProfile->nav.fw.max_throttle`).", + "units": "PWM" + }, + { + "name": "maxBankAngle", + "ctype": "uint8_t", + "desc": "Maximum bank angle allowed (`navConfig()->fw.max_bank_angle`)", + "units": "degrees" + }, + { + "name": "maxClimbAngle", + "ctype": "uint8_t", + "desc": "Maximum pitch angle during climb (`navConfig()->fw.max_climb_angle`)", + "units": "degrees" + }, + { + "name": "maxDiveAngle", + "ctype": "uint8_t", + "desc": "Maximum negative pitch angle during descent (`navConfig()->fw.max_dive_angle`)", + "units": "degrees" + }, + { + "name": "pitchToThrottle", + "ctype": "uint8_t", + "desc": "Pitch-to-throttle gain (`currentBatteryProfile->nav.fw.pitch_to_throttle`); PWM microseconds per degree (10 units ≈ 1% throttle).", + "units": "us/deg" + }, + { + "name": "loiterRadius", + "ctype": "uint16_t", + "desc": "Default loiter radius (`navConfig()->fw.loiter_radius`).", + "units": "cm" + } + ] + }, + "notes": "", + "description": "Retrieves configuration parameters specific to Fixed Wing navigation." + }, + "MSP_SET_FW_CONFIG": { + "code": 24, + "mspv": 1, + "request": { + "payload": [ + { + "name": "cruiseThrottle", + "ctype": "uint16_t", + "desc": "Sets `currentBatteryProfileMutable->nav.fw.cruise_throttle`.", + "units": "PWM" + }, + { + "name": "minThrottle", + "ctype": "uint16_t", + "desc": "Sets `currentBatteryProfileMutable->nav.fw.min_throttle`.", + "units": "PWM" + }, + { + "name": "maxThrottle", + "ctype": "uint16_t", + "desc": "Sets `currentBatteryProfileMutable->nav.fw.max_throttle`.", + "units": "PWM" + }, + { + "name": "maxBankAngle", + "ctype": "uint8_t", + "desc": "Sets `navConfigMutable()->fw.max_bank_angle`.", + "units": "degrees" + }, + { + "name": "maxClimbAngle", + "ctype": "uint8_t", + "desc": "Sets `navConfigMutable()->fw.max_climb_angle`.", + "units": "degrees" + }, + { + "name": "maxDiveAngle", + "ctype": "uint8_t", + "desc": "Sets `navConfigMutable()->fw.max_dive_angle`.", + "units": "degrees" + }, + { + "name": "pitchToThrottle", + "ctype": "uint8_t", + "desc": "Sets `currentBatteryProfileMutable->nav.fw.pitch_to_throttle` (PWM microseconds per degree; 10 units ≈ 1% throttle).", + "units": "us/deg" + }, + { + "name": "loiterRadius", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->fw.loiter_radius`.", + "units": "cm" + } + ] + }, + "reply": null, + "notes": "Expects 12 bytes.", + "description": "Sets configuration parameters specific to Fixed Wing navigation." + }, + "MSP_MODE_RANGES": { + "code": 34, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "modePermanentId", + "ctype": "uint8_t", + "desc": "Permanent ID of the flight mode (maps to `boxId` via `findBoxByActiveBoxId`). 0 if entry unused", + "units": "ID" + }, + { + "name": "auxChannelIndex", + "ctype": "uint8_t", + "desc": "0-based index of the AUX channel used for activation", + "units": "Index" + }, + { + "name": "rangeStartStep", + "ctype": "uint8_t", + "desc": "Start step (0-48). Each step is 25 PWM units; 0 is <=900 and 48 is >=2100.", + "units": "step" + }, + { + "name": "rangeEndStep", + "ctype": "uint8_t", + "desc": "End step (0-48). Uses the same 25-PWM step mapping as rangeStartStep.", + "units": "step" + } + ], + "repeating": "MAX_MODE_ACTIVATION_CONDITION_COUNT" + }, + "notes": "The number of steps and mapping to PWM values depends on internal range calculations.", + "description": "Returns all defined mode activation ranges (aux channel assignments for flight modes)." + }, + "MSP_SET_MODE_RANGE": { + "code": 35, + "mspv": 1, + "request": { + "payload": [ + { + "name": "rangeIndex", + "ctype": "uint8_t", + "desc": "Index of the mode range to set (0 to `MAX_MODE_ACTIVATION_CONDITION_COUNT - 1`)", + "units": "Index" + }, + { + "name": "modePermanentId", + "ctype": "uint8_t", + "desc": "Permanent ID of the flight mode to assign", + "units": "ID" + }, + { + "name": "auxChannelIndex", + "ctype": "uint8_t", + "desc": "0-based index of the AUX channel", + "units": "Index" + }, + { + "name": "rangeStartStep", + "ctype": "uint8_t", + "desc": "Start step (0-48). Each step is 25 PWM units; 0 is <=900 and 48 is >=2100.", + "units": "step" + }, + { + "name": "rangeEndStep", + "ctype": "uint8_t", + "desc": "End step (0-48). Uses the same 25-PWM step mapping as rangeStartStep.", + "units": "step" + } + ] + }, + "reply": null, + "notes": "Expects 5 bytes. Updates the mode configuration and recalculates used mode flags. Returns error if `rangeIndex` or `modePermanentId` is invalid.", + "description": "Sets a single mode activation range by its index." + }, + "MSP_FEATURE": { + "code": 36, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "featureMask", + "ctype": "uint32_t", + "desc": "Bitmask: active features (see `featureMask()`)", + "units": "Bitmask", + "bitmask": true + } + ] + }, + "notes": "Feature bits are defined in `feature.h`.", + "description": "Returns a bitmask of enabled features." + }, + "MSP_SET_FEATURE": { + "code": 37, + "mspv": 1, + "request": { + "payload": [ + { + "name": "featureMask", + "ctype": "uint32_t", + "desc": "Bitmask: features to enable", + "units": "Bitmask", + "bitmask": true + } + ] + }, + "reply": null, + "notes": "Expects 4 bytes. Updates feature configuration and related settings (e.g., RSSI source).", + "description": "Sets the enabled features using a bitmask. Clears all previous features first." + }, + "MSP_BOARD_ALIGNMENT": { + "code": 38, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "rollAlign", + "ctype": "int16_t", + "desc": "Board alignment roll angle (`boardAlignment()->rollDeciDegrees`). Negative values tilt left.", + "units": "deci-degrees" + }, + { + "name": "pitchAlign", + "ctype": "int16_t", + "desc": "Board alignment pitch angle (`boardAlignment()->pitchDeciDegrees`). Negative values nose down.", + "units": "deci-degrees" + }, + { + "name": "yawAlign", + "ctype": "int16_t", + "desc": "Board alignment yaw angle (`boardAlignment()->yawDeciDegrees`). Negative values rotate counter-clockwise.", + "units": "deci-degrees" + } + ] + }, + "notes": "Ranges are typically -1800 to +1800 (i.e. -180.0° to +180.0°).", + "description": "Returns the sensor board alignment angles relative to the craft frame." + }, + "MSP_SET_BOARD_ALIGNMENT": { + "code": 39, + "mspv": 1, + "request": { + "payload": [ + { + "name": "rollAlign", + "ctype": "int16_t", + "desc": "Sets `boardAlignmentMutable()->rollDeciDegrees`.", + "units": "deci-degrees" + }, + { + "name": "pitchAlign", + "ctype": "int16_t", + "desc": "Sets `boardAlignmentMutable()->pitchDeciDegrees`.", + "units": "deci-degrees" + }, + { + "name": "yawAlign", + "ctype": "int16_t", + "desc": "Sets `boardAlignmentMutable()->yawDeciDegrees`.", + "units": "deci-degrees" + } + ] + }, + "reply": null, + "notes": "Expects 6 bytes encoded as little-endian signed deci-degrees (-1800 to +1800 typical).", + "description": "Sets the sensor board alignment angles." + }, + "MSP_CURRENT_METER_CONFIG": { + "code": 40, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "scale", + "ctype": "int16_t", + "desc": "Current sensor scale factor (`batteryMetersConfig()->current.scale`). Stored in 0.1 mV/A; signed for calibration.", + "units": "0.1 mV/A" + }, + { + "name": "offset", + "ctype": "int16_t", + "desc": "Current sensor offset (`batteryMetersConfig()->current.offset`). Signed millivolt adjustment.", + "units": "mV" + }, + { + "name": "type", + "ctype": "uint8_t", + "desc": "Enum `currentSensor_e` Type of current sensor hardware", + "units": "Enum", + "enum": "currentSensor_e" + }, + { + "name": "capacity", + "ctype": "uint16_t", + "desc": "Battery capacity (constrained 0-65535) (`currentBatteryProfile->capacity.value`). Note: This is legacy, use `MSP2_INAV_BATTERY_CONFIG` for full 32-bit capacity", + "units": "mAh (legacy)" + } + ] + }, + "notes": "Scale and offset are signed values matching `batteryMetersConfig()->current` fields.", + "description": "Retrieves the configuration for the current sensor." + }, + "MSP_SET_CURRENT_METER_CONFIG": { + "code": 41, + "mspv": 1, + "request": { + "payload": [ + { + "name": "scale", + "ctype": "int16_t", + "desc": "Sets `batteryMetersConfigMutable()->current.scale` (0.1 mV/A, signed).", + "units": "0.1 mV/A" + }, + { + "name": "offset", + "ctype": "int16_t", + "desc": "Sets `batteryMetersConfigMutable()->current.offset` (signed millivolts).", + "units": "mV" + }, + { + "name": "type", + "ctype": "uint8_t", + "desc": "Enum `currentSensor_e` Sets `batteryMetersConfigMutable()->current.type`.", + "units": "Enum", + "enum": "currentSensor_e" + }, + { + "name": "capacity", + "ctype": "uint16_t", + "desc": "Sets `currentBatteryProfileMutable->capacity.value` (truncated to 16 bits)", + "units": "mAh (legacy)" + } + ] + }, + "reply": null, + "notes": "Expects 7 bytes. Signed values use little-endian two's complement.", + "description": "Sets the configuration for the current sensor." + }, + "MSP_MIXER": { + "code": 42, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "mixerMode", + "ctype": "uint8_t", + "desc": "Always 3 (QuadX) in INAV for compatibility", + "units": "", + "value": 3 + } + ] + }, + "notes": "This command is largely obsolete. Mixer configuration is handled differently in INAV (presets, custom mixes). See `MSP2_INAV_MIXER`.", + "description": "Retrieves the mixer type (Legacy, INAV always returns QuadX)." + }, + "MSP_SET_MIXER": { + "code": 43, + "mspv": 1, + "request": { + "payload": [ + { + "name": "mixerMode", + "ctype": "uint8_t", + "desc": "Mixer mode to set (ignored by INAV)", + "units": "" + } + ] + }, + "reply": null, + "notes": "Expects 1 byte. Calls `mixerUpdateStateFlags()` for potential side effects related to presets.", + "description": "Sets the mixer type (Legacy, ignored by INAV)." + }, + "MSP_RX_CONFIG": { + "code": 44, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "serialRxProvider", + "ctype": "uint8_t", + "desc": "Enum `rxSerialReceiverType_e`. Serial RX provider (`rxConfig()->serialrx_provider`).", + "units": "Enum", + "enum": "rxSerialReceiverType_e" + }, + { + "name": "maxCheck", + "ctype": "uint16_t", + "desc": "Upper channel value threshold for stick commands (`rxConfig()->maxcheck`)", + "units": "PWM" + }, + { + "name": "midRc", + "ctype": "uint16_t", + "desc": "Center channel value (`PWM_RANGE_MIDDLE`, typically 1500)", + "units": "PWM" + }, + { + "name": "minCheck", + "ctype": "uint16_t", + "desc": "Lower channel value threshold for stick commands (`rxConfig()->mincheck`)", + "units": "PWM" + }, + { + "name": "spektrumSatBind", + "ctype": "uint8_t", + "desc": "Spektrum bind pulses (`rxConfig()->spektrum_sat_bind`). 0 if `USE_SPEKTRUM_BIND` disabled.", + "units": "Count/Flag" + }, + { + "name": "rxMinUsec", + "ctype": "uint16_t", + "desc": "Minimum expected pulse width (`rxConfig()->rx_min_usec`)", + "units": "PWM" + }, + { + "name": "rxMaxUsec", + "ctype": "uint16_t", + "desc": "Maximum expected pulse width (`rxConfig()->rx_max_usec`)", + "units": "PWM" + }, + { + "name": "bfCompatRcInterpolation", + "ctype": "uint8_t", + "desc": "BF compatibility. Always 0", + "value": 0 + }, + { + "name": "bfCompatRcInterpolationInt", + "ctype": "uint8_t", + "desc": "BF compatibility. Always 0", + "value": 0 + }, + { + "name": "bfCompatAirModeThreshold", + "ctype": "uint16_t", + "desc": "BF compatibility. Always 0", + "value": 0 + }, + { + "name": "reserved1", + "ctype": "uint8_t", + "desc": "Reserved/Padding. Always 0", + "value": 0 + }, + { + "name": "reserved2", + "ctype": "uint32_t", + "desc": "Reserved/Padding. Always 0", + "value": 0 + }, + { + "name": "reserved3", + "ctype": "uint8_t", + "desc": "Reserved/Padding. Always 0", + "value": 0 + }, + { + "name": "bfCompatFpvCamAngle", + "ctype": "uint8_t", + "desc": "BF compatibility. Always 0", + "value": 0 + }, + { + "name": "receiverType", + "ctype": "uint8_t", + "desc": "Enum `rxReceiverType_e` Receiver type (Parallel PWM, PPM, Serial) ('rxConfig()->receiverType')", + "units": "Enum", + "enum": "rxReceiverType_e" + } + ] + }, + "notes": "", + "description": "Retrieves receiver configuration settings. Some fields are Betaflight compatibility placeholders." + }, + "MSP_SET_RX_CONFIG": { + "code": 45, + "mspv": 1, + "request": { + "payload": [ + { + "name": "serialRxProvider", + "ctype": "uint8_t", + "desc": "Enum `rxSerialReceiverType_e`. Sets `rxConfigMutable()->serialrx_provider`.", + "units": "Enum", + "enum": "rxSerialReceiverType_e" + }, + { + "name": "maxCheck", + "ctype": "uint16_t", + "desc": "Sets `rxConfigMutable()->maxcheck`.", + "units": "PWM" + }, + { + "name": "midRc", + "ctype": "uint16_t", + "desc": "Ignored (`PWM_RANGE_MIDDLE` is used)", + "units": "PWM" + }, + { + "name": "minCheck", + "ctype": "uint16_t", + "desc": "Sets `rxConfigMutable()->mincheck`.", + "units": "PWM" + }, + { + "name": "spektrumSatBind", + "ctype": "uint8_t", + "desc": "Sets `rxConfigMutable()->spektrum_sat_bind` (if `USE_SPEKTRUM_BIND`).", + "units": "Count/Flag" + }, + { + "name": "rxMinUsec", + "ctype": "uint16_t", + "desc": "Sets `rxConfigMutable()->rx_min_usec`.", + "units": "PWM" + }, + { + "name": "rxMaxUsec", + "ctype": "uint16_t", + "desc": "Sets `rxConfigMutable()->rx_max_usec`.", + "units": "PWM" + }, + { + "name": "bfCompatRcInterpolation", + "ctype": "uint8_t", + "desc": "Ignored" + }, + { + "name": "bfCompatRcInterpolationInt", + "ctype": "uint8_t", + "desc": "Ignored" + }, + { + "name": "bfCompatAirModeThreshold", + "ctype": "uint16_t", + "desc": "Ignored" + }, + { + "name": "reserved1", + "ctype": "uint8_t", + "desc": "Ignored" + }, + { + "name": "reserved2", + "ctype": "uint32_t", + "desc": "Ignored" + }, + { + "name": "reserved3", + "ctype": "uint8_t", + "desc": "Ignored" + }, + { + "name": "bfCompatFpvCamAngle", + "ctype": "uint8_t", + "desc": "Ignored" + }, + { + "name": "receiverType", + "ctype": "uint8_t", + "desc": "Enum `rxReceiverType_e` Sets `rxConfigMutable()->receiverType`.", + "units": "Enum", + "enum": "rxReceiverType_e" + } + ] + }, + "reply": null, + "notes": "Expects 24 bytes.", + "description": "Sets receiver configuration settings." + }, + "MSP_LED_COLORS": { + "code": 46, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "hue", + "ctype": "uint16_t", + "desc": "Hue value (0-359)", + "units": "" + }, + { + "name": "saturation", + "ctype": "uint8_t", + "desc": "Saturation value (0-255)", + "units": "" + }, + { + "name": "value", + "ctype": "uint8_t", + "desc": "Value/Brightness (0-255)", + "units": "" + } + ], + "repeating": "LED_CONFIGURABLE_COLOR_COUNT" + }, + "notes": "Only available if `USE_LED_STRIP` is defined.", + "description": "Retrieves the HSV color definitions for configurable LED colors." + }, + "MSP_SET_LED_COLORS": { + "code": 47, + "mspv": 1, + "request": { + "payload": [ + { + "name": "hue", + "ctype": "uint16_t", + "desc": "Hue value (0-359)", + "units": "" + }, + { + "name": "saturation", + "ctype": "uint8_t", + "desc": "Saturation value (0-255)", + "units": "" + }, + { + "name": "value", + "ctype": "uint8_t", + "desc": "Value/Brightness (0-255)", + "units": "" + } + ], + "repeating": "LED_CONFIGURABLE_COLOR_COUNT" + }, + "reply": null, + "notes": "Only available if `USE_LED_STRIP` is defined. Expects `LED_CONFIGURABLE_COLOR_COUNT * 4` bytes.", + "description": "Sets the HSV color definitions for configurable LED colors." + }, + "MSP_LED_STRIP_CONFIG": { + "code": 48, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "legacyLedConfig", + "ctype": "uint32_t", + "desc": "Packed LED configuration (position, function, overlay, color, direction, params). See C code for bit packing details", + "units": "" + } + ], + "repeating": "LED_MAX_STRIP_LENGTH" + }, + "variable_len": "LED_MAX_STRIP_LENGTH", + "notes": "Only available if `USE_LED_STRIP` is defined. Superseded by `MSP2_INAV_LED_STRIP_CONFIG_EX` which uses a clearer struct.", + "description": "Retrieves the configuration for each LED on the strip (legacy packed format)." + }, + "MSP_SET_LED_STRIP_CONFIG": { + "code": 49, + "mspv": 1, + "request": { + "payload": [ + { + "name": "ledIndex", + "ctype": "uint8_t", + "desc": "Index of the LED to configure (0 to `LED_MAX_STRIP_LENGTH - 1`)", + "units": "" + }, + { + "name": "legacyLedConfig", + "ctype": "uint32_t", + "desc": "Packed LED configuration to set", + "units": "" + } + ] + }, + "reply": null, + "notes": "Only available if `USE_LED_STRIP` is defined. Expects 5 bytes. Calls `reevaluateLedConfig()`. Superseded by `MSP2_INAV_SET_LED_STRIP_CONFIG_EX`.", + "description": "Sets the configuration for a single LED on the strip using the legacy packed format." + }, + "MSP_RSSI_CONFIG": { + "code": 50, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "rssiChannel", + "ctype": "uint8_t", + "desc": "AUX channel index (1-based) used for RSSI, or 0 if disabled (`rxConfig()->rssi_channel`)", + "units": "" + } + ] + }, + "notes": "", + "description": "Retrieves the channel used for analog RSSI input." + }, + "MSP_SET_RSSI_CONFIG": { + "code": 51, + "mspv": 1, + "request": { + "payload": [ + { + "name": "rssiChannel", + "ctype": "uint8_t", + "desc": "AUX channel index (1-based) to use for RSSI, or 0 to disable", + "units": "" + } + ] + }, + "reply": null, + "notes": "Expects 1 byte. Input value is constrained 0 to `MAX_SUPPORTED_RC_CHANNEL_COUNT`. Updates the effective RSSI source.", + "description": "Sets the channel used for analog RSSI input." + }, + "MSP_ADJUSTMENT_RANGES": { + "code": 52, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "adjustmentIndex", + "ctype": "uint8_t", + "desc": "Index of the adjustment slot (0 to `MAX_SIMULTANEOUS_ADJUSTMENT_COUNT - 1`)", + "units": "" + }, + { + "name": "auxChannelIndex", + "ctype": "uint8_t", + "desc": "0-based index of the AUX channel controlling the adjustment value", + "units": "" + }, + { + "name": "rangeStartStep", + "ctype": "uint8_t", + "desc": "Start step (0-48). Each step is 25 PWM units; 0 is <=900 and 48 is >=2100.", + "units": "step" + }, + { + "name": "rangeEndStep", + "ctype": "uint8_t", + "desc": "End step (0-48). Uses the same 25-PWM step mapping as rangeStartStep.", + "units": "step" + }, + { + "name": "adjustmentFunction", + "ctype": "uint8_t", + "desc": "Function/parameter being adjusted (see `adjustmentFunction_e`).", + "units": "", + "enum": "adjustmentFunction_e" + }, + { + "name": "auxSwitchChannelIndex", + "ctype": "uint8_t", + "desc": "0-based index of the AUX channel acting as an enable switch (or 0 if always enabled)", + "units": "" + } + ], + "repeating": "MAX_ADJUSTMENT_RANGE_COUNT" + }, + "notes": "See `adjustmentRange_t`.", + "description": "Returns all defined RC adjustment ranges (tuning via aux channels)." + }, + "MSP_SET_ADJUSTMENT_RANGE": { + "code": 53, + "mspv": 1, + "request": { + "payload": [ + { + "name": "rangeIndex", + "ctype": "uint8_t", + "desc": "Index of the adjustment range to set (0 to `MAX_ADJUSTMENT_RANGE_COUNT - 1`)", + "units": "" + }, + { + "name": "adjustmentIndex", + "ctype": "uint8_t", + "desc": "Adjustment slot index (0 to `MAX_SIMULTANEOUS_ADJUSTMENT_COUNT - 1`)", + "units": "" + }, + { + "name": "auxChannelIndex", + "ctype": "uint8_t", + "desc": "0-based index of the control AUX channel", + "units": "" + }, + { + "name": "rangeStartStep", + "ctype": "uint8_t", + "desc": "Start step (0-48). Each step is 25 PWM units; 0 is <=900 and 48 is >=2100.", + "units": "step" + }, + { + "name": "rangeEndStep", + "ctype": "uint8_t", + "desc": "End step (0-48). Uses the same 25-PWM step mapping as rangeStartStep.", + "units": "step" + }, + { + "name": "adjustmentFunction", + "ctype": "uint8_t", + "desc": "Function/parameter being adjusted.", + "units": "", + "enum": "adjustmentFunction_e" + }, + { + "name": "auxSwitchChannelIndex", + "ctype": "uint8_t", + "desc": "0-based index of the enable switch AUX channel (or 0)", + "units": "" + } + ] + }, + "reply": null, + "notes": "Expects 7 bytes. Returns error if `rangeIndex` or `adjustmentIndex` is invalid.", + "description": "Sets a single RC adjustment range configuration by its index." + }, + "MSP_CF_SERIAL_CONFIG": { + "code": 54, + "mspv": 1, + "not_implemented": true, + "request": null, + "reply": null, + "notes": "Not implemented in INAV `fc_msp.c`. Use `MSP2_COMMON_SERIAL_CONFIG`.", + "description": "Deprecated command to get serial port configuration." + }, + "MSP_SET_CF_SERIAL_CONFIG": { + "code": 55, + "mspv": 1, + "not_implemented": true, + "request": null, + "reply": null, + "notes": "Not implemented in INAV `fc_msp.c`. Use `MSP2_COMMON_SET_SERIAL_CONFIG`.", + "description": "Deprecated command to set serial port configuration." + }, + "MSP_VOLTAGE_METER_CONFIG": { + "code": 56, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "vbatScale", + "ctype": "uint8_t", + "desc": "Voltage sensor scale factor / 10 (`batteryMetersConfig()->voltage.scale / 10`). 0 if `USE_ADC` disabled", + "units": "Scale / 10" + }, + { + "name": "vbatMinCell", + "ctype": "uint8_t", + "desc": "Minimum cell voltage / 10 (`currentBatteryProfile->voltage.cellMin / 10`). 0 if `USE_ADC` disabled", + "units": "0.1V" + }, + { + "name": "vbatMaxCell", + "ctype": "uint8_t", + "desc": "Maximum cell voltage / 10 (`currentBatteryProfile->voltage.cellMax / 10`). 0 if `USE_ADC` disabled", + "units": "0.1V" + }, + { + "name": "vbatWarningCell", + "ctype": "uint8_t", + "desc": "Warning cell voltage / 10 (`currentBatteryProfile->voltage.cellWarning / 10`). 0 if `USE_ADC` disabled", + "units": "0.1V" + } + ] + }, + "notes": "Superseded by `MSP2_INAV_BATTERY_CONFIG`.", + "description": "Retrieves legacy voltage meter configuration (scaled values)." + }, + "MSP_SET_VOLTAGE_METER_CONFIG": { + "code": 57, + "mspv": 1, + "request": { + "payload": [ + { + "name": "vbatScale", + "ctype": "uint8_t", + "desc": "Sets `batteryMetersConfigMutable()->voltage.scale = value * 10` (if `USE_ADC`)", + "units": "Scale / 10" + }, + { + "name": "vbatMinCell", + "ctype": "uint8_t", + "desc": "Sets `currentBatteryProfileMutable->voltage.cellMin = value * 10` (if `USE_ADC`)", + "units": "0.1V" + }, + { + "name": "vbatMaxCell", + "ctype": "uint8_t", + "desc": "Sets `currentBatteryProfileMutable->voltage.cellMax = value * 10` (if `USE_ADC`)", + "units": "0.1V" + }, + { + "name": "vbatWarningCell", + "ctype": "uint8_t", + "desc": "Sets `currentBatteryProfileMutable->voltage.cellWarning = value * 10` (if `USE_ADC`)", + "units": "0.1V" + } + ] + }, + "reply": null, + "notes": "Expects 4 bytes. Superseded by `MSP2_INAV_SET_BATTERY_CONFIG`.", + "description": "Sets legacy voltage meter configuration (scaled values)." + }, + "MSP_SONAR_ALTITUDE": { + "code": 58, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "rangefinderAltitude", + "ctype": "int32_t", + "desc": "Latest altitude reading from the rangefinder (`rangefinderGetLatestAltitude()`). 0 if `USE_RANGEFINDER` disabled or no reading.", + "units": "cm" + } + ] + }, + "notes": "", + "description": "Retrieves the altitude measured by the primary rangefinder (sonar or lidar)." + }, + "MSP_RX_MAP": { + "code": 64, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "rcMap", + "desc": "Array defining the mapping from input channel index to logical function (Roll, Pitch, Yaw, Throttle, Aux1...)", + "ctype": "uint8_t", + "array": true, + "array_size": 4, + "array_size_define": "MAX_MAPPABLE_RX_INPUTS", + "units": "" + } + ] + }, + "notes": "`MAX_MAPPABLE_RX_INPUTS` is currently 4 (Roll, Pitch, Yaw, Throttle).", + "description": "Retrieves the RC channel mapping array (AETR, etc.)." + }, + "MSP_SET_RX_MAP": { + "code": 65, + "mspv": 1, + "request": { + "payload": [ + { + "name": "rcMap", + "desc": "Array defining the new channel mapping", + "ctype": "uint8_t", + "array": true, + "array_size": 4, + "array_size_define": "MAX_MAPPABLE_RX_INPUTS", + "units": "" + } + ] + }, + "reply": null, + "notes": "Expects `MAX_MAPPABLE_RX_INPUTS` bytes (currently 4).", + "description": "Sets the RC channel mapping array." + }, + "MSP_REBOOT": { + "code": 68, + "mspv": 1, + "request": null, + "reply": null, + "notes": "The FC sends an ACK *before* rebooting. The `mspPostProcessFn` is set to `mspRebootFn` to perform the reboot after the reply is sent. Will fail if the craft is armed.", + "description": "Commands the flight controller to reboot." + }, + "MSP_DATAFLASH_SUMMARY": { + "code": 70, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "flashReady", + "ctype": "uint8_t", + "desc": "Boolean: 1 if flash chip is ready, 0 otherwise. (`flashIsReady()`). 0 if `USE_FLASHFS` disabled", + "units": "" + }, + { + "name": "sectorCount", + "ctype": "uint32_t", + "desc": "Total number of sectors on the flash chip (`geometry->sectors`). 0 if `USE_FLASHFS` disabled", + "units": "" + }, + { + "name": "totalSize", + "ctype": "uint32_t", + "desc": "Total size of the flash chip in bytes (`geometry->totalSize`). 0 if `USE_FLASHFS` disabled", + "units": "" + }, + { + "name": "usedSize", + "ctype": "uint32_t", + "desc": "Currently used size in bytes (FlashFS offset) (`flashfsGetOffset()`). 0 if `USE_FLASHFS` disabled", + "units": "" + } + ] + }, + "notes": "Requires `USE_FLASHFS`.", + "description": "Retrieves summary information about the onboard dataflash chip (if present and used for Blackbox via FlashFS)." + }, + "MSP_DATAFLASH_READ": { + "code": 71, + "mspv": 1, + "request": { + "payload": [ + { + "name": "address", + "ctype": "uint32_t", + "desc": "Starting address to read from within the FlashFS volume", + "units": "" + }, + { + "name": "size", + "ctype": "uint16_t", + "desc": "(Optional) Number of bytes to read. Defaults to 128 if not provided", + "units": "", + "optional": true + } + ] + }, + "reply": { + "payload": [ + { + "name": "address", + "ctype": "uint32_t", + "desc": "The starting address from which data was actually read", + "units": "" + }, + { + "name": "data", + "desc": "The data read from flash. Length is MIN(requested size, remaining buffer space, remaining flashfs data)", + "ctype": "uint8_t", + "array": true, + "array_size": 0, + "units": "" + } + ] + }, + "variable_len": true, + "notes": "Requires `USE_FLASHFS`. Read length may be truncated by buffer size or end of flashfs volume.", + "description": "Reads a block of data from the onboard dataflash (FlashFS)." + }, + "MSP_DATAFLASH_ERASE": { + "code": 72, + "mspv": 1, + "request": null, + "reply": null, + "notes": "Requires `USE_FLASHFS`. This is a potentially long operation. Use with caution.", + "description": "Erases the entire onboard dataflash chip (FlashFS volume)." + }, + "MSP_LOOP_TIME": { + "code": 73, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "looptime", + "ctype": "uint16_t", + "desc": "Configured loop time (`gyroConfig()->looptime`)", + "units": "PWM" + } + ] + }, + "notes": "This is the *configured* target loop time, not necessarily the *actual* measured cycle time (see `MSP_STATUS`).", + "description": "Retrieves the configured loop time (PID loop frequency denominator)." + }, + "MSP_SET_LOOP_TIME": { + "code": 74, + "mspv": 1, + "request": { + "payload": [ + { + "name": "looptime", + "ctype": "uint16_t", + "desc": "New loop time to set (`gyroConfigMutable()->looptime`)", + "units": "PWM" + } + ] + }, + "reply": null, + "notes": "Expects 2 bytes.", + "description": "Sets the configured loop time." + }, + "MSP_FAILSAFE_CONFIG": { + "code": 75, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "failsafeDelay", + "ctype": "uint8_t", + "desc": "Delay before failsafe stage 1 activates (`failsafeConfig()->failsafe_delay`)", + "units": "0.1s" + }, + { + "name": "failsafeOffDelay", + "ctype": "uint8_t", + "desc": "Delay after signal recovery before returning control (`failsafeConfig()->failsafe_off_delay`)", + "units": "0.1s" + }, + { + "name": "failsafeThrottle", + "ctype": "uint16_t", + "desc": "Throttle level during failsafe stage 2 (`currentBatteryProfile->failsafe_throttle`)", + "units": "PWM" + }, + { + "name": "legacyKillSwitch", + "ctype": "uint8_t", + "desc": "Legacy flag, always 0", + "value": 0 + }, + { + "name": "failsafeThrottleLowDelay", + "ctype": "uint16_t", + "desc": "Delay for throttle-based failsafe detection (`failsafeConfig()->failsafe_throttle_low_delay`). Units of 0.1 seconds.", + "units": "0.1s" + }, + { + "name": "failsafeProcedure", + "ctype": "uint8_t", + "desc": "Enum `failsafeProcedure_e` Failsafe procedure (Drop, RTH, Land, etc.) ('failsafeConfig()->failsafe_procedure')", + "units": "Enum", + "enum": "failsafeProcedure_e" + }, + { + "name": "failsafeRecoveryDelay", + "ctype": "uint8_t", + "desc": "Delay after RTH finishes before attempting recovery (`failsafeConfig()->failsafe_recovery_delay`)", + "units": "0.1s" + }, + { + "name": "failsafeFWRollAngle", + "ctype": "int16_t", + "desc": "Fixed-wing failsafe roll angle (`failsafeConfig()->failsafe_fw_roll_angle`). Signed deci-degrees.", + "units": "deci-degrees" + }, + { + "name": "failsafeFWPitchAngle", + "ctype": "int16_t", + "desc": "Fixed-wing failsafe pitch angle (`failsafeConfig()->failsafe_fw_pitch_angle`). Signed deci-degrees.", + "units": "deci-degrees" + }, + { + "name": "failsafeFWYawRate", + "ctype": "int16_t", + "desc": "Fixed-wing failsafe yaw rate (`failsafeConfig()->failsafe_fw_yaw_rate`). Signed degrees per second.", + "units": "deg/s" + }, + { + "name": "failsafeStickThreshold", + "ctype": "uint16_t", + "desc": "Stick movement threshold to exit failsafe (`failsafeConfig()->failsafe_stick_motion_threshold`)", + "units": "PWM units" + }, + { + "name": "failsafeMinDistance", + "ctype": "uint16_t", + "desc": "Minimum distance from home for RTH failsafe (`failsafeConfig()->failsafe_min_distance`). Units of centimeters.", + "units": "cm" + }, + { + "name": "failsafeMinDistanceProc", + "ctype": "uint8_t", + "desc": "Enum `failsafeProcedure_e` Failsafe procedure if below min distance ('failsafeConfig()->failsafe_min_distance_procedure')", + "units": "Enum", + "enum": "failsafeProcedure_e" + } + ] + }, + "notes": "", + "description": "Retrieves the failsafe configuration settings." + }, + "MSP_SET_FAILSAFE_CONFIG": { + "code": 76, + "mspv": 1, + "request": { + "payload": [ + { + "name": "failsafeDelay", + "ctype": "uint8_t", + "desc": "Sets `failsafeConfigMutable()->failsafe_delay`.", + "units": "0.1s" + }, + { + "name": "failsafeOffDelay", + "ctype": "uint8_t", + "desc": "Sets `failsafeConfigMutable()->failsafe_off_delay`.", + "units": "0.1s" + }, + { + "name": "failsafeThrottle", + "ctype": "uint16_t", + "desc": "Sets `currentBatteryProfileMutable->failsafe_throttle`.", + "units": "PWM" + }, + { + "name": "legacyKillSwitch", + "ctype": "uint8_t", + "desc": "Ignored" + }, + { + "name": "failsafeThrottleLowDelay", + "ctype": "uint16_t", + "desc": "Sets `failsafeConfigMutable()->failsafe_throttle_low_delay`. Units of 0.1 seconds.", + "units": "0.1s" + }, + { + "name": "failsafeProcedure", + "ctype": "uint8_t", + "desc": "Enum `failsafeProcedure_e`. Sets `failsafeConfigMutable()->failsafe_procedure`.", + "units": "Enum", + "enum": "failsafeProcedure_e" + }, + { + "name": "failsafeRecoveryDelay", + "ctype": "uint8_t", + "desc": "Sets `failsafeConfigMutable()->failsafe_recovery_delay`.", + "units": "0.1s" + }, + { + "name": "failsafeFWRollAngle", + "ctype": "int16_t", + "desc": "Sets `failsafeConfigMutable()->failsafe_fw_roll_angle`. Signed deci-degrees.", + "units": "deci-degrees" + }, + { + "name": "failsafeFWPitchAngle", + "ctype": "int16_t", + "desc": "Sets `failsafeConfigMutable()->failsafe_fw_pitch_angle`. Signed deci-degrees.", + "units": "deci-degrees" + }, + { + "name": "failsafeFWYawRate", + "ctype": "int16_t", + "desc": "Sets `failsafeConfigMutable()->failsafe_fw_yaw_rate`. Signed degrees per second.", + "units": "deg/s" + }, + { + "name": "failsafeStickThreshold", + "ctype": "uint16_t", + "desc": "Sets `failsafeConfigMutable()->failsafe_stick_motion_threshold`.", + "units": "PWM units" + }, + { + "name": "failsafeMinDistance", + "ctype": "uint16_t", + "desc": "Sets `failsafeConfigMutable()->failsafe_min_distance`. Units of centimeters.", + "units": "cm" + }, + { + "name": "failsafeMinDistanceProc", + "ctype": "uint8_t", + "desc": "Enum `failsafeProcedure_e`. Sets `failsafeConfigMutable()->failsafe_min_distance_procedure`.", + "units": "Enum", + "enum": "failsafeProcedure_e" + } + ] + }, + "reply": null, + "notes": "Expects 20 bytes.", + "description": "Sets the failsafe configuration settings." + }, + "MSP_SDCARD_SUMMARY": { + "code": 79, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "sdCardSupported", + "ctype": "uint8_t", + "desc": "Bitmask: Bit 0 = 1 if SD card support compiled in (`USE_SDCARD`)", + "units": "Bitmask", + "bitmask": true + }, + { + "name": "sdCardState", + "ctype": "uint8_t", + "desc": "Enum (`mspSDCardState_e`): Current state (Not Present, Fatal, Card Init, FS Init, Ready). 0 if `USE_SDCARD` disabled", + "units": "", + "enum": "mspSDCardState_e" + }, + { + "name": "fsError", + "ctype": "uint8_t", + "desc": "Last filesystem error code (`afatfs_getLastError()`). 0 if `USE_SDCARD` disabled", + "units": "" + }, + { + "name": "freeSpaceKB", + "ctype": "uint32_t", + "desc": "Free space in KiB (`afatfs_getContiguousFreeSpace() / 1024`). 0 if `USE_SDCARD` disabled", + "units": "" + }, + { + "name": "totalSpaceKB", + "ctype": "uint32_t", + "desc": "Total space in KiB (`sdcard_getMetadata()->numBlocks / 2`). 0 if `USE_SDCARD` disabled", + "units": "" + } + ] + }, + "notes": "Requires `USE_SDCARD` and `USE_ASYNCFATFS`.", + "description": "Retrieves summary information about the SD card status and filesystem." + }, + "MSP_BLACKBOX_CONFIG": { + "code": 80, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "blackboxDevice", + "ctype": "uint8_t", + "desc": "Always 0 (API no longer supported)", + "units": "", + "value": 0 + }, + { + "name": "blackboxRateNum", + "ctype": "uint8_t", + "desc": "Always 0", + "units": "", + "value": 0 + }, + { + "name": "blackboxRateDenom", + "ctype": "uint8_t", + "desc": "Always 0", + "units": "", + "value": 0 + }, + { + "name": "blackboxPDenom", + "ctype": "uint8_t", + "desc": "Always 0", + "units": "", + "value": 0 + } + ] + }, + "notes": "Returns fixed zero values. Use `MSP2_BLACKBOX_CONFIG`.", + "description": "Legacy command to retrieve Blackbox configuration. Superseded by `MSP2_BLACKBOX_CONFIG`." + }, + "MSP_SET_BLACKBOX_CONFIG": { + "code": 81, + "mspv": 1, + "not_implemented": true, + "request": null, + "reply": null, + "notes": "Not implemented in `fc_msp.c`. Use `MSP2_SET_BLACKBOX_CONFIG`.", + "description": "Legacy command to set Blackbox configuration. Superseded by `MSP2_SET_BLACKBOX_CONFIG`." + }, + "MSP_TRANSPONDER_CONFIG": { + "code": 82, + "mspv": 1, + "not_implemented": true, + "request": null, + "reply": null, + "notes": "Not implemented in INAV `fc_msp.c`.", + "description": "Get VTX Transponder settings (likely specific to RaceFlight/Betaflight, not standard INAV VTX)." + }, + "MSP_SET_TRANSPONDER_CONFIG": { + "code": 83, + "mspv": 1, + "not_implemented": true, + "request": null, + "reply": null, + "notes": "Not implemented in INAV `fc_msp.c`.", + "description": "Set VTX Transponder settings." + }, + "MSP_OSD_CONFIG": { + "code": 84, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "osdDriverType", + "ctype": "uint8_t", + "desc": "Enum `osdDriver_e`: `OSD_DRIVER_MAX7456` if `USE_OSD`, else `OSD_DRIVER_NONE`.", + "units": "Enum", + "enum": "osdDriver_e" + }, + { + "name": "videoSystem", + "ctype": "uint8_t", + "desc": "Enum `videoSystem_e`: Video system (Auto/PAL/NTSC) (`osdConfig()->video_system`). Sent even if OSD disabled", + "units": "Enum", + "enum": "videoSystem_e" + }, + { + "name": "units", + "ctype": "uint8_t", + "desc": "Enum `osd_unit_e` Measurement units (Metric/Imperial) (`osdConfig()->units`). Sent even if OSD disabled", + "units": "Enum", + "enum": "osd_unit_e" + }, + { + "name": "rssiAlarm", + "ctype": "uint8_t", + "desc": "RSSI alarm threshold (`osdConfig()->rssi_alarm`). Sent even if OSD disabled", + "units": "%" + }, + { + "name": "capAlarm", + "ctype": "uint16_t", + "desc": "Capacity alarm threshold (`currentBatteryProfile->capacity.warning`). Truncated to 16 bits. Sent even if OSD disabled.", + "units": "mAh/mWh" + }, + { + "name": "timerAlarm", + "ctype": "uint16_t", + "desc": "Timer alarm threshold in minutes (`osdConfig()->time_alarm`). Sent even if OSD disabled.", + "units": "minutes" + }, + { + "name": "altAlarm", + "ctype": "uint16_t", + "desc": "Altitude alarm threshold (`osdConfig()->alt_alarm`). Sent even if OSD disabled", + "units": "meters" + }, + { + "name": "distAlarm", + "ctype": "uint16_t", + "desc": "Distance alarm threshold (`osdConfig()->dist_alarm`). Sent even if OSD disabled", + "units": "meters" + }, + { + "name": "negAltAlarm", + "ctype": "uint16_t", + "desc": "Negative altitude alarm threshold (`osdConfig()->neg_alt_alarm`). Sent even if OSD disabled", + "units": "meters" + }, + { + "name": "itemPositions", + "desc": "Packed X/Y position for each OSD item on screen 0 (`osdLayoutsConfig()->item_pos[0][i]`). Sent even if OSD disabled", + "ctype": "uint16_t", + "array": true, + "array_size": 0, + "array_size_define": "OSD_ITEM_COUNT", + "units": "packed" + } + ] + }, + "variable_len": true, + "notes": "1 byte if `USE_OSD` disabled; full payload (1 + fields + 2*OSD_ITEM_COUNT bytes) otherwise.", + "description": "Retrieves OSD configuration settings and layout for screen 0. Coordinates are packed as `(Y << 8) | X`. When `USE_OSD` is not compiled in, only `osdDriverType` = `OSD_DRIVER_NONE` is returned." + }, + "MSP_SET_OSD_CONFIG": { + "code": 85, + "mspv": 1, + "request": null, + "reply": null, + "notes": "Requires `USE_OSD`. Distinguishes formats based on the first byte. Format 1 requires at least 10 bytes. Format 2 requires 3 bytes. Triggers an OSD redraw. See `MSP2_INAV_OSD_SET_*` for more advanced control.", + "description": "Sets OSD configuration or a single item's position on screen 0.", + "variants": { + "dataSize >= 10": { + "description": "dataSize >= 10", + "request": { + "payload": [ + { + "name": "selector", + "ctype": "uint8_t", + "desc": "Must be 0xFF (-1) to indicate a configuration update.", + "value": 255 + }, + { + "name": "videoSystem", + "ctype": "uint8_t", + "desc": "Enum `videoSystem_e`: Video system (Auto/PAL/NTSC) (`osdConfig()->video_system`).", + "units": "Enum", + "enum": "videoSystem_e" + }, + { + "name": "units", + "ctype": "uint8_t", + "desc": "Enum `osd_unit_e` Measurement units (Metric/Imperial) (`osdConfig()->units`).", + "units": "Enum", + "enum": "osd_unit_e" + }, + { + "name": "rssiAlarm", + "ctype": "uint8_t", + "desc": "RSSI alarm threshold (`osdConfig()->rssi_alarm`).", + "units": "%" + }, + { + "name": "capAlarm", + "ctype": "uint16_t", + "desc": "Capacity alarm threshold (`currentBatteryProfile->capacity.warning`). Truncated to 16 bits.", + "units": "mAh/mWh" + }, + { + "name": "timerAlarm", + "ctype": "uint16_t", + "desc": "Timer alarm threshold in minutes (`osdConfig()->time_alarm`).", + "units": "minutes" + }, + { + "name": "altAlarm", + "ctype": "uint16_t", + "desc": "Altitude alarm threshold (`osdConfig()->alt_alarm`).", + "units": "meters" + }, + { + "name": "distAlarm", + "ctype": "uint16_t", + "desc": "Distance alarm threshold (`osdConfig()->dist_alarm`). Optional trailing field.", + "units": "meters", + "optional": true + }, + { + "name": "negAltAlarm", + "ctype": "uint16_t", + "desc": "Negative altitude alarm threshold (`osdConfig()->neg_alt_alarm`). Optional trailing field.", + "units": "meters", + "optional": true + } + ] + }, + "reply": null + }, + "dataSize == 3": { + "description": "Single item position update", + "request": { + "payload": [ + { + "name": "itemIndex", + "ctype": "uint8_t", + "desc": "Index of the OSD item to update (0 to `OSD_ITEM_COUNT - 1`).", + "units": "Index" + }, + { + "name": "itemPosition", + "ctype": "uint16_t", + "desc": "Packed X/Y position (`(Y << 8) | X`) for the specified item.", + "units": "packed" + } + ] + }, + "reply": null + } + } + }, + "MSP_OSD_CHAR_READ": { + "code": 86, + "mspv": 1, + "not_implemented": true, + "request": null, + "reply": null, + "notes": "Not implemented in INAV `fc_msp.c`. Requires direct hardware access, typically done via DisplayPort.", + "description": "Reads character data from the OSD font memory." + }, + "MSP_OSD_CHAR_WRITE": { + "code": 87, + "mspv": 1, + "request": null, + "reply": null, + "variable_len": true, + "notes": "Requires `USE_OSD`. Minimum payload is `OSD_CHAR_VISIBLE_BYTES + 1` (8-bit address + 54 bytes). Payload size determines the address width and whether the extra metadata bytes are present. Writes characters via `displayWriteFontCharacter()`.", + "description": "Writes character data to the OSD font memory.", + "variants": { + "payloadSize >= OSD_CHAR_BYTES + 2 (>=66 bytes)": { + "description": "16-bit character index with full 64-byte payload (visible + metadata).", + "request": { + "payload": [ + { + "name": "address", + "ctype": "uint16_t", + "desc": "Character slot index (0-1023)." + }, + { + "name": "charData", + "desc": "All 64 bytes, including driver metadata.", + "ctype": "uint8_t", + "array": true, + "array_size": 64, + "array_size_define": "OSD_CHAR_BYTES" + } + ] + }, + "reply": null + }, + "payloadSize == OSD_CHAR_BYTES + 1 (65 bytes)": { + "description": "8-bit character index with full 64-byte payload.", + "request": { + "payload": [ + { + "name": "address", + "ctype": "uint8_t", + "desc": "Character slot index (0-255)." + }, + { + "name": "charData", + "desc": "All 64 bytes, including driver metadata.", + "ctype": "uint8_t", + "array": true, + "array_size": 64, + "array_size_define": "OSD_CHAR_BYTES" + } + ] + }, + "reply": null + }, + "payloadSize == OSD_CHAR_VISIBLE_BYTES + 2 (56 bytes)": { + "description": "16-bit character index with only the 54 visible bytes.", + "request": { + "payload": [ + { + "name": "address", + "ctype": "uint16_t", + "desc": "Character slot index (0-1023)." + }, + { + "name": "charData", + "desc": "Visible pixel data only (no metadata).", + "ctype": "uint8_t", + "array": true, + "array_size": 54, + "array_size_define": "OSD_CHAR_VISIBLE_BYTES" + } + ] + }, + "reply": null + }, + "payloadSize == OSD_CHAR_VISIBLE_BYTES + 1 (55 bytes)": { + "description": "8-bit character index with only the 54 visible bytes.", + "request": { + "payload": [ + { + "name": "address", + "ctype": "uint8_t", + "desc": "Character slot index (0-255)." + }, + { + "name": "charData", + "desc": "Visible pixel data only (no metadata).", + "ctype": "uint8_t", + "array": true, + "array_size": 54, + "array_size_define": "OSD_CHAR_VISIBLE_BYTES" + } + ] + }, + "reply": null + } + } + }, + "MSP_VTX_CONFIG": { + "code": 88, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "vtxDeviceType", + "ctype": "uint8_t", + "desc": "Enum (`vtxDevType_e`): Type of VTX device detected/configured. `VTXDEV_UNKNOWN` if none", + "units": "", + "enum": "vtxDevType_e" + }, + { + "name": "band", + "ctype": "uint8_t", + "desc": "VTX band number (from `vtxSettingsConfig`)", + "units": "" + }, + { + "name": "channel", + "ctype": "uint8_t", + "desc": "VTX channel number (from `vtxSettingsConfig`)", + "units": "" + }, + { + "name": "power", + "ctype": "uint8_t", + "desc": "VTX power level index (from `vtxSettingsConfig()`).", + "units": "" + }, + { + "name": "pitMode", + "ctype": "uint8_t", + "desc": "Boolean: 1 if VTX is currently in pit mode, 0 otherwise.", + "units": "" + }, + { + "name": "vtxReady", + "ctype": "uint8_t", + "desc": "Boolean: 1 if VTX device reported ready, 0 otherwise", + "units": "" + }, + { + "name": "lowPowerDisarm", + "ctype": "uint8_t", + "desc": "Enum `vtxLowerPowerDisarm_e`: Low-power behaviour while disarmed (`vtxSettingsConfig()->lowPowerDisarm`).", + "units": "Enum", + "enum": "vtxLowerPowerDisarm_e" + }, + { + "name": "vtxTableAvailable", + "ctype": "uint8_t", + "desc": "Boolean: 1 if VTX tables (band/power) are available for query", + "units": "" + }, + { + "name": "bandCount", + "ctype": "uint8_t", + "desc": "Number of bands supported by the VTX device", + "units": "" + }, + { + "name": "channelCount", + "ctype": "uint8_t", + "desc": "Number of channels per band supported by the VTX device", + "units": "" + }, + { + "name": "powerCount", + "ctype": "uint8_t", + "desc": "Number of power levels supported by the VTX device", + "units": "" + } + ] + }, + "variable_len": true, + "notes": "Returns 1 byte (`VTXDEV_UNKNOWN`) when no VTX is detected or `USE_VTX_CONTROL` is disabled; otherwise sends full payload. BF compatibility field `frequency` (uint16) is missing compared to some BF versions. Use `MSP_VTXTABLE_BAND` and `MSP_VTXTABLE_POWERLEVEL` for details.", + "description": "Retrieves the current VTX (Video Transmitter) configuration and capabilities." + }, + "MSP_SET_VTX_CONFIG": { + "code": 89, + "mspv": 1, + "request": null, + "reply": null, + "variable_len": true, + "notes": "Requires dataSize >= 2. If no VTX device or device type is VTXDEV_UNKNOWN, fields are read and discarded. The first uint16 is interpreted as band/channel when value <= VTXCOMMON_MSP_BANDCHAN_CHKVAL, otherwise treated as a frequency value that is not applied by this path. Subsequent fields are applied only if present. If dataSize < 2 the command returns MSP_RESULT_ERROR.", + "description": "Sets VTX band/channel and related options. Fields are a progressive superset based on payload length.", + "variants": { + "payloadSize >= 14": { + "description": "Full payload (Betaflight 1.42+): includes explicit band/channel/frequency and capability counts.", + "request": { + "payload": [ + { + "name": "bandChanOrFreq", + "ctype": "uint16_t", + "desc": "Encoded band/channel if <= `VTXCOMMON_MSP_BANDCHAN_CHKVAL`; otherwise frequency placeholder." + }, + { + "name": "power", + "ctype": "uint8_t" + }, + { + "name": "pitMode", + "ctype": "uint8_t" + }, + { + "name": "lowPowerDisarm", + "ctype": "uint8_t", + "enum": "vtxLowerPowerDisarm_e" + }, + { + "name": "pitModeFreq", + "ctype": "uint16_t" + }, + { + "name": "band", + "ctype": "uint8_t" + }, + { + "name": "channel", + "ctype": "uint8_t" + }, + { + "name": "frequency", + "ctype": "uint16_t" + }, + { + "name": "bandCount", + "ctype": "uint8_t", + "desc": "Read and ignored." + }, + { + "name": "channelCount", + "ctype": "uint8_t", + "desc": "Read and ignored." + }, + { + "name": "powerCount", + "ctype": "uint8_t", + "desc": "If 0 < value < current capability, caps `vtxDevice->capability.powerCount`." + } + ] + }, + "reply": null + }, + "payloadSize >= 11": { + "description": "Extends payload with explicit frequency.", + "request": { + "payload": [ + { + "name": "bandChanOrFreq", + "ctype": "uint16_t" + }, + { + "name": "power", + "ctype": "uint8_t" + }, + { + "name": "pitMode", + "ctype": "uint8_t" + }, + { + "name": "lowPowerDisarm", + "ctype": "uint8_t", + "enum": "vtxLowerPowerDisarm_e" + }, + { + "name": "pitModeFreq", + "ctype": "uint16_t" + }, + { + "name": "band", + "ctype": "uint8_t" + }, + { + "name": "channel", + "ctype": "uint8_t" + }, + { + "name": "frequency", + "ctype": "uint16_t", + "desc": "Read and ignored by INAV." + } + ] + }, + "reply": null + }, + "payloadSize >= 9": { + "description": "Adds explicit band/channel overrides (API 1.42 extension).", + "request": { + "payload": [ + { + "name": "bandChanOrFreq", + "ctype": "uint16_t" + }, + { + "name": "power", + "ctype": "uint8_t" + }, + { + "name": "pitMode", + "ctype": "uint8_t" + }, + { + "name": "lowPowerDisarm", + "ctype": "uint8_t", + "enum": "vtxLowerPowerDisarm_e" + }, + { + "name": "pitModeFreq", + "ctype": "uint16_t" + }, + { + "name": "band", + "ctype": "uint8_t", + "desc": "1..N; overrides band when present." + }, + { + "name": "channel", + "ctype": "uint8_t", + "desc": "1..8; overrides channel when present." + } + ] + }, + "reply": null + }, + "payloadSize >= 7": { + "description": "Adds pit-mode frequency placeholder.", + "request": { + "payload": [ + { + "name": "bandChanOrFreq", + "ctype": "uint16_t" + }, + { + "name": "power", + "ctype": "uint8_t" + }, + { + "name": "pitMode", + "ctype": "uint8_t" + }, + { + "name": "lowPowerDisarm", + "ctype": "uint8_t", + "enum": "vtxLowerPowerDisarm_e" + }, + { + "name": "pitModeFreq", + "ctype": "uint16_t", + "desc": "Read and skipped." + } + ] + }, + "reply": null + }, + "payloadSize >= 5": { + "description": "Adds low-power disarm behaviour.", + "request": { + "payload": [ + { + "name": "bandChanOrFreq", + "ctype": "uint16_t" + }, + { + "name": "power", + "ctype": "uint8_t" + }, + { + "name": "pitMode", + "ctype": "uint8_t" + }, + { + "name": "lowPowerDisarm", + "ctype": "uint8_t", + "enum": "vtxLowerPowerDisarm_e", + "desc": "0=Off, 1=Always, 2=Until first arm." + } + ] + }, + "reply": null + }, + "payloadSize >= 4": { + "description": "Adds power index and pit mode flag.", + "request": { + "payload": [ + { + "name": "bandChanOrFreq", + "ctype": "uint16_t" + }, + { + "name": "power", + "ctype": "uint8_t" + }, + { + "name": "pitMode", + "ctype": "uint8_t" + } + ] + }, + "reply": null + }, + "payloadSize == 2": { + "description": "Minimum payload (band/channel encoded in 0..63).", + "request": { + "payload": [ + { + "name": "bandChanOrFreq", + "ctype": "uint16_t", + "desc": "If <= `VTXCOMMON_MSP_BANDCHAN_CHKVAL`, decoded as band/channel; otherwise treated as a frequency placeholder." + } + ] + }, + "reply": null + } + } + }, + "MSP_ADVANCED_CONFIG": { + "code": 90, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "gyroSyncDenom", + "ctype": "uint8_t", + "desc": "Always 1 (BF compatibility)", + "units": "", + "value": 1 + }, + { + "name": "pidProcessDenom", + "ctype": "uint8_t", + "desc": "Always 1 (BF compatibility)", + "units": "", + "value": 1 + }, + { + "name": "useUnsyncedPwm", + "ctype": "uint8_t", + "desc": "Always 1 (BF compatibility, INAV uses async PWM based on protocol)", + "units": "", + "value": 1 + }, + { + "name": "motorPwmProtocol", + "ctype": "uint8_t", + "desc": "Motor PWM protocol type (`motorConfig()->motorPwmProtocol`).", + "units": "Enum", + "enum": "motorPwmProtocolTypes_e" + }, + { + "name": "motorPwmRate", + "ctype": "uint16_t", + "desc": "Motor PWM rate (if applicable) (`motorConfig()->motorPwmRate`).", + "units": "Hz" + }, + { + "name": "servoPwmRate", + "ctype": "uint16_t", + "desc": "Servo PWM rate (`servoConfig()->servoPwmRate`).", + "units": "Hz" + }, + { + "name": "legacyGyroSync", + "ctype": "uint8_t", + "desc": "Always 0 (BF compatibility)", + "units": "", + "value": 0 + } + ] + }, + "notes": "", + "description": "Retrieves advanced hardware-related configuration (PWM protocols, rates). Some fields are BF compatibility placeholders." + }, + "MSP_SET_ADVANCED_CONFIG": { + "code": 91, + "mspv": 1, + "request": { + "payload": [ + { + "name": "gyroSyncDenom", + "ctype": "uint8_t", + "desc": "Ignored (legacy Betaflight field).", + "units": "" + }, + { + "name": "pidProcessDenom", + "ctype": "uint8_t", + "desc": "Ignored (legacy Betaflight field).", + "units": "" + }, + { + "name": "useUnsyncedPwm", + "ctype": "uint8_t", + "desc": "Ignored (legacy Betaflight field).", + "units": "" + }, + { + "name": "motorPwmProtocol", + "ctype": "uint8_t", + "desc": "Sets `motorConfigMutable()->motorPwmProtocol`.", + "units": "Enum", + "enum": "motorPwmProtocolTypes_e" + }, + { + "name": "motorPwmRate", + "ctype": "uint16_t", + "desc": "Sets `motorConfigMutable()->motorPwmRate`.", + "units": "Hz" + }, + { + "name": "servoPwmRate", + "ctype": "uint16_t", + "desc": "Sets `servoConfigMutable()->servoPwmRate`.", + "units": "Hz" + }, + { + "name": "legacyGyroSync", + "ctype": "uint8_t", + "desc": "Ignored (legacy Betaflight field).", + "units": "" + } + ] + }, + "reply": null, + "notes": "Expects 9 bytes.", + "description": "Sets advanced hardware-related configuration (PWM protocols, rates)." + }, + "MSP_FILTER_CONFIG": { + "code": 92, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "gyroMainLpfHz", + "ctype": "uint8_t", + "desc": "Gyro main low-pass filter cutoff frequency (`gyroConfig()->gyro_main_lpf_hz`)", + "units": "Hz" + }, + { + "name": "dtermLpfHz", + "ctype": "uint16_t", + "desc": "D-term low-pass filter cutoff frequency (`pidProfile()->dterm_lpf_hz`)", + "units": "Hz" + }, + { + "name": "yawLpfHz", + "ctype": "uint16_t", + "desc": "Yaw low-pass filter cutoff frequency (`pidProfile()->yaw_lpf_hz`)", + "units": "Hz" + }, + { + "name": "legacyGyroNotchHz", + "ctype": "uint16_t", + "desc": "Always 0 (Legacy)", + "value": 0 + }, + { + "name": "legacyGyroNotchCutoff", + "ctype": "uint16_t", + "desc": "Always 1 (Legacy)", + "value": 1 + }, + { + "name": "bfCompatDtermNotchHz", + "ctype": "uint16_t", + "desc": "Always 0 (BF compatibility)", + "value": 0 + }, + { + "name": "bfCompatDtermNotchCutoff", + "ctype": "uint16_t", + "desc": "Always 1 (BF compatibility)", + "value": 1 + }, + { + "name": "bfCompatGyroNotch2Hz", + "ctype": "uint16_t", + "desc": "Always 0 (BF compatibility)", + "value": 0 + }, + { + "name": "bfCompatGyroNotch2Cutoff", + "ctype": "uint16_t", + "desc": "Always 1 (BF compatibility)", + "value": 1 + }, + { + "name": "accNotchHz", + "ctype": "uint16_t", + "desc": "Accelerometer notch filter center frequency (`accelerometerConfig()->acc_notch_hz`)", + "units": "Hz" + }, + { + "name": "accNotchCutoff", + "ctype": "uint16_t", + "desc": "Accelerometer notch filter cutoff frequency (`accelerometerConfig()->acc_notch_cutoff`)", + "units": "Hz" + }, + { + "name": "legacyGyroStage2LpfHz", + "ctype": "uint16_t", + "desc": "Always 0 (Legacy)", + "value": 0 + } + ] + }, + "notes": "", + "description": "Retrieves filter configuration settings (Gyro, D-term, Yaw, Accel). Some fields are BF compatibility placeholders or legacy." + }, + "MSP_SET_FILTER_CONFIG": { + "code": 93, + "mspv": 1, + "request": { + "payload": [ + { + "name": "gyroMainLpfHz", + "ctype": "uint8_t", + "desc": "Sets `gyroConfigMutable()->gyro_main_lpf_hz`. (Size >= 5)", + "units": "Hz" + }, + { + "name": "dtermLpfHz", + "ctype": "uint16_t", + "desc": "Sets `pidProfileMutable()->dterm_lpf_hz` (constrained 0-500). (Size >= 5)", + "units": "Hz" + }, + { + "name": "yawLpfHz", + "ctype": "uint16_t", + "desc": "Sets `pidProfileMutable()->yaw_lpf_hz` (constrained 0-255). (Size >= 5)", + "units": "Hz" + }, + { + "name": "legacyGyroNotchHz", + "ctype": "uint16_t", + "desc": "Ignored. (Size >= 9)" + }, + { + "name": "legacyGyroNotchCutoff", + "ctype": "uint16_t", + "desc": "Ignored. (Size >= 9)" + }, + { + "name": "bfCompatDtermNotchHz", + "ctype": "uint16_t", + "desc": "Ignored. (Size >= 13)" + }, + { + "name": "bfCompatDtermNotchCutoff", + "ctype": "uint16_t", + "desc": "Ignored. (Size >= 13)" + }, + { + "name": "bfCompatGyroNotch2Hz", + "ctype": "uint16_t", + "desc": "Ignored. (Size >= 17)" + }, + { + "name": "bfCompatGyroNotch2Cutoff", + "ctype": "uint16_t", + "desc": "Ignored. (Size >= 17)" + }, + { + "name": "accNotchHz", + "ctype": "uint16_t", + "desc": "Sets `accelerometerConfigMutable()->acc_notch_hz` (constrained 0-255). (Size >= 21)", + "units": "Hz" + }, + { + "name": "accNotchCutoff", + "ctype": "uint16_t", + "desc": "Sets `accelerometerConfigMutable()->acc_notch_cutoff` (constrained 1-255). (Size >= 21)", + "units": "Hz" + }, + { + "name": "legacyGyroStage2LpfHz", + "ctype": "uint16_t", + "desc": "Ignored. (Size >= 22)" + } + ] + }, + "reply": null, + "notes": "Requires at least 22 bytes; intermediate length checks enforce legacy Betaflight frame layout and call `pidInitFilters()` once the D-term notch placeholders are consumed.", + "description": "Sets filter configuration settings. Handles different payload lengths for backward compatibility." + }, + "MSP_PID_ADVANCED": { + "code": 94, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "legacyRollPitchItermIgnore", + "ctype": "uint16_t", + "desc": "Always 0 (Legacy)", + "value": 0 + }, + { + "name": "legacyYawItermIgnore", + "ctype": "uint16_t", + "desc": "Always 0 (Legacy)", + "value": 0 + }, + { + "name": "legacyYawPLimit", + "ctype": "uint16_t", + "desc": "Always 0 (Legacy)", + "value": 0 + }, + { + "name": "bfCompatDeltaMethod", + "ctype": "uint8_t", + "desc": "Always 0 (BF compatibility)", + "value": 0 + }, + { + "name": "bfCompatVbatPidComp", + "ctype": "uint8_t", + "desc": "Always 0 (BF compatibility)", + "value": 0 + }, + { + "name": "bfCompatSetpointRelaxRatio", + "ctype": "uint8_t", + "desc": "Always 0 (BF compatibility)", + "value": 0 + }, + { + "name": "reserved1", + "ctype": "uint8_t", + "desc": "Always 0", + "value": 0 + }, + { + "name": "legacyPidSumLimit", + "ctype": "uint16_t", + "desc": "Always 0 (Legacy)", + "value": 0 + }, + { + "name": "bfCompatItermThrottleGain", + "ctype": "uint8_t", + "desc": "Always 0 (BF compatibility)", + "value": 0 + }, + { + "name": "accelLimitRollPitch", + "ctype": "uint16_t", + "desc": "Axis acceleration limit for Roll/Pitch / 10 (`pidProfile()->axisAccelerationLimitRollPitch / 10`)", + "units": "dps / 10" + }, + { + "name": "accelLimitYaw", + "ctype": "uint16_t", + "desc": "Axis acceleration limit for Yaw / 10 (`pidProfile()->axisAccelerationLimitYaw / 10`)", + "units": "dps / 10" + } + ] + }, + "notes": "Acceleration limits are scaled by 10 for compatibility.", + "description": "Retrieves advanced PID tuning parameters. Many fields are BF compatibility placeholders." + }, + "MSP_SET_PID_ADVANCED": { + "code": 95, + "mspv": 1, + "request": { + "payload": [ + { + "name": "legacyRollPitchItermIgnore", + "ctype": "uint16_t", + "desc": "Ignored (legacy compatibility)." + }, + { + "name": "legacyYawItermIgnore", + "ctype": "uint16_t", + "desc": "Ignored (legacy compatibility)." + }, + { + "name": "legacyYawPLimit", + "ctype": "uint16_t", + "desc": "Ignored (legacy compatibility)." + }, + { + "name": "bfCompatDeltaMethod", + "ctype": "uint8_t", + "desc": "Ignored (BF compatibility)." + }, + { + "name": "bfCompatVbatPidComp", + "ctype": "uint8_t", + "desc": "Ignored (BF compatibility)." + }, + { + "name": "bfCompatSetpointRelaxRatio", + "ctype": "uint8_t", + "desc": "Ignored (BF compatibility)." + }, + { + "name": "reserved1", + "ctype": "uint8_t", + "desc": "Ignored (reserved)." + }, + { + "name": "legacyPidSumLimit", + "ctype": "uint16_t", + "desc": "Ignored (legacy compatibility)." + }, + { + "name": "bfCompatItermThrottleGain", + "ctype": "uint8_t", + "desc": "Ignored (BF compatibility)." + }, + { + "name": "accelLimitRollPitch", + "ctype": "uint16_t", + "desc": "Sets `pidProfileMutable()->axisAccelerationLimitRollPitch = value * 10`.", + "units": "dps / 10" + }, + { + "name": "accelLimitYaw", + "ctype": "uint16_t", + "desc": "Sets `pidProfileMutable()->axisAccelerationLimitYaw = value * 10`.", + "units": "dps / 10" + } + ] + }, + "reply": null, + "notes": "Expects 17 bytes.", + "description": "Sets advanced PID tuning parameters." + }, + "MSP_SENSOR_CONFIG": { + "code": 96, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "accHardware", + "ctype": "uint8_t", + "desc": "Enum (`accelerationSensor_e`): Accelerometer hardware type (`accelerometerConfig()->acc_hardware`)", + "units": "", + "enum": "accelerationSensor_e" + }, + { + "name": "baroHardware", + "ctype": "uint8_t", + "desc": "Enum (`baroSensor_e`): Barometer hardware type (`barometerConfig()->baro_hardware`). 0 if `USE_BARO` disabled", + "units": "", + "enum": "baroSensor_e" + }, + { + "name": "magHardware", + "ctype": "uint8_t", + "desc": "Enum (`magSensor_e`): Magnetometer hardware type (`compassConfig()->mag_hardware`). 0 if `USE_MAG` disabled", + "units": "", + "enum": "magSensor_e" + }, + { + "name": "pitotHardware", + "ctype": "uint8_t", + "desc": "Enum (`pitotSensor_e`): Pitot tube hardware type (`pitotmeterConfig()->pitot_hardware`). 0 if `USE_PITOT` disabled", + "units": "", + "enum": "pitotSensor_e" + }, + { + "name": "rangefinderHardware", + "ctype": "uint8_t", + "desc": "Enum (`rangefinderType_e`): Rangefinder hardware type (`rangefinderConfig()->rangefinder_hardware`). 0 if `USE_RANGEFINDER` disabled", + "units": "", + "enum": "rangefinderType_e" + }, + { + "name": "opflowHardware", + "ctype": "uint8_t", + "desc": "Enum (`opticalFlowSensor_e`): Optical flow hardware type (`opticalFlowConfig()->opflow_hardware`). 0 if `USE_OPFLOW` disabled", + "units": "", + "enum": "opticalFlowSensor_e" + } + ] + }, + "notes": "", + "description": "Retrieves the configured hardware type for various sensors." + }, + "MSP_SET_SENSOR_CONFIG": { + "code": 97, + "mspv": 1, + "request": { + "payload": [ + { + "name": "accHardware", + "ctype": "uint8_t", + "desc": "Sets `accelerometerConfigMutable()->acc_hardware`", + "units": "", + "enum": "accelerationSensor_e" + }, + { + "name": "baroHardware", + "ctype": "uint8_t", + "desc": "Sets `barometerConfigMutable()->baro_hardware` (if `USE_BARO`)", + "units": "", + "enum": "baroSensor_e" + }, + { + "name": "magHardware", + "ctype": "uint8_t", + "desc": "Sets `compassConfigMutable()->mag_hardware` (if `USE_MAG`)", + "units": "", + "enum": "magSensor_e" + }, + { + "name": "pitotHardware", + "ctype": "uint8_t", + "desc": "Sets `pitotmeterConfigMutable()->pitot_hardware` (if `USE_PITOT`)", + "units": "", + "enum": "pitotSensor_e" + }, + { + "name": "rangefinderHardware", + "ctype": "uint8_t", + "desc": "Sets `rangefinderConfigMutable()->rangefinder_hardware` (if `USE_RANGEFINDER`)", + "units": "", + "enum": "rangefinderType_e" + }, + { + "name": "opflowHardware", + "ctype": "uint8_t", + "desc": "Sets `opticalFlowConfigMutable()->opflow_hardware` (if `USE_OPFLOW`)", + "units": "", + "enum": "opticalFlowSensor_e" + } + ] + }, + "reply": null, + "notes": "Expects 6 bytes.", + "description": "Sets the configured hardware type for various sensors." + }, + "MSP_SPECIAL_PARAMETERS": { + "code": 98, + "mspv": 1, + "not_implemented": true, + "request": null, + "reply": null, + "notes": "Not implemented in INAV `fc_msp.c`.", + "description": "Betaflight specific" + }, + "MSP_SET_SPECIAL_PARAMETERS": { + "code": 99, + "mspv": 1, + "not_implemented": true, + "request": null, + "reply": null, + "notes": "Not implemented in INAV `fc_msp.c`.", + "description": "Betaflight specific" + }, + "MSP_IDENT": { + "code": 100, + "mspv": 1, + "not_implemented": true, + "request": null, + "reply": { + "payload": [ + { + "name": "MultiWii version", + "ctype": "uint8_t", + "desc": "Scaled version major*100+minor", + "units": "n/a" + }, + { + "name": "Mixer Mode", + "ctype": "uint8_t", + "desc": "Mixer type", + "units": "Enum" + }, + { + "name": "MSP Version", + "ctype": "uint8_t", + "desc": "Scaled version major*100+minor", + "units": "n/a" + }, + { + "name": "Platform Capability", + "ctype": "uint32_t", + "desc": "Bitmask: MW capabilities", + "units": "Bitmask", + "bitmask": true + } + ] + }, + "notes": "Obsolete. Listed for legacy compatibility only.", + "description": "Provides basic flight controller identity information. Not implemented in modern INAV, but used by legacy versions and MultiWii." + }, + "MSP_STATUS": { + "code": 101, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "cycleTime", + "ctype": "uint16_t", + "desc": "Main loop cycle time (`cycleTime`)", + "units": "µs" + }, + { + "name": "i2cErrors", + "ctype": "uint16_t", + "desc": "Number of I2C errors encountered (`i2cGetErrorCounter()`). 0 if `USE_I2C` not defined", + "units": "Count" + }, + { + "name": "sensorStatus", + "ctype": "uint16_t", + "desc": "Bitmask: available/active sensors (`packSensorStatus()`). See notes", + "units": "Bitmask", + "bitmask": true + }, + { + "name": "activeModesLow", + "ctype": "uint32_t", + "desc": "Bitmask: First 32 bits of the active flight modes bitmask (`packBoxModeFlags()`)", + "units": "Bitmask", + "bitmask": true + }, + { + "name": "profile", + "ctype": "uint8_t", + "desc": "Current configuration profile index (0-based) (`getConfigProfile()`)", + "units": "Index" + } + ] + }, + "notes": "Superseded by `MSP_STATUS_EX` and `MSP2_INAV_STATUS`. `sensorStatus` bitmask: (Bit 0: ACC, 1: BARO, 2: MAG, 3: GPS, 4: RANGEFINDER, 5: OPFLOW, 6: PITOT, 7: TEMP; Bit 15: hardware failure). `activeModesLow` only contains the first 32 modes; use `MSP_ACTIVEBOXES` for the full set.", + "description": "Provides basic flight controller status including cycle time, errors, sensor status, active modes (first 32), and the current configuration profile." + }, + "MSP_RAW_IMU": { + "code": 102, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "accX", + "ctype": "int16_t", + "desc": "Raw accelerometer X reading, scaled (`acc.accADCf[X] * 512`)", + "units": "~1/512 G" + }, + { + "name": "accY", + "ctype": "int16_t", + "desc": "Raw accelerometer Y reading, scaled (`acc.accADCf[Y] * 512`)", + "units": "~1/512 G" + }, + { + "name": "accZ", + "ctype": "int16_t", + "desc": "Raw accelerometer Z reading, scaled (`acc.accADCf[Z] * 512`)", + "units": "~1/512 G" + }, + { + "name": "gyroX", + "ctype": "int16_t", + "desc": "Gyroscope X-axis rate (`gyroRateDps(X)`)", + "units": "deg/s" + }, + { + "name": "gyroY", + "ctype": "int16_t", + "desc": "Gyroscope Y-axis rate (`gyroRateDps(Y)`)", + "units": "deg/s" + }, + { + "name": "gyroZ", + "ctype": "int16_t", + "desc": "Gyroscope Z-axis rate (`gyroRateDps(Z)`)", + "units": "deg/s" + }, + { + "name": "magX", + "ctype": "int16_t", + "desc": "Raw magnetometer X reading (`mag.magADC[X]`). 0 if `USE_MAG` disabled", + "units": "Raw units" + }, + { + "name": "magY", + "ctype": "int16_t", + "desc": "Raw magnetometer Y reading (`mag.magADC[Y]`). 0 if `USE_MAG` disabled", + "units": "Raw units" + }, + { + "name": "magZ", + "ctype": "int16_t", + "desc": "Raw magnetometer Z reading (`mag.magADC[Z]`). 0 if `USE_MAG` disabled", + "units": "Raw units" + } + ] + }, + "notes": "Acc scaling is approximate (512 LSB/G). Mag units depend on the sensor.", + "description": "Provides raw sensor readings from the IMU (Accelerometer, Gyroscope, Magnetometer)." + }, + "MSP_SERVO": { + "code": 103, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "servoOutputs", + "desc": "Array of current servo output values (typically 1000-2000)", + "ctype": "int16_t", + "array": true, + "array_size": 18, + "array_size_define": "MAX_SUPPORTED_SERVOS", + "units": "PWM" + } + ] + }, + "variable_len": true, + "notes": "", + "description": "Provides the current output values for all supported servos." + }, + "MSP_MOTOR": { + "code": 104, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "motorOutputs", + "desc": "Array of current motor output values (typically 1000-2000). Values beyond `MAX_SUPPORTED_MOTORS` are 0", + "ctype": "int16_t", + "array": true, + "array_size": 8, + "units": "PWM" + } + ] + }, + "notes": "", + "description": "Provides the current output values for the first 8 motors." + }, + "MSP_RC": { + "code": 105, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "rcChannels", + "desc": "Array of current RC channel values (typically 1000-2000). Length depends on detected channels", + "ctype": "int16_t", + "array": true, + "array_size": 0, + "units": "PWM" + } + ] + }, + "variable_len": true, + "notes": "Array length equals `rxRuntimeConfig.channelCount`.", + "description": "Provides the current values of the received RC channels." + }, + "MSP_RAW_GPS": { + "code": 106, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "fixType", + "ctype": "uint8_t", + "desc": "Enum `gpsFixType_e` GPS fix type (`gpsSol.fixType`)", + "units": "Enum", + "enum": "gpsFixType_e" + }, + { + "name": "numSat", + "ctype": "uint8_t", + "desc": "Number of satellites used in solution (`gpsSol.numSat`)", + "units": "Count" + }, + { + "name": "latitude", + "ctype": "int32_t", + "desc": "Latitude (`gpsSol.llh.lat`)", + "units": "deg * 1e7" + }, + { + "name": "longitude", + "ctype": "int32_t", + "desc": "Longitude (`gpsSol.llh.lon`)", + "units": "deg * 1e7" + }, + { + "name": "altitude", + "ctype": "int16_t", + "desc": "Altitude above MSL (`gpsSol.llh.alt`) sent as centimeters", + "units": "cm" + }, + { + "name": "speed", + "ctype": "int16_t", + "desc": "Ground speed (`gpsSol.groundSpeed`)", + "units": "cm/s" + }, + { + "name": "groundCourse", + "ctype": "int16_t", + "desc": "Ground course (`gpsSol.groundCourse`)", + "units": "deci-degrees" + }, + { + "name": "hdop", + "ctype": "uint16_t", + "desc": "Horizontal Dilution of Precision (`gpsSol.hdop`)", + "units": "HDOP * 100" + } + ] + }, + "notes": "Only available if `USE_GPS` is defined. Altitude is truncated to meters.", + "description": "Provides raw GPS data (fix status, coordinates, altitude, speed, course)." + }, + "MSP_COMP_GPS": { + "code": 107, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "distanceToHome", + "ctype": "uint16_t", + "desc": "Distance to the home point (`GPS_distanceToHome`)", + "units": "meters" + }, + { + "name": "directionToHome", + "ctype": "int16_t", + "desc": "Direction to the home point (0-360) (`GPS_directionToHome`)", + "units": "degrees" + }, + { + "name": "gpsHeartbeat", + "ctype": "uint8_t", + "desc": "Indicates if GPS data is being received (`gpsSol.flags.gpsHeartbeat`)", + "units": "Boolean" + } + ] + }, + "notes": "Only available if `USE_GPS` is defined.", + "description": "Provides computed GPS values: distance and direction to home." + }, + "MSP_ATTITUDE": { + "code": 108, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "roll", + "ctype": "int16_t", + "desc": "Roll angle (`attitude.values.roll`)", + "units": "deci-degrees" + }, + { + "name": "pitch", + "ctype": "int16_t", + "desc": "Pitch angle (`attitude.values.pitch`)", + "units": "deci-degrees" + }, + { + "name": "yaw", + "ctype": "int16_t", + "desc": "Yaw/Heading angle (`DECIDEGREES_TO_DEGREES(attitude.values.yaw)`)", + "units": "degrees" + } + ] + }, + "notes": "Yaw is in degrees.", + "description": "Provides the current attitude estimate (roll, pitch, yaw)." + }, + "MSP_ALTITUDE": { + "code": 109, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "estimatedAltitude", + "ctype": "int32_t", + "desc": "Estimated altitude above home/sea level (`getEstimatedActualPosition(Z)`)", + "units": "cm" + }, + { + "name": "variometer", + "ctype": "int16_t", + "desc": "Estimated vertical speed (`getEstimatedActualVelocity(Z)`)", + "units": "cm/s" + }, + { + "name": "baroAltitude", + "ctype": "int32_t", + "desc": "Latest raw altitude from barometer (`baroGetLatestAltitude()`). 0 if `USE_BARO` disabled", + "units": "cm" + } + ] + }, + "notes": "", + "description": "Provides estimated altitude, vertical speed (variometer), and raw barometric altitude." + }, + "MSP_ANALOG": { + "code": 110, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "vbat", + "ctype": "uint8_t", + "desc": "Battery voltage, scaled (`getBatteryVoltage() / 10`), constrained 0-255", + "units": "0.1V" + }, + { + "name": "mAhDrawn", + "ctype": "uint16_t", + "desc": "Consumed battery capacity (`getMAhDrawn()`), constrained 0-65535", + "units": "mAh" + }, + { + "name": "rssi", + "ctype": "uint16_t", + "desc": "Received Signal Strength Indicator (`getRSSI()`). Units depend on source", + "units": "0-1023 or %" + }, + { + "name": "amperage", + "ctype": "int16_t", + "desc": "Current draw (`getAmperage()`), constrained -32768 to 32767", + "units": "0.01A" + } + ] + }, + "notes": "Superseded by `MSP2_INAV_ANALOG` which provides higher precision and more fields.", + "description": "Provides analog sensor readings: battery voltage, current consumption (mAh), RSSI, and current draw (Amps)." + }, + "MSP_RC_TUNING": { + "code": 111, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "legacyRcRate", + "ctype": "uint8_t", + "desc": "Always 100 (Legacy, unused)", + "units": "", + "value": 100 + }, + { + "name": "rcExpo", + "ctype": "uint8_t", + "desc": "Roll/Pitch RC Expo (`currentControlRateProfile->stabilized.rcExpo8`)", + "units": "" + }, + { + "name": "rollRate", + "ctype": "uint8_t", + "desc": "Roll Rate (`currentControlRateProfile->stabilized.rates[FD_ROLL]`)", + "units": "" + }, + { + "name": "pitchRate", + "ctype": "uint8_t", + "desc": "Pitch Rate (`currentControlRateProfile->stabilized.rates[FD_PITCH]`)", + "units": "" + }, + { + "name": "yawRate", + "ctype": "uint8_t", + "desc": "Yaw Rate (`currentControlRateProfile->stabilized.rates[FD_YAW]`)", + "units": "" + }, + { + "name": "dynamicThrottlePID", + "ctype": "uint8_t", + "desc": "Dynamic Throttle PID (TPA) value (`currentControlRateProfile->throttle.dynPID`)", + "units": "" + }, + { + "name": "throttleMid", + "ctype": "uint8_t", + "desc": "Throttle Midpoint (`currentControlRateProfile->throttle.rcMid8`)", + "units": "" + }, + { + "name": "throttleExpo", + "ctype": "uint8_t", + "desc": "Throttle Expo (`currentControlRateProfile->throttle.rcExpo8`)", + "units": "" + }, + { + "name": "tpaBreakpoint", + "ctype": "uint16_t", + "desc": "Throttle PID Attenuation (TPA) breakpoint (`currentControlRateProfile->throttle.pa_breakpoint`)", + "units": "" + }, + { + "name": "rcYawExpo", + "ctype": "uint8_t", + "desc": "Yaw RC Expo (`currentControlRateProfile->stabilized.rcYawExpo8`)", + "units": "" + } + ] + }, + "notes": "Superseded by `MSP2_INAV_RATE_PROFILE` which includes manual rates/expos.", + "description": "Retrieves RC tuning parameters (rates, expos, TPA) for the current control rate profile." + }, + "MSP_ACTIVEBOXES": { + "code": 113, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "activeModes", + "ctype": "boxBitmask_t", + "desc": "Bitmask: all active modes (`packBoxModeFlags()`). Size depends on `boxBitmask_t` definition", + "units": "Bitmask", + "bitmask": true + } + ] + }, + "notes": "Use this instead of `MSP_STATUS` or `MSP_STATUS_EX` if more than 32 modes are possible.", + "description": "Provides the full bitmask of currently active flight modes (boxes)." + }, + "MSP_MISC": { + "code": 114, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "midRc", + "ctype": "uint16_t", + "desc": "Mid RC value (`PWM_RANGE_MIDDLE`, typically 1500)", + "units": "PWM" + }, + { + "name": "legacyMinThrottle", + "ctype": "uint16_t", + "desc": "Always 0 (Legacy)", + "value": 0 + }, + { + "name": "maxThrottle", + "ctype": "uint16_t", + "desc": "Maximum throttle command (`getMaxThrottle()`)", + "units": "PWM" + }, + { + "name": "minCommand", + "ctype": "uint16_t", + "desc": "Minimum motor command when disarmed (`motorConfig()->mincommand`)", + "units": "PWM" + }, + { + "name": "failsafeThrottle", + "ctype": "uint16_t", + "desc": "Failsafe throttle level (`currentBatteryProfile->failsafe_throttle`)", + "units": "PWM" + }, + { + "name": "gpsType", + "ctype": "uint8_t", + "desc": "Enum `gpsProvider_e` GPS provider type (`gpsConfig()->provider`). 0 if `USE_GPS` disabled", + "units": "Enum", + "enum": "gpsProvider_e" + }, + { + "name": "legacyGpsBaud", + "ctype": "uint8_t", + "desc": "Always 0 (Legacy)", + "value": 0 + }, + { + "name": "gpsSbasMode", + "ctype": "uint8_t", + "desc": "Enum `sbasMode_e` GPS SBAS mode (`gpsConfig()->sbasMode`). 0 if `USE_GPS` disabled", + "units": "Enum", + "enum": "sbasMode_e" + }, + { + "name": "legacyMwCurrentOut", + "ctype": "uint8_t", + "desc": "Always 0 (Legacy)", + "value": 0 + }, + { + "name": "rssiChannel", + "ctype": "uint8_t", + "desc": "RSSI channel index (1-based) (`rxConfig()->rssi_channel`)", + "units": "Index" + }, + { + "name": "reserved1", + "ctype": "uint8_t", + "desc": "Always 0", + "value": 0 + }, + { + "name": "magDeclination", + "ctype": "uint16_t", + "desc": "Magnetic declination / 10 (`compassConfig()->mag_declination / 10`). 0 if `USE_MAG` disabled", + "units": "0.1 degrees" + }, + { + "name": "vbatScale", + "ctype": "uint8_t", + "desc": "Voltage scale / 10 (`batteryMetersConfig()->voltage.scale / 10`). 0 if `USE_ADC` disabled", + "units": "Scale / 10" + }, + { + "name": "vbatMinCell", + "ctype": "uint8_t", + "desc": "Min cell voltage / 10 (`currentBatteryProfile->voltage.cellMin / 10`). 0 if `USE_ADC` disabled", + "units": "0.1V" + }, + { + "name": "vbatMaxCell", + "ctype": "uint8_t", + "desc": "Max cell voltage / 10 (`currentBatteryProfile->voltage.cellMax / 10`). 0 if `USE_ADC` disabled", + "units": "0.1V" + }, + { + "name": "vbatWarningCell", + "ctype": "uint8_t", + "desc": "Warning cell voltage / 10 (`currentBatteryProfile->voltage.cellWarning / 10`). 0 if `USE_ADC` disabled", + "units": "0.1V" + } + ] + }, + "notes": "Superseded by `MSP2_INAV_MISC` and other specific commands which offer better precision and more fields.", + "description": "Retrieves miscellaneous configuration settings, mostly related to RC, GPS, Mag, and Battery voltage (legacy formats)." + }, + "MSP_BOXNAMES": { + "code": 116, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "boxNamesString", + "desc": "String containing mode names separated by ';'. Null termination not guaranteed by MSP, relies on payload size. (`serializeBoxNamesReply()`)", + "ctype": "char", + "array": true, + "array_size": 0, + "units": "" + } + ] + }, + "variable_len": true, + "notes": "The exact set of names depends on compiled features and configuration. Due to the size of the payload, it is recommended that [`MSP_BOXIDS`](#msp_boxids-119--0x77) is used instead.", + "description": "Provides a semicolon-separated string containing the names of all available flight modes (boxes)." + }, + "MSP_PIDNAMES": { + "code": 117, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "pidNamesString", + "desc": "String \"ROLL;PITCH;YAW;ALT;Pos;PosR;NavR;LEVEL;MAG;VEL;\". Null termination not guaranteed by MSP", + "ctype": "char", + "array": true, + "array_size": 0, + "units": "" + } + ] + }, + "variable_len": true, + "notes": "", + "description": "Provides a semicolon-separated string containing the names of the PID controllers." + }, + "MSP_WP": { + "code": 118, + "mspv": 1, + "request": { + "payload": [ + { + "name": "waypointIndex", + "ctype": "uint8_t", + "desc": "Index of the waypoint to retrieve (0 to `NAV_MAX_WAYPOINTS - 1`)", + "units": "" + } + ] + }, + "reply": { + "payload": [ + { + "name": "waypointIndex", + "ctype": "uint8_t", + "desc": "Index of the returned waypoint", + "units": "Index" + }, + { + "name": "action", + "ctype": "uint8_t", + "desc": "Enum `navWaypointActions_e` Waypoint action type", + "units": "Enum", + "enum": "navWaypointActions_e" + }, + { + "name": "latitude", + "ctype": "int32_t", + "desc": "Latitude coordinate", + "units": "deg * 1e7" + }, + { + "name": "longitude", + "ctype": "int32_t", + "desc": "Longitude coordinate", + "units": "deg * 1e7" + }, + { + "name": "altitude", + "ctype": "int32_t", + "desc": "Altitude coordinate (relative to home or sea level, see flag)", + "units": "cm" + }, + { + "name": "param1", + "ctype": "int16_t", + "desc": "Parameter 1 (meaning depends on action)", + "units": "Varies" + }, + { + "name": "param2", + "ctype": "int16_t", + "desc": "Parameter 2 (meaning depends on action)", + "units": "Varies" + }, + { + "name": "param3", + "ctype": "int16_t", + "desc": "Parameter 3 (meaning depends on action)", + "units": "Varies" + }, + { + "name": "flag", + "ctype": "uint8_t", + "desc": "Bitmask: Waypoint flags (`NAV_WP_FLAG_*`)", + "units": "Bitmask", + "bitmask": true + } + ] + }, + "notes": "See `navWaypoint_t` and `navWaypointActions_e`.", + "description": "Get/Set a single waypoint from the mission plan." + }, + "MSP_BOXIDS": { + "code": 119, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "boxIds", + "desc": "Array of permanent IDs for each configured box (`serializeBoxReply()`). Length depends on number of boxes", + "ctype": "uint8_t", + "array": true, + "array_size": 0, + "units": "" + } + ] + }, + "variable_len": true, + "notes": "Useful for mapping mode range configurations (`MSP_MODE_RANGES`) back to user-understandable modes via `MSP_BOXNAMES`.", + "description": "Provides a list of permanent IDs associated with the available flight modes (boxes)." + }, + "MSP_SERVO_CONFIGURATIONS": { + "code": 120, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "min", + "ctype": "int16_t", + "desc": "Minimum servo endpoint (`servoParams(i)->min`)", + "units": "PWM" + }, + { + "name": "max", + "ctype": "int16_t", + "desc": "Maximum servo endpoint (`servoParams(i)->max`)", + "units": "PWM" + }, + { + "name": "middle", + "ctype": "int16_t", + "desc": "Middle/Neutral servo position (`servoParams(i)->middle`)", + "units": "PWM" + }, + { + "name": "rate", + "ctype": "int8_t", + "desc": "Servo rate/scaling (`servoParams(i)->rate`, -125..125). Encoded as two's complement", + "units": "% (-100 to 100)" + }, + { + "name": "reserved1", + "ctype": "uint8_t", + "desc": "Always 0", + "value": 0 + }, + { + "name": "reserved2", + "ctype": "uint8_t", + "desc": "Always 0", + "value": 0 + }, + { + "name": "legacyForwardChan", + "ctype": "uint8_t", + "desc": "Always 255 (Legacy)", + "value": 255 + }, + { + "name": "legacyReversedSources", + "ctype": "uint32_t", + "desc": "Always 0 (Legacy)", + "value": 0 + } + ], + "repeating": "MAX_SUPPORTED_SERVOS" + }, + "variable_len": "MAX_SUPPORTED_SERVOS", + "notes": "Superseded by `MSP2_INAV_SERVO_CONFIG` which has a cleaner structure.", + "description": "Retrieves the configuration parameters for all supported servos (min, max, middle, rate). Legacy format with unused fields." + }, + "MSP_NAV_STATUS": { + "code": 121, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "navMode", + "ctype": "uint8_t", + "desc": "Enum (`navSystemStatus_Mode_e`): Current navigation mode (None, RTH, NAV, Hold, etc.) (`NAV_Status.mode`)", + "units": "", + "enum": "navSystemStatus_Mode_e" + }, + { + "name": "navState", + "ctype": "uint8_t", + "desc": "Enum (`navSystemStatus_State_e`): Current navigation state (`NAV_Status.state`)", + "units": "", + "enum": "navSystemStatus_State_e" + }, + { + "name": "activeWpAction", + "ctype": "uint8_t", + "desc": "Enum (`navWaypointActions_e`): Action of the currently executing waypoint (`NAV_Status.activeWpAction`)", + "units": "", + "enum": "navWaypointActions_e" + }, + { + "name": "activeWpNumber", + "ctype": "uint8_t", + "desc": "Index: Index of the currently executing waypoint (`NAV_Status.activeWpNumber`)", + "units": "" + }, + { + "name": "navError", + "ctype": "uint8_t", + "desc": "Enum (`navSystemStatus_Error_e`): Current navigation error code (`NAV_Status.error`)", + "units": "", + "enum": "navSystemStatus_Error_e" + }, + { + "name": "targetHeading", + "ctype": "int16_t", + "desc": "Target heading for heading controller (`getHeadingHoldTarget()`)", + "units": "degrees" + } + ] + }, + "notes": "Requires `USE_GPS`.", + "description": "Retrieves the current status of the navigation system." + }, + "MSP_NAV_CONFIG": { + "code": 122, + "mspv": 1, + "not_implemented": true + }, + "MSP_3D": { + "code": 124, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "deadbandLow", + "ctype": "uint16_t", + "desc": "Lower deadband limit for 3D mode (`reversibleMotorsConfig()->deadband_low`)", + "units": "PWM" + }, + { + "name": "deadbandHigh", + "ctype": "uint16_t", + "desc": "Upper deadband limit for 3D mode (`reversibleMotorsConfig()->deadband_high`)", + "units": "PWM" + }, + { + "name": "neutral", + "ctype": "uint16_t", + "desc": "Neutral throttle point for 3D mode (`reversibleMotorsConfig()->neutral`)", + "units": "PWM" + } + ] + }, + "notes": "Requires reversible motor support.", + "description": "Retrieves settings related to 3D/reversible motor operation." + }, + "MSP_RC_DEADBAND": { + "code": 125, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "deadband", + "ctype": "uint8_t", + "desc": "General RC deadband for Roll/Pitch (`rcControlsConfig()->deadband`)", + "units": "PWM" + }, + { + "name": "yawDeadband", + "ctype": "uint8_t", + "desc": "Specific deadband for Yaw (`rcControlsConfig()->yaw_deadband`)", + "units": "PWM" + }, + { + "name": "altHoldDeadband", + "ctype": "uint8_t", + "desc": "Deadband for altitude hold adjustments (`rcControlsConfig()->alt_hold_deadband`)", + "units": "PWM" + }, + { + "name": "throttleDeadband", + "ctype": "uint16_t", + "desc": "Deadband around throttle mid-stick (`rcControlsConfig()->mid_throttle_deadband`)", + "units": "PWM" + } + ] + }, + "notes": "", + "description": "Retrieves RC input deadband settings." + }, + "MSP_SENSOR_ALIGNMENT": { + "code": 126, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "gyroAlign", + "ctype": "uint8_t", + "desc": "Always 0 (Legacy alignment enum)", + "units": "", + "value": 0 + }, + { + "name": "accAlign", + "ctype": "uint8_t", + "desc": "Always 0 (Legacy alignment enum)", + "units": "", + "value": 0 + }, + { + "name": "magAlign", + "ctype": "uint8_t", + "desc": "Magnetometer alignment (`compassConfig()->mag_align`). 0 if `USE_MAG` disabled", + "units": "" + }, + { + "name": "opflowAlign", + "ctype": "uint8_t", + "desc": "Optical flow alignment (`opticalFlowConfig()->opflow_align`). 0 if `USE_OPFLOW` disabled", + "units": "" + } + ] + }, + "notes": "Board alignment is now typically handled by `MSP_BOARD_ALIGNMENT`. This returns legacy enum values where applicable.", + "description": "Retrieves sensor alignment settings (legacy format)." + }, + "MSP_LED_STRIP_MODECOLOR": { + "code": 127, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "modeIndex", + "ctype": "uint8_t", + "desc": "Index of the LED mode Enum (`ledModeIndex_e`). `LED_MODE_COUNT` for special colors", + "units": "", + "enum": "ledModeIndex_e" + }, + { + "name": "directionOrSpecialIndex", + "ctype": "uint8_t", + "desc": "Index of the direction (`ledDirectionId_e`) or special color (`ledSpecialColorIds_e`)", + "units": "" + }, + { + "name": "colorIndex", + "ctype": "uint8_t", + "desc": "Index of the color assigned from `ledStripConfig()->colors`", + "units": "" + } + ], + "repeating": "LED_MODE_COUNT * LED_DIRECTION_COUNT + LED_SPECIAL_COLOR_COUNT" + }, + "variable_len": "LED_MODE_COUNT * LED_DIRECTION_COUNT + LED_SPECIAL_COLOR_COUNT", + "notes": "Only available if `USE_LED_STRIP` is defined. Entries where `modeIndex == LED_MODE_COUNT` describe special colors.", + "description": "Retrieves the color index assigned to each LED mode and function/direction combination, including special colors." + }, + "MSP_BATTERY_STATE": { + "code": 130, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "cellCount", + "ctype": "uint8_t", + "desc": "Number of battery cells (`getBatteryCellCount()`)", + "units": "Count" + }, + { + "name": "capacity", + "ctype": "uint16_t", + "desc": "Battery capacity (`currentBatteryProfile->capacity.value`)", + "units": "mAh" + }, + { + "name": "vbatScaled", + "ctype": "uint8_t", + "desc": "Battery voltage / 10 (`getBatteryVoltage() / 10`)", + "units": "0.1V" + }, + { + "name": "mAhDrawn", + "ctype": "uint16_t", + "desc": "Consumed capacity (`getMAhDrawn()`)", + "units": "mAh" + }, + { + "name": "amperage", + "ctype": "int16_t", + "desc": "Current draw (`getAmperage()`)", + "units": "0.01A" + }, + { + "name": "batteryState", + "ctype": "uint8_t", + "desc": "Enum `batteryState_e` Current battery state (`getBatteryState()`, see `BATTERY_STATE_*`)", + "units": "Enum", + "enum": "batteryState_e" + }, + { + "name": "vbatActual", + "ctype": "uint16_t", + "desc": "Actual battery voltage (`getBatteryVoltage()`)", + "units": "0.01V" + } + ] + }, + "notes": "Only available if `USE_DJI_HD_OSD` or `USE_MSP_DISPLAYPORT` is defined. Some values are duplicated from `MSP_ANALOG` / `MSP2_INAV_ANALOG` but potentially with different scaling/types.", + "description": "Provides battery state information, formatted primarily for DJI FPV Goggles compatibility." + }, + "MSP_VTXTABLE_BAND": { + "code": 137, + "mspv": 1, + "request": null, + "reply": null, + "notes": "The ID is defined, but no handler exists in the provided C code. Likely intended to query band names and frequencies.", + "description": "Retrieves information about a specific VTX band from the VTX table. (Implementation missing in provided `fc_msp.c`)" + }, + "MSP_VTXTABLE_POWERLEVEL": { + "code": 138, + "mspv": 1, + "request": { + "payload": [ + { + "name": "powerLevelIndex", + "ctype": "uint8_t", + "desc": "1-based index of the power level to query", + "units": "" + } + ] + }, + "reply": { + "payload": [ + { + "name": "powerLevelIndex", + "ctype": "uint8_t", + "desc": "1-based index of the returned power level", + "units": "" + }, + { + "name": "powerValue", + "ctype": "uint16_t", + "desc": "Always 0 (Actual power value in mW is not stored/returned via MSP)", + "units": "", + "value": 0 + }, + { + "name": "labelLength", + "ctype": "uint8_t", + "desc": "Length of the power level label string that follows", + "units": "" + }, + { + "name": "label", + "desc": "Power level label string (e.g., \"25\", \"200\"). Length given by previous field", + "ctype": "char", + "array": true, + "array_size": 0, + "units": "" + } + ] + }, + "variable_len": true, + "notes": "Requires `USE_VTX_CONTROL`. Returns error if index is out of bounds. The `powerValue` field is unused.", + "description": "Retrieves information about a specific VTX power level from the VTX table." + }, + "MSP_STATUS_EX": { + "code": 150, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "cycleTime", + "ctype": "uint16_t", + "desc": "Main loop cycle time", + "units": "µs" + }, + { + "name": "i2cErrors", + "ctype": "uint16_t", + "desc": "I2C errors", + "units": "Count" + }, + { + "name": "sensorStatus", + "ctype": "uint16_t", + "desc": "Bitmask: Sensor status", + "units": "Bitmask", + "bitmask": true + }, + { + "name": "activeModesLow", + "ctype": "uint32_t", + "desc": "Bitmask: First 32 active modes", + "units": "Bitmask", + "bitmask": true + }, + { + "name": "profile", + "ctype": "uint8_t", + "desc": "Current config profile index", + "units": "Index" + }, + { + "name": "cpuLoad", + "ctype": "uint16_t", + "desc": "Average system load percentage (`averageSystemLoadPercent`)", + "units": "%" + }, + { + "name": "armingFlags", + "ctype": "uint16_t", + "desc": "Bitmask: Flight controller arming flags (`armingFlags`). Note: Truncated to 16 bits", + "units": "Bitmask", + "bitmask": true + }, + { + "name": "accCalibAxisFlags", + "ctype": "uint8_t", + "desc": "Bitmask: Accelerometer calibrated axes flags (`accGetCalibrationAxisFlags()`)", + "units": "Bitmask", + "bitmask": true + } + ] + }, + "notes": "Superseded by `MSP2_INAV_STATUS` which provides the full 32-bit `armingFlags` and other enhancements.", + "description": "Provides extended flight controller status, including CPU load, arming flags, and calibration status, in addition to `MSP_STATUS` fields." + }, + "MSP_SENSOR_STATUS": { + "code": 151, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "overallHealth", + "ctype": "uint8_t", + "desc": "1 if all essential hardware is healthy, 0 otherwise (`isHardwareHealthy()`)", + "units": "Boolean" + }, + { + "name": "gyroStatus", + "ctype": "uint8_t", + "desc": "Enum `hardwareSensorStatus_e` Gyro hardware status (`getHwGyroStatus()`)", + "units": "Enum", + "enum": "hardwareSensorStatus_e" + }, + { + "name": "accStatus", + "ctype": "uint8_t", + "desc": "Enum `hardwareSensorStatus_e` Accelerometer hardware status (`getHwAccelerometerStatus()`)", + "units": "Enum", + "enum": "hardwareSensorStatus_e" + }, + { + "name": "magStatus", + "ctype": "uint8_t", + "desc": "Enum `hardwareSensorStatus_e` Compass hardware status (`getHwCompassStatus()`)", + "units": "Enum", + "enum": "hardwareSensorStatus_e" + }, + { + "name": "baroStatus", + "ctype": "uint8_t", + "desc": "Enum `hardwareSensorStatus_e` Barometer hardware status (`getHwBarometerStatus()`)", + "units": "Enum", + "enum": "hardwareSensorStatus_e" + }, + { + "name": "gpsStatus", + "ctype": "uint8_t", + "desc": "Enum `hardwareSensorStatus_e` GPS hardware status (`getHwGPSStatus()`)", + "units": "Enum", + "enum": "hardwareSensorStatus_e" + }, + { + "name": "rangefinderStatus", + "ctype": "uint8_t", + "desc": "Enum `hardwareSensorStatus_e` Rangefinder hardware status (`getHwRangefinderStatus()`)", + "units": "Enum", + "enum": "hardwareSensorStatus_e" + }, + { + "name": "pitotStatus", + "ctype": "uint8_t", + "desc": "Enum `hardwareSensorStatus_e` Pitot hardware status (`getHwPitotmeterStatus()`)", + "units": "Enum", + "enum": "hardwareSensorStatus_e" + }, + { + "name": "opflowStatus", + "ctype": "uint8_t", + "desc": "Enum `hardwareSensorStatus_e` Optical Flow hardware status (`getHwOpticalFlowStatus()`)", + "units": "Enum", + "enum": "hardwareSensorStatus_e" + } + ] + }, + "notes": "Status values map to the `hardwareSensorStatus_e` enum: `HW_SENSOR_NONE`, `HW_SENSOR_OK`, `HW_SENSOR_UNAVAILABLE`, `HW_SENSOR_UNHEALTHY`.", + "description": "Provides the hardware status for each individual sensor system." + }, + "MSP_UID": { + "code": 160, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "uid0", + "ctype": "uint32_t", + "desc": "First 32 bits of the unique ID (`U_ID_0`)", + "units": "" + }, + { + "name": "uid1", + "ctype": "uint32_t", + "desc": "Middle 32 bits of the unique ID (`U_ID_1`)", + "units": "" + }, + { + "name": "uid2", + "ctype": "uint32_t", + "desc": "Last 32 bits of the unique ID (`U_ID_2`)", + "units": "" + } + ] + }, + "notes": "Total 12 bytes, representing a 96-bit unique ID.", + "description": "Provides the unique identifier of the microcontroller." + }, + "MSP_GPSSVINFO": { + "code": 164, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "protocolVersion", + "ctype": "uint8_t", + "desc": "Always 1 (Stub version)", + "units": "", + "value": 1 + }, + { + "name": "numChannels", + "ctype": "uint8_t", + "desc": "Always 0 (Number of SV info channels reported)", + "units": "", + "value": 0 + }, + { + "name": "hdopHundredsDigit", + "ctype": "uint8_t", + "desc": "Hundreds digit of HDOP (stub always writes 0)", + "units": "" + }, + { + "name": "hdopTensDigit", + "ctype": "uint8_t", + "desc": "Tens digit of HDOP (`gpsSol.hdop / 100`, truncated)", + "units": "" + }, + { + "name": "hdopUnitsDigit", + "ctype": "uint8_t", + "desc": "Units digit of HDOP (`gpsSol.hdop / 100`, duplicated by stub)", + "units": "" + } + ] + }, + "notes": "Requires `USE_GPS`. This is just a stub in INAV and does not provide actual per-satellite signal info. HDOP digits are not formatted correctly: tens and units both contain `gpsSol.hdop / 100`.", + "description": "Provides satellite signal strength information (legacy U-Blox compatibility stub)." + }, + "MSP_GPSSTATISTICS": { + "code": 166, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "lastMessageDt", + "ctype": "uint16_t", + "desc": "Time since last valid GPS message (`gpsStats.lastMessageDt`)", + "units": "ms" + }, + { + "name": "errors", + "ctype": "uint32_t", + "desc": "Number of GPS communication errors (`gpsStats.errors`)", + "units": "Count" + }, + { + "name": "timeouts", + "ctype": "uint32_t", + "desc": "Number of GPS communication timeouts (`gpsStats.timeouts`)", + "units": "Count" + }, + { + "name": "packetCount", + "ctype": "uint32_t", + "desc": "Number of valid GPS packets received (`gpsStats.packetCount`)", + "units": "Count" + }, + { + "name": "hdop", + "ctype": "uint16_t", + "desc": "Horizontal Dilution of Precision (`gpsSol.hdop`)", + "units": "HDOP * 100" + }, + { + "name": "eph", + "ctype": "uint16_t", + "desc": "Estimated Horizontal Position Accuracy (`gpsSol.eph`)", + "units": "cm" + }, + { + "name": "epv", + "ctype": "uint16_t", + "desc": "Estimated Vertical Position Accuracy (`gpsSol.epv`)", + "units": "cm" + } + ] + }, + "notes": "Requires `USE_GPS`.", + "description": "Provides debugging statistics for the GPS communication link." + }, + "MSP_OSD_VIDEO_CONFIG": { + "code": 180, + "mspv": 1, + "request": null, + "reply": null, + "name": "MSP_OSD_VIDEO_CONFIG", + "missing": true + }, + "MSP_SET_OSD_VIDEO_CONFIG": { + "code": 181, + "mspv": 1, + "request": null, + "reply": null, + "name": "MSP_SET_OSD_VIDEO_CONFIG", + "missing": true + }, + "MSP_DISPLAYPORT": { + "code": 182, + "mspv": 1, + "request": null, + "reply": null, + "name": "MSP_DISPLAYPORT", + "missing": true + }, + "MSP_SET_TX_INFO": { + "code": 186, + "mspv": 1, + "request": { + "payload": [ + { + "name": "rssi", + "ctype": "uint8_t", + "desc": "RSSI value (0-255) provided by the external source; firmware scales it to 10-bit (`value << 2`)", + "units": "Raw" + } + ] + }, + "reply": null, + "notes": "Calls `setRSSIFromMSP()`. Expects 1 byte.", + "description": "Allows a transmitter LUA script (or similar) to send runtime information (currently only RSSI) to the firmware." + }, + "MSP_TX_INFO": { + "code": 187, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "rssiSource", + "ctype": "uint8_t", + "desc": "Enum: Source of the RSSI value (`getRSSISource()`, see `rssiSource_e`)", + "units": "", + "enum": "rssiSource_e" + }, + { + "name": "rtcDateTimeIsSet", + "ctype": "uint8_t", + "desc": "Boolean: 1 if the RTC has been set, 0 otherwise", + "units": "" + } + ] + }, + "notes": "See `rssiSource_e`.", + "description": "Provides information potentially useful for transmitter LUA scripts." + }, + "MSP_SET_RAW_RC": { + "code": 200, + "mspv": 1, + "request": { + "payload": [ + { + "name": "rcChannels", + "desc": "Array of RC channel values (typically 1000-2000). Number of channels determined by payload size", + "ctype": "uint16_t", + "array": true, + "array_size": 0, + "units": "PWM" + } + ] + }, + "reply": null, + "variable_len": true, + "notes": "Requires `USE_RX_MSP`. Maximum channels `MAX_SUPPORTED_RC_CHANNEL_COUNT`. Calls `rxMspFrameReceive()`.", + "description": "Provides raw RC channel data to the flight controller, typically used when the receiver is connected via MSP (e.g., MSP RX feature)." + }, + "MSP_SET_RAW_GPS": { + "code": 201, + "mspv": 1, + "request": { + "payload": [ + { + "name": "fixType", + "ctype": "uint8_t", + "desc": "Enum `gpsFixType_e` GPS fix type", + "units": "Enum", + "enum": "gpsFixType_e" + }, + { + "name": "numSat", + "ctype": "uint8_t", + "desc": "Number of satellites", + "units": "Count" + }, + { + "name": "latitude", + "ctype": "int32_t", + "desc": "Latitude", + "units": "deg * 1e7" + }, + { + "name": "longitude", + "ctype": "int32_t", + "desc": "Longitude", + "units": "deg * 1e7" + }, + { + "name": "altitude", + "ctype": "uint16_t", + "desc": "Altitude in meters (converted to centimeters internally; limited to 0-65535 m)", + "units": "m" + }, + { + "name": "speed", + "ctype": "uint16_t", + "desc": "Ground speed (`gpsSol.groundSpeed`)", + "units": "cm/s" + } + ] + }, + "reply": null, + "notes": "Requires `USE_GPS`. Expects 14 bytes. Updates `gpsSol` structure and calls `onNewGPSData()`. Note the altitude unit mismatch (meters in MSP, cm internal). Does not provide velocity components.", + "description": "Provides raw GPS data to the flight controller, typically for simulation or external GPS injection." + }, + "MSP_SET_BOX": { + "code": 203, + "mspv": 1, + "not_implemented": true, + "request": null, + "reply": null, + "notes": "Not implemented in INAV `fc_msp.c`. Mode changes are typically handled via RC channels (`MSP_MODE_RANGES`).", + "description": "Sets the state of flight modes (boxes). (Likely unused/obsolete in INAV)." + }, + "MSP_SET_RC_TUNING": { + "code": 204, + "mspv": 1, + "request": { + "payload": [ + { + "name": "legacyRcRate", + "ctype": "uint8_t", + "desc": "Ignored", + "units": "" + }, + { + "name": "rcExpo", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->stabilized.rcExpo8`", + "units": "" + }, + { + "name": "rollRate", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->stabilized.rates[FD_ROLL]` (constrained)", + "units": "" + }, + { + "name": "pitchRate", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->stabilized.rates[FD_PITCH]` (constrained)", + "units": "" + }, + { + "name": "yawRate", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->stabilized.rates[FD_YAW]` (constrained)", + "units": "" + }, + { + "name": "dynamicThrottlePID", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->throttle.dynPID` (constrained)", + "units": "" + }, + { + "name": "throttleMid", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->throttle.rcMid8`", + "units": "" + }, + { + "name": "throttleExpo", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->throttle.rcExpo8`", + "units": "" + }, + { + "name": "tpaBreakpoint", + "ctype": "uint16_t", + "desc": "Sets `currentControlRateProfile->throttle.pa_breakpoint`", + "units": "" + }, + { + "name": "rcYawExpo", + "ctype": "uint8_t", + "desc": "(Optional) Sets `currentControlRateProfile->stabilized.rcYawExpo8`", + "units": "", + "optional": true + } + ] + }, + "reply": null, + "notes": "Expects 10 or 11 bytes. Calls `schedulePidGainsUpdate()`. Superseded by `MSP2_INAV_SET_RATE_PROFILE`.", + "description": "Sets RC tuning parameters (rates, expos, TPA) for the current control rate profile." + }, + "MSP_ACC_CALIBRATION": { + "code": 205, + "mspv": 1, + "request": null, + "reply": null, + "notes": "Will fail if armed. Calls `accStartCalibration()`.", + "description": "Starts the accelerometer calibration procedure." + }, + "MSP_MAG_CALIBRATION": { + "code": 206, + "mspv": 1, + "request": null, + "reply": null, + "notes": "Will fail if armed. Enables the `CALIBRATE_MAG` state flag.", + "description": "Starts the magnetometer calibration procedure." + }, + "MSP_SET_MISC": { + "code": 207, + "mspv": 1, + "request": { + "payload": [ + { + "name": "midRc", + "ctype": "uint16_t", + "desc": "Ignored", + "units": "PWM" + }, + { + "name": "legacyMinThrottle", + "ctype": "uint16_t", + "desc": "Ignored" + }, + { + "name": "legacyMaxThrottle", + "ctype": "uint16_t", + "desc": "Ignored" + }, + { + "name": "minCommand", + "ctype": "uint16_t", + "desc": "Sets `motorConfigMutable()->mincommand` (constrained 0-PWM_RANGE_MAX)", + "units": "PWM" + }, + { + "name": "failsafeThrottle", + "ctype": "uint16_t", + "desc": "Sets `currentBatteryProfileMutable->failsafe_throttle` (constrained PWM_RANGE_MIN/MAX)", + "units": "PWM" + }, + { + "name": "gpsType", + "ctype": "uint8_t", + "desc": "Enum `gpsProvider_e` (Sets `gpsConfigMutable()->provider`)", + "units": "Enum", + "enum": "gpsProvider_e" + }, + { + "name": "legacyGpsBaud", + "ctype": "uint8_t", + "desc": "Ignored" + }, + { + "name": "gpsSbasMode", + "ctype": "uint8_t", + "desc": "Enum `sbasMode_e` (Sets `gpsConfigMutable()->sbasMode`)", + "units": "Enum", + "enum": "sbasMode_e" + }, + { + "name": "legacyMwCurrentOut", + "ctype": "uint8_t", + "desc": "Ignored" + }, + { + "name": "rssiChannel", + "ctype": "uint8_t", + "desc": "Sets `rxConfigMutable()->rssi_channel` (constrained 0-MAX_SUPPORTED_RC_CHANNEL_COUNT). Updates source", + "units": "Index" + }, + { + "name": "reserved1", + "ctype": "uint8_t", + "desc": "Ignored" + }, + { + "name": "magDeclination", + "ctype": "uint16_t", + "desc": "Sets `compassConfigMutable()->mag_declination = value * 10` (if `USE_MAG`)", + "units": "0.1 degrees" + }, + { + "name": "vbatScale", + "ctype": "uint8_t", + "desc": "Sets `batteryMetersConfigMutable()->voltage.scale = value * 10` (if `USE_ADC`)", + "units": "Scale / 10" + }, + { + "name": "vbatMinCell", + "ctype": "uint8_t", + "desc": "Sets `currentBatteryProfileMutable->voltage.cellMin = value * 10` (if `USE_ADC`)", + "units": "0.1V" + }, + { + "name": "vbatMaxCell", + "ctype": "uint8_t", + "desc": "Sets `currentBatteryProfileMutable->voltage.cellMax = value * 10` (if `USE_ADC`)", + "units": "0.1V" + }, + { + "name": "vbatWarningCell", + "ctype": "uint8_t", + "desc": "Sets `currentBatteryProfileMutable->voltage.cellWarning = value * 10` (if `USE_ADC`)", + "units": "0.1V" + } + ] + }, + "reply": null, + "notes": "Expects 22 bytes. Superseded by `MSP2_INAV_SET_MISC`.", + "description": "Sets miscellaneous configuration settings (legacy formats/scaling)." + }, + "MSP_RESET_CONF": { + "code": 208, + "mspv": 1, + "request": null, + "reply": null, + "notes": "Will fail if armed. Suspends RX, calls `resetEEPROM()`, `writeEEPROM()`, `readEEPROM()`, resumes RX. Use with caution!", + "description": "Resets all configuration settings to their default values and saves to EEPROM." + }, + "MSP_SET_WP": { + "code": 209, + "mspv": 1, + "request": { + "payload": [ + { + "name": "waypointIndex", + "ctype": "uint8_t", + "desc": "Index of the waypoint to set (0 to `NAV_MAX_WAYPOINTS - 1`)", + "units": "Index" + }, + { + "name": "action", + "ctype": "uint8_t", + "desc": "Enum `navWaypointActions_e` Waypoint action type", + "units": "Enum", + "enum": "navWaypointActions_e" + }, + { + "name": "latitude", + "ctype": "int32_t", + "desc": "Latitude coordinate", + "units": "deg * 1e7" + }, + { + "name": "longitude", + "ctype": "int32_t", + "desc": "Longitude coordinate", + "units": "deg * 1e7" + }, + { + "name": "altitude", + "ctype": "int32_t", + "desc": "Altitude coordinate", + "units": "cm" + }, + { + "name": "param1", + "ctype": "uint16_t", + "desc": "Parameter 1", + "units": "Varies" + }, + { + "name": "param2", + "ctype": "uint16_t", + "desc": "Parameter 2", + "units": "Varies" + }, + { + "name": "param3", + "ctype": "uint16_t", + "desc": "Parameter 3", + "units": "Varies" + }, + { + "name": "flag", + "ctype": "uint8_t", + "desc": "Bitmask: Waypoint flags (`navWaypointFlags_e`)", + "units": "Bitmask", + "bitmask": true, + "enum": "navWaypointFlags_e" + } + ] + }, + "reply": null, + "notes": "Expects 21 bytes. Calls `setWaypoint()`. If `USE_FW_AUTOLAND` is enabled, this also interacts with autoland approach settings based on waypoint index and flags.", + "description": "Sets a single waypoint in the mission plan." + }, + "MSP_SELECT_SETTING": { + "code": 210, + "mspv": 1, + "request": { + "payload": [ + { + "name": "profileIndex", + "ctype": "uint8_t", + "desc": "Index of the profile to activate (0-based)", + "units": "" + } + ] + }, + "reply": null, + "notes": "Will fail if armed. Calls `setConfigProfileAndWriteEEPROM()`.", + "description": "Selects the active configuration profile and saves it." + }, + "MSP_SET_HEAD": { + "code": 211, + "mspv": 1, + "request": { + "payload": [ + { + "name": "heading", + "ctype": "uint16_t", + "desc": "Target heading (0-359)", + "units": "degrees" + } + ] + }, + "reply": null, + "notes": "Expects 2 bytes. Calls `updateHeadingHoldTarget()`.", + "description": "Sets the target heading for the heading hold controller (e.g., during MAG mode)." + }, + "MSP_SET_SERVO_CONFIGURATION": { + "code": 212, + "mspv": 1, + "request": { + "payload": [ + { + "name": "servoIndex", + "ctype": "uint8_t", + "desc": "Index of the servo to configure (0 to `MAX_SUPPORTED_SERVOS - 1`)", + "units": "Index" + }, + { + "name": "min", + "ctype": "uint16_t", + "desc": "Minimum servo endpoint", + "units": "PWM" + }, + { + "name": "max", + "ctype": "uint16_t", + "desc": "Maximum servo endpoint", + "units": "PWM" + }, + { + "name": "middle", + "ctype": "uint16_t", + "desc": "Middle/Neutral servo position", + "units": "PWM" + }, + { + "name": "rate", + "ctype": "uint8_t", + "desc": "Servo rate/scaling", + "units": "%" + }, + { + "name": "reserved1", + "ctype": "uint8_t", + "desc": "Ignored" + }, + { + "name": "reserved2", + "ctype": "uint8_t", + "desc": "Ignored" + }, + { + "name": "legacyForwardChan", + "ctype": "uint8_t", + "desc": "Ignored" + }, + { + "name": "legacyReversedSources", + "ctype": "uint32_t", + "desc": "Ignored" + } + ] + }, + "reply": null, + "notes": "Expects 15 bytes. Returns error if index is invalid. Calls `servoComputeScalingFactors()`. Superseded by `MSP2_INAV_SET_SERVO_CONFIG`.", + "description": "Sets the configuration for a single servo (legacy format)." + }, + "MSP_SET_MOTOR": { + "code": 214, + "mspv": 1, + "request": { + "payload": [ + { + "name": "motorValues", + "desc": "Array of motor values to set when disarmed. Only affects first `MAX_SUPPORTED_MOTORS` entries", + "ctype": "uint16_t", + "array": true, + "array_size": 8, + "units": "PWM" + } + ] + }, + "reply": null, + "notes": "Expects 16 bytes. Modifies the `motor_disarmed` array. These values are *not* saved persistently.", + "description": "Sets the disarmed motor values, typically used for motor testing or propeller balancing functions in a configurator." + }, + "MSP_SET_NAV_CONFIG": { + "code": 215, + "mspv": 1, + "not_implemented": true + }, + "MSP_SET_3D": { + "code": 217, + "mspv": 1, + "request": { + "payload": [ + { + "name": "deadbandLow", + "ctype": "uint16_t", + "desc": "Sets `reversibleMotorsConfigMutable()->deadband_low`", + "units": "PWM" + }, + { + "name": "deadbandHigh", + "ctype": "uint16_t", + "desc": "Sets `reversibleMotorsConfigMutable()->deadband_high`", + "units": "PWM" + }, + { + "name": "neutral", + "ctype": "uint16_t", + "desc": "Sets `reversibleMotorsConfigMutable()->neutral`", + "units": "PWM" + } + ] + }, + "reply": null, + "notes": "Expects 6 bytes. Requires reversible motor support.", + "description": "Sets parameters related to 3D/reversible motor operation." + }, + "MSP_SET_RC_DEADBAND": { + "code": 218, + "mspv": 1, + "request": { + "payload": [ + { + "name": "deadband", + "ctype": "uint8_t", + "desc": "Sets `rcControlsConfigMutable()->deadband`", + "units": "PWM" + }, + { + "name": "yawDeadband", + "ctype": "uint8_t", + "desc": "Sets `rcControlsConfigMutable()->yaw_deadband`", + "units": "PWM" + }, + { + "name": "altHoldDeadband", + "ctype": "uint8_t", + "desc": "Sets `rcControlsConfigMutable()->alt_hold_deadband`", + "units": "PWM" + }, + { + "name": "throttleDeadband", + "ctype": "uint16_t", + "desc": "Sets `rcControlsConfigMutable()->mid_throttle_deadband`", + "units": "PWM" + } + ] + }, + "reply": null, + "notes": "Expects 5 bytes.", + "description": "Sets RC input deadband values." + }, + "MSP_SET_RESET_CURR_PID": { + "code": 219, + "mspv": 1, + "request": null, + "reply": null, + "notes": "Calls `PG_RESET_CURRENT(pidProfile)`. To save, follow with `MSP_EEPROM_WRITE`.", + "description": "Resets the PIDs of the *current* profile to their default values. Does not save." + }, + "MSP_SET_SENSOR_ALIGNMENT": { + "code": 220, + "mspv": 1, + "request": { + "payload": [ + { + "name": "gyroAlign", + "ctype": "uint8_t", + "desc": "Ignored", + "units": "" + }, + { + "name": "accAlign", + "ctype": "uint8_t", + "desc": "Ignored", + "units": "" + }, + { + "name": "magAlign", + "ctype": "uint8_t", + "desc": "Sets `compassConfigMutable()->mag_align` (if `USE_MAG`)", + "units": "" + }, + { + "name": "opflowAlign", + "ctype": "uint8_t", + "desc": "Sets `opticalFlowConfigMutable()->opflow_align` (if `USE_OPFLOW`)", + "units": "" + } + ] + }, + "reply": null, + "notes": "Expects 4 bytes. Use `MSP_SET_BOARD_ALIGNMENT` for primary board orientation.", + "description": "Sets sensor alignment (legacy format)." + }, + "MSP_SET_LED_STRIP_MODECOLOR": { + "code": 221, + "mspv": 1, + "request": { + "payload": [ + { + "name": "modeIndex", + "ctype": "uint8_t", + "desc": "Index of the LED mode (`ledModeIndex_e` or `LED_MODE_COUNT` for special)", + "units": "" + }, + { + "name": "directionOrSpecialIndex", + "ctype": "uint8_t", + "desc": "Index of the direction (`ledDirectionId_e`) or special color (`ledSpecialColorIds_e`)", + "units": "" + }, + { + "name": "colorIndex", + "ctype": "uint8_t", + "desc": "Index of the color to assign from `ledStripConfig()->colors`", + "units": "" + } + ] + }, + "reply": null, + "notes": "Only available if `USE_LED_STRIP` is defined. Expects 3 bytes. Returns error if setting fails (invalid index).", + "description": "Sets the color index for a specific LED mode/function combination." + }, + "MSP_SET_ACC_TRIM": { + "code": 239, + "mspv": 1, + "not_implemented": true, + "request": null, + "reply": null, + "notes": "Not implemented in INAV `fc_msp.c`. Use `MSP_ACC_CALIBRATION`.", + "description": "Sets the accelerometer trim values (leveling calibration)." + }, + "MSP_ACC_TRIM": { + "code": 240, + "mspv": 1, + "not_implemented": true, + "request": null, + "reply": null, + "notes": "Not implemented in INAV `fc_msp.c`. Calibration data via `MSP_CALIBRATION_DATA`.", + "description": "Gets the accelerometer trim values." + }, + "MSP_SERVO_MIX_RULES": { + "code": 241, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "targetChannel", + "ctype": "uint8_t", + "desc": "Servo output channel index (0-based)", + "units": "Index" + }, + { + "name": "inputSource", + "ctype": "uint8_t", + "desc": "Enum `inputSource_e` Input source for the mix (RC chan, Roll, Pitch...)", + "units": "Enum", + "enum": "inputSource_e" + }, + { + "name": "rate", + "ctype": "int16_t", + "desc": "Mixing rate/weight (`-1000` to `+1000`, percent with sign)", + "units": "%" + }, + { + "name": "speed", + "ctype": "uint8_t", + "desc": "Speed/Slew rate limit (`0`=instant, higher slows response)", + "units": "0-255" + }, + { + "name": "reserved1", + "ctype": "uint8_t", + "desc": "Always 0", + "value": 0 + }, + { + "name": "legacyMax", + "ctype": "uint8_t", + "desc": "Always 100 (Legacy)", + "value": 100 + }, + { + "name": "legacyBox", + "ctype": "uint8_t", + "desc": "Always 0 (Legacy)", + "value": 0 + } + ], + "repeating": "MAX_SERVO_RULES" + }, + "variable_len": "MAX_SERVO_RULES", + "notes": "Superseded by `MSP2_INAV_SERVO_MIXER`.", + "description": "Retrieves the custom servo mixer rules (legacy format)." + }, + "MSP_SET_SERVO_MIX_RULE": { + "code": 242, + "mspv": 1, + "request": { + "payload": [ + { + "name": "ruleIndex", + "ctype": "uint8_t", + "desc": "Index of the rule to set (0 to `MAX_SERVO_RULES - 1`)", + "units": "Index" + }, + { + "name": "targetChannel", + "ctype": "uint8_t", + "desc": "Servo output channel index", + "units": "Index" + }, + { + "name": "inputSource", + "ctype": "uint8_t", + "desc": "Enum `inputSource_e` Input source for the mix", + "units": "Enum", + "enum": "inputSource_e" + }, + { + "name": "rate", + "ctype": "int16_t", + "desc": "Mixing rate/weight (`-1000` to `+1000`, percent with sign)", + "units": "%" + }, + { + "name": "speed", + "ctype": "uint8_t", + "desc": "Speed/Slew rate limit (`0`=instant, higher slows response)", + "units": "0-255" + }, + { + "name": "legacyMinMax", + "ctype": "uint16_t", + "desc": "Ignored" + }, + { + "name": "legacyBox", + "ctype": "uint8_t", + "desc": "Ignored" + } + ] + }, + "reply": null, + "notes": "Expects 9 bytes. Returns error if index invalid. Calls `loadCustomServoMixer()`. Superseded by `MSP2_INAV_SET_SERVO_MIXER`.", + "description": "Sets a single custom servo mixer rule (legacy format)." + }, + "MSP_SET_PASSTHROUGH": { + "code": 245, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "status", + "ctype": "uint8_t", + "desc": "1 if passthrough started successfully, 0 on error (e.g., port not found). For 4way, returns number of ESCs found", + "units": "" + } + ] + }, + "notes": "Accepts 0 bytes (defaults to ESC 4-way) or up to 2 bytes for mode/argument. If successful, sets `mspPostProcessFn` to the appropriate handler (`mspSerialPassthroughFn` or `esc4wayProcess`). This handler takes over the serial port after the reply is sent. Requires `USE_SERIAL_4WAY_BLHELI_INTERFACE` for ESC passthrough.", + "description": "Enables serial passthrough mode to peripherals like ESCs (BLHeli 4-way) or other serial devices." + }, + "MSP_RTC": { + "code": 246, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "seconds", + "ctype": "int32_t", + "desc": "Seconds since epoch (or relative time if not set). 0 if RTC time unknown", + "units": "Seconds" + }, + { + "name": "millis", + "ctype": "uint16_t", + "desc": "Millisecond part of the time. 0 if RTC time unknown", + "units": "Milliseconds" + } + ] + }, + "notes": "Requires RTC hardware/support. Returns (0, 0) if time is not available/set.", + "description": "Retrieves the current Real-Time Clock time." + }, + "MSP_SET_RTC": { + "code": 247, + "mspv": 1, + "request": { + "payload": [ + { + "name": "seconds", + "ctype": "int32_t", + "desc": "Seconds component of time to set", + "units": "Seconds" + }, + { + "name": "millis", + "ctype": "uint16_t", + "desc": "Millisecond component of time to set", + "units": "Milliseconds" + } + ] + }, + "reply": null, + "notes": "Requires RTC hardware/support. Expects 6 bytes. Uses `rtcSet()`.", + "description": "Sets the Real-Time Clock time." + }, + "MSP_EEPROM_WRITE": { + "code": 250, + "mspv": 1, + "request": null, + "reply": null, + "notes": "Will fail if armed. Suspends RX, calls `writeEEPROM()`, `readEEPROM()`, resumes RX.", + "description": "Saves the current configuration from RAM to non-volatile memory (EEPROM/Flash)." + }, + "MSP_RESERVE_1": { + "code": 251, + "mspv": 1, + "request": null, + "reply": null, + "name": "MSP_RESERVE_1", + "missing": true + }, + "MSP_RESERVE_2": { + "code": 252, + "mspv": 1, + "request": null, + "reply": null, + "name": "MSP_RESERVE_2", + "missing": true + }, + "MSP_DEBUGMSG": { + "code": 253, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "Message Text", + "desc": "Debug message text (not NUL-terminated). See [serial printf debugging](https://github.com/iNavFlight/inav/blob/master/docs/development/serial_printf_debugging.md)", + "ctype": "char", + "array": true, + "array_size": 0, + "units": "" + } + ] + }, + "variable_len": true, + "notes": "Published via the LOG UART or shared MSP/LOG port using `mspSerialPushPort()`.", + "description": "Retrieves debug (\"serial printf\") messages from the firmware." + }, + "MSP_DEBUG": { + "code": 254, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "debugValues", + "desc": "First 4 values from the `debug` array", + "ctype": "uint16_t", + "array": true, + "array_size": 4, + "units": "" + } + ] + }, + "notes": "Useful for developers. Values are truncated to the lower 16 bits of each `debug[]` entry. See `MSP2_INAV_DEBUG` for full 32-bit values.", + "description": "Retrieves values from the firmware's `debug[]` array (legacy 16-bit version)." + }, + "MSP_V2_FRAME": { + "code": 255, + "mspv": 1, + "request": null, + "reply": null, + "notes": "See MSPv2 documentation for the actual frame structure that follows this indicator.", + "description": "This ID is used as a *payload indicator* within an MSPv1 message structure (`$M>`) to signify that the following payload conforms to the MSPv2 format. It's not a command itself." + }, + "MSP2_COMMON_TZ": { + "code": 4097, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "tzOffsetMinutes", + "ctype": "int16_t", + "desc": "Time zone offset from UTC (`timeConfig()->tz_offset`)", + "units": "Minutes" + }, + { + "name": "tzAutoDst", + "ctype": "uint8_t", + "desc": "Automatic daylight saving time enabled (`timeConfig()->tz_automatic_dst`)", + "units": "Boolean" + } + ] + }, + "notes": "", + "description": "Gets the time zone offset configuration." + }, + "MSP2_COMMON_SET_TZ": { + "code": 4098, + "mspv": 2, + "request": null, + "reply": null, + "notes": "Accepts 2 or 3 bytes.", + "description": "Sets the time zone offset configuration.", + "variants": { + "dataSize == 2": { + "description": "dataSize == 2", + "request": { + "payload": [ + { + "name": "tz_offset", + "ctype": "int16_t", + "desc": "Timezone offset from UTC.", + "units": "minutes" + } + ] + }, + "reply": null + }, + "dataSize == 3": { + "description": "dataSize == 3", + "request": { + "payload": [ + { + "name": "tz_offset", + "ctype": "int16_t", + "desc": "Timezone offset from UTC.", + "units": "minutes" + }, + { + "name": "tz_automatic_dst", + "ctype": "uint8_t", + "desc": "Automatic DST enable (0/1).", + "units": "bool" + } + ] + }, + "reply": null + } + } + }, + "MSP2_COMMON_SETTING": { + "code": 4099, + "mspv": 2, + "request": { + "payload": [ + { + "name": "settingIdentifier", + "ctype": "Varies", + "desc": "Setting name (null-terminated string) OR index selector (`0x00` followed by `uint16_t` index)", + "polymorph": true, + "units": "" + } + ] + }, + "reply": { + "payload": [ + { + "name": "settingValue", + "desc": "Raw byte value of the setting. Size depends on the setting's type (`settingGetValueSize()`)", + "ctype": "uint8_t", + "array": true, + "array_size": 0, + "units": "" + } + ] + }, + "variable_len": true, + "notes": "Returns error if setting not found. Use `MSP2_COMMON_SETTING_INFO` to discover settings, types, and sizes.", + "description": "Gets the value of a specific configuration setting, identified by name or index." + }, + "MSP2_COMMON_SET_SETTING": { + "code": 4100, + "mspv": 2, + "request": { + "payload": [ + { + "name": "settingIdentifier", + "ctype": "Varies", + "desc": "Setting name (null-terminated string) OR Index (0x00 followed by `uint16_t` index)", + "polymorph": true, + "units": "" + }, + { + "name": "settingValue", + "desc": "Raw byte value to set for the setting. Size must match the setting's type", + "ctype": "uint8_t", + "array": true, + "array_size": 0, + "units": "" + } + ] + }, + "reply": null, + "variable_len": true, + "notes": "Performs type checking and range validation (min/max). Returns error if setting not found, value size mismatch, or value out of range. Handles different data types (`uint8`, `int16`, `float`, `string`, etc.) internally.", + "description": "Sets the value of a specific configuration setting, identified by name or index." + }, + "MSP2_COMMON_MOTOR_MIXER": { + "code": 4101, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "motorMix", + "desc": "Weights for a single motor `[throttle, roll, pitch, yaw]`, each encoded as `(mix + 2.0) * 1000` (range 0-4000)", + "ctype": "uint16_t", + "array": true, + "array_size": 4, + "units": "Scaled (0-4000)" + } + ], + "repeating": "MAX_SUPPORTED_MOTORS" + }, + "variable_len": "MAX_SUPPORTED_MOTORS", + "notes": "Scaling is `(float_weight + 2.0) * 1000`. `primaryMotorMixer()` provides the data. If multiple mixer profiles are enabled (`MAX_MIXER_PROFILE_COUNT > 1`), an additional block of mixes for the next profile follows immediately.", + "description": "Retrieves the current motor mixer configuration (throttle, roll, pitch, yaw weights) for each motor." + }, + "MSP2_COMMON_SET_MOTOR_MIXER": { + "code": 4102, + "mspv": 2, + "request": { + "payload": [ + { + "name": "motorIndex", + "ctype": "uint8_t", + "desc": "Index of the motor to configure (0 to `MAX_SUPPORTED_MOTORS - 1`)", + "units": "Index" + }, + { + "name": "throttleWeight", + "ctype": "uint16_t", + "desc": "Sets throttle weight from `(value / 1000.0) - 2.0", + "units": "Scaled (0-4000)" + }, + { + "name": "rollWeight", + "ctype": "uint16_t", + "desc": "Sets roll weight from `(value / 1000.0) - 2.0", + "units": "Scaled (0-4000)" + }, + { + "name": "pitchWeight", + "ctype": "uint16_t", + "desc": "Sets pitch weight from `(value / 1000.0) - 2.0", + "units": "Scaled (0-4000)" + }, + { + "name": "yawWeight", + "ctype": "uint16_t", + "desc": "Sets yaw weight from `(value / 1000.0) - 2.0", + "units": "Scaled (0-4000)" + } + ] + }, + "reply": null, + "notes": "Expects 9 bytes. Modifies `primaryMotorMixerMutable()`. Returns error if index is invalid.", + "description": "Sets the motor mixer weights for a single motor in the primary mixer profile." + }, + "MSP2_COMMON_SETTING_INFO": { + "code": 4103, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "settingName", + "desc": "Null-terminated setting name", + "ctype": "char", + "array": true, + "array_size": 0, + "units": "" + }, + { + "name": "pgn", + "ctype": "uint16_t", + "desc": "Parameter Group Number (PGN) ID", + "units": "" + }, + { + "name": "type", + "ctype": "uint8_t", + "desc": "Variable type (`VAR_UINT8`, `VAR_FLOAT`, etc.)", + "units": "" + }, + { + "name": "section", + "ctype": "uint8_t", + "desc": "Setting section (`MASTER_VALUE`, `PROFILE_VALUE`, etc.)", + "units": "" + }, + { + "name": "mode", + "ctype": "uint8_t", + "desc": "Setting mode (`MODE_NORMAL`, `MODE_LOOKUP`, etc.)", + "units": "" + }, + { + "name": "minValue", + "ctype": "int32_t", + "desc": "Minimum allowed value (as signed 32-bit)", + "units": "" + }, + { + "name": "maxValue", + "ctype": "uint32_t", + "desc": "Maximum allowed value (as unsigned 32-bit)", + "units": "" + }, + { + "name": "settingIndex", + "ctype": "uint16_t", + "desc": "Absolute index of the setting", + "units": "" + }, + { + "name": "profileIndex", + "ctype": "uint8_t", + "desc": "Current profile index (if applicable, else 0)", + "units": "" + }, + { + "name": "profileCount", + "ctype": "uint8_t", + "desc": "Total number of profiles (if applicable, else 0)", + "units": "" + }, + { + "name": "lookupNames", + "desc": "(If `mode == MODE_LOOKUP`) Series of null-terminated strings for each possible value from min to max", + "ctype": "char", + "array": true, + "array_size": 0, + "units": "" + }, + { + "name": "settingValue", + "desc": "Current raw byte value of the setting", + "ctype": "uint8_t", + "array": true, + "array_size": 0, + "units": "" + } + ] + }, + "variable_len": true, + "notes": "", + "description": "Gets detailed information about a specific configuration setting (name, type, range, flags, current value, etc.)." + }, + "MSP2_COMMON_PG_LIST": { + "code": 4104, + "mspv": 2, + "request": { + "payload": [ + { + "name": "pgn", + "ctype": "uint16_t", + "desc": "(Optional) PGN ID to query. If omitted, returns all used PGNs", + "units": "", + "optional": true + } + ] + }, + "reply": { + "payload": [ + { + "name": "pgn", + "ctype": "uint16_t", + "desc": "Parameter Group Number (PGN) ID", + "units": "" + }, + { + "name": "startIndex", + "ctype": "uint16_t", + "desc": "Absolute index of the first setting in this group", + "units": "" + }, + { + "name": "endIndex", + "ctype": "uint16_t", + "desc": "Absolute index of the last setting in this group", + "units": "" + } + ], + "repeating": "for each PGN found" + }, + "variable_len": "for each PGN found", + "notes": "Allows efficient fetching of related settings by group.", + "description": "Gets a list of Parameter Group Numbers (PGNs) used by settings, along with the start and end setting indexes for each group. Can request info for a single PGN." + }, + "MSP2_COMMON_SERIAL_CONFIG": { + "code": 4105, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "identifier", + "ctype": "uint8_t", + "desc": "Port identifier Enum (`serialPortIdentifier_e`)", + "units": "", + "enum": "serialPortIdentifier_e" + }, + { + "name": "functionMask", + "ctype": "uint32_t", + "desc": "Bitmask: enabled functions (`FUNCTION_*`)", + "units": "Bitmask", + "bitmask": true + }, + { + "name": "mspBaudIndex", + "ctype": "uint8_t", + "desc": "Baud rate index for MSP function", + "units": "" + }, + { + "name": "gpsBaudIndex", + "ctype": "uint8_t", + "desc": "Baud rate index for GPS function", + "units": "" + }, + { + "name": "telemetryBaudIndex", + "ctype": "uint8_t", + "desc": "Baud rate index for Telemetry function", + "units": "" + }, + { + "name": "peripheralBaudIndex", + "ctype": "uint8_t", + "desc": "Baud rate index for other peripheral functions", + "units": "" + } + ], + "repeating": "for each available serial port" + }, + "variable_len": "for each available serial port", + "notes": "Baud rate indexes map to actual baud rates (e.g., 9600, 115200). See `baudRates` array.", + "description": "Retrieves the configuration for all available serial ports." + }, + "MSP2_COMMON_SET_SERIAL_CONFIG": { + "code": 4106, + "mspv": 2, + "request": { + "payload": [ + { + "name": "identifier", + "ctype": "uint8_t", + "desc": "Port identifier Enum (`serialPortIdentifier_e`)", + "units": "", + "enum": "serialPortIdentifier_e" + }, + { + "name": "functionMask", + "ctype": "uint32_t", + "desc": "Bitmask: functions to enable", + "units": "Bitmask", + "bitmask": true + }, + { + "name": "mspBaudIndex", + "ctype": "uint8_t", + "desc": "Baud rate index for MSP", + "units": "" + }, + { + "name": "gpsBaudIndex", + "ctype": "uint8_t", + "desc": "Baud rate index for GPS", + "units": "" + }, + { + "name": "telemetryBaudIndex", + "ctype": "uint8_t", + "desc": "Baud rate index for Telemetry", + "units": "" + }, + { + "name": "peripheralBaudIndex", + "ctype": "uint8_t", + "desc": "Baud rate index for peripherals", + "units": "" + } + ], + "repeating": "for each port being configured" + }, + "reply": null, + "variable_len": "for each port being configured", + "notes": "Payload size must be a multiple of the size of one port config entry (1 + 4 + 4 = 9 bytes). Returns error if identifier is invalid or size is incorrect. Baud rate indexes are constrained `BAUD_MIN` to `BAUD_MAX`.", + "description": "Sets the configuration for one or more serial ports." + }, + "MSP2_COMMON_SET_RADAR_POS": { + "code": 4107, + "mspv": 2, + "request": { + "payload": [ + { + "name": "poiIndex", + "ctype": "uint8_t", + "desc": "Index of the POI slot (0 to `RADAR_MAX_POIS - 1`)", + "units": "Index" + }, + { + "name": "state", + "ctype": "uint8_t", + "desc": "Status of the POI (0=undefined, 1=armed, 2=lost)", + "units": "" + }, + { + "name": "latitude", + "ctype": "int32_t", + "desc": "Latitude of the POI", + "units": "deg * 1e7" + }, + { + "name": "longitude", + "ctype": "int32_t", + "desc": "Longitude of the POI", + "units": "deg * 1e7" + }, + { + "name": "altitude", + "ctype": "int32_t", + "desc": "Altitude of the POI", + "units": "cm" + }, + { + "name": "heading", + "ctype": "uint16_t", + "desc": "Heading of the POI", + "units": "degrees" + }, + { + "name": "speed", + "ctype": "uint16_t", + "desc": "Speed of the POI", + "units": "cm/s" + }, + { + "name": "linkQuality", + "ctype": "uint8_t", + "desc": "Link quality indicator", + "units": "0-4" + } + ] + }, + "reply": null, + "notes": "Expects 19 bytes. POI index is clamped to `RADAR_MAX_POIS - 1`. Updates the `radar_pois` array.", + "description": "Sets the position and status information for a \"radar\" Point of Interest (POI). Used for displaying other craft/objects on the OSD map." + }, + "MSP2_COMMON_SET_RADAR_ITD": { + "code": 4108, + "mspv": 2, + "request": null, + "reply": null, + "not_implemented": true, + "notes": "Not implemented in INAV `fc_msp.c`.", + "description": "Sets radar information to display (likely internal/unused)." + }, + "MSP2_COMMON_SET_MSP_RC_LINK_STATS": { + "code": 4109, + "mspv": 2, + "request": { + "payload": [ + { + "name": "sublinkID", + "ctype": "uint8_t", + "desc": "Sublink identifier (usually 0)" + }, + { + "name": "validLink", + "ctype": "uint8_t", + "desc": "Indicates if the link is currently valid (not in failsafe)", + "units": "Boolean" + }, + { + "name": "rssiPercent", + "ctype": "uint8_t", + "desc": "Uplink RSSI percentage (0-100)", + "units": "%" + }, + { + "name": "uplinkRSSI_dBm", + "ctype": "uint8_t", + "desc": "Uplink RSSI in dBm (sent as positive, e.g., 70 means -70dBm)", + "units": "-dBm" + }, + { + "name": "downlinkLQ", + "ctype": "uint8_t", + "desc": "Downlink Link Quality (0-100)", + "units": "%" + }, + { + "name": "uplinkLQ", + "ctype": "uint8_t", + "desc": "Uplink Link Quality (0-100)", + "units": "%" + }, + { + "name": "uplinkSNR", + "ctype": "int8_t", + "desc": "Uplink Signal-to-Noise Ratio", + "units": "dB" + } + ] + }, + "reply": null, + "notes": "Requires `USE_RX_MSP`. Expects at least 7 bytes. Updates `rxLinkStatistics` and sets RSSI via `setRSSIFromMSP_RC()` only if `sublinkID` is 0. This message expects **no reply** (`MSP_RESULT_NO_REPLY`).", + "description": "Provides RC link statistics (RSSI, LQ) to the FC, typically from an MSP-based RC link (like ExpressLRS). Sent periodically by the RC link." + }, + "MSP2_COMMON_SET_MSP_RC_INFO": { + "code": 4110, + "mspv": 2, + "request": { + "payload": [ + { + "name": "sublinkID", + "ctype": "uint8_t", + "desc": "Sublink identifier (usually 0)" + }, + { + "name": "uplinkTxPower", + "ctype": "uint16_t", + "desc": "Uplink transmitter power level", + "units": "mW" + }, + { + "name": "downlinkTxPower", + "ctype": "uint16_t", + "desc": "Downlink transmitter power level", + "units": "mW" + }, + { + "name": "band", + "ctype": "char", + "desc": "Operating band string (e.g., \"2G4\", \"900\"), null-terminated/padded", + "array": true, + "array_size": 4 + }, + { + "name": "mode", + "ctype": "char", + "desc": "Operating mode/rate string (e.g., \"100HZ\", \"F1000\"), null-terminated/padded", + "array": true, + "array_size": 6 + } + ] + }, + "reply": null, + "notes": "Requires `USE_RX_MSP`. Expects at least 15 bytes. Updates `rxLinkStatistics` only if `sublinkID` is 0. Converts band/mode strings to uppercase. This message expects **no reply** (`MSP_RESULT_NO_REPLY`).", + "description": "Provides additional RC link information (power levels, band, mode) to the FC from an MSP-based RC link. Sent less frequently than link stats." + }, + "MSP2_COMMON_GET_RADAR_GPS": { + "code": 4111, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "poiLatitude", + "ctype": "int32_t", + "desc": "Latitude of a radar POI", + "units": "deg * 1e7" + }, + { + "name": "poiLongitude", + "ctype": "int32_t", + "desc": "Longitude of a radar POI", + "units": "deg * 1e7" + }, + { + "name": "poiAltitude", + "ctype": "int32_t", + "desc": "Altitude of a radar POI", + "units": "cm" + } + ], + "repeating": "RADAR_MAX_POIS" + }, + "notes": "Returns the stored GPS coordinates for all radar POIs (`radar_pois[i].gps`).", + "description": "Provides the GPS positions (latitude, longitude, altitude) for each radar point of interest." + }, + "MSP2_SENSOR_RANGEFINDER": { + "code": 7937, + "mspv": 2, + "request": { + "payload": [ + { + "name": "quality", + "ctype": "uint8_t", + "desc": "Quality of the measurement", + "units": "0-255" + }, + { + "name": "distanceMm", + "ctype": "int32_t", + "desc": "Measured distance. Negative value indicates out of range", + "units": "mm" + } + ] + }, + "reply": null, + "notes": "Requires `USE_RANGEFINDER_MSP`. Calls `mspRangefinderReceiveNewData()`.", + "description": "Provides rangefinder data (distance, quality) from an external MSP-based sensor." + }, + "MSP2_SENSOR_OPTIC_FLOW": { + "code": 7938, + "mspv": 2, + "request": { + "payload": [ + { + "name": "quality", + "ctype": "uint8_t", + "desc": "Quality of the measurement (0-255)", + "units": "" + }, + { + "name": "motionX", + "ctype": "int32_t", + "desc": "Raw integrated flow value X", + "units": "" + }, + { + "name": "motionY", + "ctype": "int32_t", + "desc": "Raw integrated flow value Y", + "units": "" + } + ] + }, + "reply": null, + "notes": "Requires `USE_OPFLOW_MSP`. Calls `mspOpflowReceiveNewData()`.", + "description": "Provides optical flow data (motion, quality) from an external MSP-based sensor." + }, + "MSP2_SENSOR_GPS": { + "code": 7939, + "mspv": 2, + "request": { + "payload": [ + { + "name": "instance", + "ctype": "uint8_t", + "desc": "Sensor instance number (for multi-GPS)" + }, + { + "name": "gpsWeek", + "ctype": "uint16_t", + "desc": "GPS week number (0xFFFF if unavailable)" + }, + { + "name": "msTOW", + "ctype": "uint32_t", + "desc": "Milliseconds Time of Week", + "units": "ms" + }, + { + "name": "fixType", + "ctype": "uint8_t", + "desc": "Enum `gpsFixType_e` Type of GPS fix", + "units": "Enum", + "enum": "gpsFixType_e" + }, + { + "name": "satellitesInView", + "ctype": "uint8_t", + "desc": "Number of satellites used in solution", + "units": "Count" + }, + { + "name": "hPosAccuracy", + "ctype": "uint16_t", + "desc": "Horizontal position accuracy estimate in milimeters", + "units": "mm" + }, + { + "name": "vPosAccuracy", + "ctype": "uint16_t", + "desc": "Vertical position accuracy estimate in milimeters", + "units": "mm" + }, + { + "name": "hVelAccuracy", + "ctype": "uint16_t", + "desc": "Horizontal velocity accuracy estimate", + "units": "cm/s" + }, + { + "name": "hdop", + "ctype": "uint16_t", + "desc": "Horizontal Dilution of Precision", + "units": "HDOP * 100" + }, + { + "name": "longitude", + "ctype": "int32_t", + "desc": "Longitude", + "units": "deg * 1e7" + }, + { + "name": "latitude", + "ctype": "int32_t", + "desc": "Latitude", + "units": "deg * 1e7" + }, + { + "name": "mslAltitude", + "ctype": "int32_t", + "desc": "Altitude above Mean Sea Level", + "units": "cm" + }, + { + "name": "nedVelNorth", + "ctype": "int32_t", + "desc": "North velocity (NED frame)", + "units": "cm/s" + }, + { + "name": "nedVelEast", + "ctype": "int32_t", + "desc": "East velocity (NED frame)", + "units": "cm/s" + }, + { + "name": "nedVelDown", + "ctype": "int32_t", + "desc": "Down velocity (NED frame)", + "units": "cm/s" + }, + { + "name": "groundCourse", + "ctype": "uint16_t", + "desc": "Ground course (0-36000)", + "units": "deg * 100" + }, + { + "name": "trueYaw", + "ctype": "uint16_t", + "desc": "True heading/yaw (0-36000, 65535 if unavailable)", + "units": "deg * 100" + }, + { + "name": "year", + "ctype": "uint16_t", + "desc": "Year (e.g., 2023)" + }, + { + "name": "month", + "ctype": "uint8_t", + "desc": "Month (1-12)" + }, + { + "name": "day", + "ctype": "uint8_t", + "desc": "Day of month (1-31)" + }, + { + "name": "hour", + "ctype": "uint8_t", + "desc": "Hour (0-23)" + }, + { + "name": "min", + "ctype": "uint8_t", + "desc": "Minute (0-59)" + }, + { + "name": "sec", + "ctype": "uint8_t", + "desc": "Second (0-59)" + } + ] + }, + "reply": null, + "notes": "Requires `USE_GPS_PROTO_MSP`. Calls `mspGPSReceiveNewData()`.", + "description": "Provides detailed GPS data from an external MSP-based GPS module." + }, + "MSP2_SENSOR_COMPASS": { + "code": 7940, + "mspv": 2, + "request": { + "payload": [ + { + "name": "instance", + "ctype": "uint8_t", + "desc": "Sensor instance number" + }, + { + "name": "timeMs", + "ctype": "uint32_t", + "desc": "Timestamp from the sensor", + "units": "ms" + }, + { + "name": "magX", + "ctype": "int16_t", + "desc": "Front component reading", + "units": "mGauss" + }, + { + "name": "magY", + "ctype": "int16_t", + "desc": "Right component reading", + "units": "mGauss" + }, + { + "name": "magZ", + "ctype": "int16_t", + "desc": "Down component reading", + "units": "mGauss" + } + ] + }, + "reply": null, + "notes": "Requires `USE_MAG_MSP`. Calls `mspMagReceiveNewData()`.", + "description": "Provides magnetometer data from an external MSP-based compass module." + }, + "MSP2_SENSOR_BAROMETER": { + "code": 7941, + "mspv": 2, + "request": { + "payload": [ + { + "name": "instance", + "ctype": "uint8_t", + "desc": "Sensor instance number" + }, + { + "name": "timeMs", + "ctype": "uint32_t", + "desc": "Timestamp from the sensor", + "units": "ms" + }, + { + "name": "pressurePa", + "ctype": "float", + "desc": "Absolute pressure", + "units": "Pa" + }, + { + "name": "temp", + "ctype": "int16_t", + "desc": "Temperature", + "units": "0.01 deg C" + } + ] + }, + "reply": null, + "notes": "Requires `USE_BARO_MSP`. Calls `mspBaroReceiveNewData()`.", + "description": "Provides barometer data from an external MSP-based barometer module." + }, + "MSP2_SENSOR_AIRSPEED": { + "code": 7942, + "mspv": 2, + "request": { + "payload": [ + { + "name": "instance", + "ctype": "uint8_t", + "desc": "Sensor instance number" + }, + { + "name": "timeMs", + "ctype": "uint32_t", + "desc": "Timestamp from the sensor", + "units": "ms" + }, + { + "name": "diffPressurePa", + "ctype": "float", + "desc": "Differential pressure", + "units": "Pa" + }, + { + "name": "temp", + "ctype": "int16_t", + "desc": "Temperature", + "units": "0.01 deg C" + } + ] + }, + "reply": null, + "notes": "Requires `USE_PITOT_MSP`. Calls `mspPitotmeterReceiveNewData()`.", + "description": "Provides airspeed data from an external MSP-based pitot sensor module." + }, + "MSP2_SENSOR_HEADTRACKER": { + "code": 7943, + "mspv": 2, + "request": { + "payload": [ + { + "name": "...", + "ctype": "Varies", + "desc": "", + "polymorph": true, + "units": "Head tracker angles (e.g., int16 Roll, Pitch, Yaw in deci-degrees)" + } + ] + }, + "reply": null, + "variable_len": true, + "notes": "Requires `USE_HEADTRACKER` and `USE_HEADTRACKER_MSP`. Calls `mspHeadTrackerReceiverNewData()`. Payload structure needs verification from `mspHeadTrackerReceiverNewData` implementation.", + "description": "Provides head tracker orientation data." + }, + "MSP2_INAV_STATUS": { + "code": 8192, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "cycleTime", + "ctype": "uint16_t", + "desc": "Main loop cycle time", + "units": "µs" + }, + { + "name": "i2cErrors", + "ctype": "uint16_t", + "desc": "I2C errors", + "units": "Count" + }, + { + "name": "sensorStatus", + "ctype": "uint16_t", + "desc": "Bitmask: Sensor status", + "units": "Bitmask", + "bitmask": true + }, + { + "name": "cpuLoad", + "ctype": "uint16_t", + "desc": "Average system load percentage", + "units": "%" + }, + { + "name": "profileAndBattProfile", + "ctype": "uint8_t", + "desc": "Bits 0-3: Config profile index (`getConfigProfile()`), Bits 4-7: Battery profile index (`getConfigBatteryProfile()`)", + "units": "Packed" + }, + { + "name": "armingFlags", + "ctype": "uint32_t", + "desc": "Bitmask: Full 32-bit flight controller arming flags (`armingFlags`)", + "units": "Bitmask", + "bitmask": true + }, + { + "name": "activeModes", + "ctype": "boxBitmask_t", + "desc": "Bitmask words for active flight modes (`packBoxModeFlags()`)", + "units": "Bitmask", + "bitmask": true + }, + { + "name": "mixerProfile", + "ctype": "uint8_t", + "desc": "Current mixer profile index (`getConfigMixerProfile()`)", + "units": "Index" + } + ] + }, + "notes": "`sensorStatus` bits follow `packSensorStatus()` (bit 15 indicates hardware failure). `profileAndBattProfile` packs the current config profile in the low nibble and the battery profile in the high nibble. `activeModes` is emitted as a little-endian array of 32-bit words sized to `CHECKBOX_ITEM_COUNT`.", + "description": "Provides comprehensive flight controller status, extending `MSP_STATUS_EX` with full arming flags, battery profile, and mixer profile." + }, + "MSP2_INAV_OPTICAL_FLOW": { + "code": 8193, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "quality", + "ctype": "uint8_t", + "desc": "Raw quality indicator from the sensor (`opflow.rawQuality`). 0 if `USE_OPFLOW` disabled", + "units": "0-255" + }, + { + "name": "flowRateX", + "ctype": "int16_t", + "desc": "Optical flow rate X (roll axis) (`RADIANS_TO_DEGREES(opflow.flowRate[X])`). 0 if `USE_OPFLOW` disabled", + "units": "degrees/s" + }, + { + "name": "flowRateY", + "ctype": "int16_t", + "desc": "Optical flow rate Y (pitch axis) (`RADIANS_TO_DEGREES(opflow.flowRate[Y])`). 0 if `USE_OPFLOW` disabled", + "units": "degrees/s" + }, + { + "name": "bodyRateX", + "ctype": "int16_t", + "desc": "Compensated body rate X (roll axis) (`RADIANS_TO_DEGREES(opflow.bodyRate[X])`). 0 if `USE_OPFLOW` disabled", + "units": "degrees/s" + }, + { + "name": "bodyRateY", + "ctype": "int16_t", + "desc": "Compensated body rate Y (pitch axis) (`RADIANS_TO_DEGREES(opflow.bodyRate[Y])`). 0 if `USE_OPFLOW` disabled", + "units": "degrees/s" + } + ] + }, + "notes": "Requires `USE_OPFLOW`.", + "description": "Provides data from the optical flow sensor." + }, + "MSP2_INAV_ANALOG": { + "code": 8194, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "batteryFlags", + "ctype": "uint8_t", + "desc": "Bitmask: Bit0=Full on plug-in, Bit1=Use capacity thresholds, Bits2-3=`batteryState_e` (`getBatteryState()`), Bits4-7=Cell count (`getBatteryCellCount()`)", + "units": "Bitmask", + "bitmask": true + }, + { + "name": "vbat", + "ctype": "uint16_t", + "desc": "Battery voltage (`getBatteryVoltage()`)", + "units": "0.01V" + }, + { + "name": "amperage", + "ctype": "int16_t", + "desc": "Current draw (`getAmperage()`)", + "units": "0.01A" + }, + { + "name": "powerDraw", + "ctype": "uint32_t", + "desc": "Power draw (`getPower()`)", + "units": "0.01W" + }, + { + "name": "mAhDrawn", + "ctype": "uint32_t", + "desc": "Consumed capacity (`getMAhDrawn()`)", + "units": "mAh" + }, + { + "name": "mWhDrawn", + "ctype": "uint32_t", + "desc": "Consumed energy (`getMWhDrawn()`)", + "units": "mWh" + }, + { + "name": "remainingCapacity", + "ctype": "uint32_t", + "desc": "Estimated remaining capacity (`getBatteryRemainingCapacity()`)", + "units": "Capacity unit (`batteryMetersConfig()->capacity_unit`)" + }, + { + "name": "percentageRemaining", + "ctype": "uint8_t", + "desc": "Estimated remaining capacity percentage (`calculateBatteryPercentage()`)", + "units": "%" + }, + { + "name": "rssi", + "ctype": "uint16_t", + "desc": "RSSI value (`getRSSI()`)", + "units": "Raw (0-1023)" + } + ] + }, + "notes": "Requires `USE_CURRENT_METER`/`USE_ADC` for current-related fields; values fall back to zero when unavailable. Capacity fields are reported in the units configured by `batteryMetersConfig()->capacity_unit` (mAh or mWh).", + "description": "Provides detailed analog sensor readings, superseding `MSP_ANALOG` with higher precision and additional fields." + }, + "MSP2_INAV_MISC": { + "code": 8195, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "midRc", + "ctype": "uint16_t", + "desc": "Mid RC value (`PWM_RANGE_MIDDLE`)", + "units": "PWM" + }, + { + "name": "legacyMinThrottle", + "ctype": "uint16_t", + "desc": "Always 0 (Legacy)", + "value": 0 + }, + { + "name": "maxThrottle", + "ctype": "uint16_t", + "desc": "Maximum throttle command (`getMaxThrottle()`)", + "units": "PWM" + }, + { + "name": "minCommand", + "ctype": "uint16_t", + "desc": "Minimum motor command (`motorConfig()->mincommand`)", + "units": "PWM" + }, + { + "name": "failsafeThrottle", + "ctype": "uint16_t", + "desc": "Failsafe throttle level (`currentBatteryProfile->failsafe_throttle`)", + "units": "PWM" + }, + { + "name": "gpsType", + "ctype": "uint8_t", + "desc": "Enum `gpsProvider_e` GPS provider type (`gpsConfig()->provider`). 0 if `USE_GPS` disabled", + "units": "Enum", + "enum": "gpsProvider_e" + }, + { + "name": "legacyGpsBaud", + "ctype": "uint8_t", + "desc": "Always 0 (Legacy)", + "value": 0 + }, + { + "name": "gpsSbasMode", + "ctype": "uint8_t", + "desc": "Enum `sbasMode_e` GPS SBAS mode (`gpsConfig()->sbasMode`). 0 if `USE_GPS` disabled", + "units": "Enum", + "enum": "sbasMode_e" + }, + { + "name": "rssiChannel", + "ctype": "uint8_t", + "desc": "RSSI channel index (1-based, 0 disables) (`rxConfig()->rssi_channel`)", + "units": "Index" + }, + { + "name": "magDeclination", + "ctype": "int16_t", + "desc": "Magnetic declination / 10 (`compassConfig()->mag_declination / 10`). 0 if `USE_MAG` disabled", + "units": "0.1 degrees" + }, + { + "name": "vbatScale", + "ctype": "uint16_t", + "desc": "Voltage scale (`batteryMetersConfig()->voltage.scale`). 0 if `USE_ADC` disabled", + "units": "Scale" + }, + { + "name": "vbatSource", + "ctype": "uint8_t", + "desc": "Enum `batVoltageSource_e` Voltage source (`batteryMetersConfig()->voltageSource`). 0 if `USE_ADC` disabled", + "units": "Enum", + "enum": "batVoltageSource_e" + }, + { + "name": "cellCount", + "ctype": "uint8_t", + "desc": "Configured cell count (`currentBatteryProfile->cells`). 0 if `USE_ADC` disabled", + "units": "Count" + }, + { + "name": "vbatCellDetect", + "ctype": "uint16_t", + "desc": "Cell detection voltage (`currentBatteryProfile->voltage.cellDetect`). 0 if `USE_ADC` disabled", + "units": "0.01V" + }, + { + "name": "vbatMinCell", + "ctype": "uint16_t", + "desc": "Min cell voltage (`currentBatteryProfile->voltage.cellMin`). 0 if `USE_ADC` disabled", + "units": "0.01V" + }, + { + "name": "vbatMaxCell", + "ctype": "uint16_t", + "desc": "Max cell voltage (`currentBatteryProfile->voltage.cellMax`). 0 if `USE_ADC` disabled", + "units": "0.01V" + }, + { + "name": "vbatWarningCell", + "ctype": "uint16_t", + "desc": "Warning cell voltage (`currentBatteryProfile->voltage.cellWarning`). 0 if `USE_ADC` disabled", + "units": "0.01V" + }, + { + "name": "capacityValue", + "ctype": "uint32_t", + "desc": "Battery capacity (`currentBatteryProfile->capacity.value`)", + "units": "mAh/mWh" + }, + { + "name": "capacityWarning", + "ctype": "uint32_t", + "desc": "Capacity warning threshold (`currentBatteryProfile->capacity.warning`)", + "units": "mAh/mWh" + }, + { + "name": "capacityCritical", + "ctype": "uint32_t", + "desc": "Capacity critical threshold (`currentBatteryProfile->capacity.critical`)", + "units": "mAh/mWh" + }, + { + "name": "capacityUnit", + "ctype": "uint8_t", + "desc": "Enum `batCapacityUnit_e` Capacity unit (`batteryMetersConfig()->capacity_unit`)", + "units": "Enum", + "enum": "batCapacityUnit_e" + } + ] + }, + "notes": "", + "description": "Retrieves miscellaneous configuration settings, superseding `MSP_MISC` with higher precision and capacity fields." + }, + "MSP2_INAV_SET_MISC": { + "code": 8196, + "mspv": 2, + "request": { + "payload": [ + { + "name": "midRc", + "ctype": "uint16_t", + "desc": "Ignored", + "units": "PWM" + }, + { + "name": "legacyMinThrottle", + "ctype": "uint16_t", + "desc": "Ignored" + }, + { + "name": "legacyMaxThrottle", + "ctype": "uint16_t", + "desc": "Ignored" + }, + { + "name": "minCommand", + "ctype": "uint16_t", + "desc": "Sets `motorConfigMutable()->mincommand` (constrained)", + "units": "PWM" + }, + { + "name": "failsafeThrottle", + "ctype": "uint16_t", + "desc": "Sets `currentBatteryProfileMutable->failsafe_throttle` (constrained)", + "units": "PWM" + }, + { + "name": "gpsType", + "ctype": "uint8_t", + "desc": "Enum `gpsProvider_e` Sets `gpsConfigMutable()->provider` (if `USE_GPS`)", + "units": "Enum", + "enum": "gpsProvider_e" + }, + { + "name": "legacyGpsBaud", + "ctype": "uint8_t", + "desc": "Ignored" + }, + { + "name": "gpsSbasMode", + "ctype": "uint8_t", + "desc": "Enum `sbasMode_e` Sets `gpsConfigMutable()->sbasMode` (if `USE_GPS`)", + "units": "Enum", + "enum": "sbasMode_e" + }, + { + "name": "rssiChannel", + "ctype": "uint8_t", + "desc": "Sets `rxConfigMutable()->rssi_channel` (1-based, 0 disables) when <= `MAX_SUPPORTED_RC_CHANNEL_COUNT`", + "units": "Index" + }, + { + "name": "magDeclination", + "ctype": "int16_t", + "desc": "Sets `compassConfigMutable()->mag_declination = value * 10` (if `USE_MAG`)", + "units": "0.1 degrees" + }, + { + "name": "vbatScale", + "ctype": "uint16_t", + "desc": "Sets `batteryMetersConfigMutable()->voltage.scale` (if `USE_ADC`)", + "units": "Scale" + }, + { + "name": "vbatSource", + "ctype": "uint8_t", + "desc": "Enum `batVoltageSource_e` Sets `batteryMetersConfigMutable()->voltageSource` (if `USE_ADC`, validated)", + "units": "Enum", + "enum": "batVoltageSource_e" + }, + { + "name": "cellCount", + "ctype": "uint8_t", + "desc": "Sets `currentBatteryProfileMutable->cells` (if `USE_ADC`)", + "units": "Count" + }, + { + "name": "vbatCellDetect", + "ctype": "uint16_t", + "desc": "Sets `currentBatteryProfileMutable->voltage.cellDetect` (if `USE_ADC`)", + "units": "0.01V" + }, + { + "name": "vbatMinCell", + "ctype": "uint16_t", + "desc": "Sets `currentBatteryProfileMutable->voltage.cellMin` (if `USE_ADC`)", + "units": "0.01V" + }, + { + "name": "vbatMaxCell", + "ctype": "uint16_t", + "desc": "Sets `currentBatteryProfileMutable->voltage.cellMax` (if `USE_ADC`)", + "units": "0.01V" + }, + { + "name": "vbatWarningCell", + "ctype": "uint16_t", + "desc": "Sets `currentBatteryProfileMutable->voltage.cellWarning` (if `USE_ADC`)", + "units": "0.01V" + }, + { + "name": "capacityValue", + "ctype": "uint32_t", + "desc": "Sets `currentBatteryProfileMutable->capacity.value`", + "units": "mAh/mWh" + }, + { + "name": "capacityWarning", + "ctype": "uint32_t", + "desc": "Sets `currentBatteryProfileMutable->capacity.warning`", + "units": "mAh/mWh" + }, + { + "name": "capacityCritical", + "ctype": "uint32_t", + "desc": "Sets `currentBatteryProfileMutable->capacity.critical`", + "units": "mAh/mWh" + }, + { + "name": "capacityUnit", + "ctype": "uint8_t", + "desc": "Enum `batCapacityUnit_e` Sets `batteryMetersConfigMutable()->capacity_unit` (validated, updates OSD energy unit if changed)", + "units": "Enum", + "enum": "batCapacityUnit_e" + } + ] + }, + "reply": null, + "notes": "Expects 41 bytes. Performs validation on `vbatSource` and `capacityUnit`.", + "description": "Sets miscellaneous configuration settings, superseding `MSP_SET_MISC`." + }, + "MSP2_INAV_BATTERY_CONFIG": { + "code": 8197, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "vbatScale", + "ctype": "uint16_t", + "desc": "Voltage scale (`batteryMetersConfig()->voltage.scale`)", + "units": "Scale" + }, + { + "name": "vbatSource", + "ctype": "uint8_t", + "desc": "Enum `batVoltageSource_e` Voltage source (`batteryMetersConfig()->voltageSource`)", + "units": "Enum", + "enum": "batVoltageSource_e" + }, + { + "name": "cellCount", + "ctype": "uint8_t", + "desc": "Configured cell count (`currentBatteryProfile->cells`)", + "units": "Count" + }, + { + "name": "vbatCellDetect", + "ctype": "uint16_t", + "desc": "Cell detection voltage (`currentBatteryProfile->voltage.cellDetect`)", + "units": "0.01V" + }, + { + "name": "vbatMinCell", + "ctype": "uint16_t", + "desc": "Min cell voltage (`currentBatteryProfile->voltage.cellMin`)", + "units": "0.01V" + }, + { + "name": "vbatMaxCell", + "ctype": "uint16_t", + "desc": "Max cell voltage (`currentBatteryProfile->voltage.cellMax`)", + "units": "0.01V" + }, + { + "name": "vbatWarningCell", + "ctype": "uint16_t", + "desc": "Warning cell voltage (`currentBatteryProfile->voltage.cellWarning`)", + "units": "0.01V" + }, + { + "name": "currentOffset", + "ctype": "int16_t", + "desc": "Current sensor offset (`batteryMetersConfig()->current.offset`)", + "units": "mV" + }, + { + "name": "currentScale", + "ctype": "int16_t", + "desc": "Current sensor scale (`batteryMetersConfig()->current.scale`)", + "units": "0.1 mV/A" + }, + { + "name": "capacityValue", + "ctype": "uint32_t", + "desc": "Battery capacity (`currentBatteryProfile->capacity.value`)", + "units": "mAh/mWh" + }, + { + "name": "capacityWarning", + "ctype": "uint32_t", + "desc": "Capacity warning threshold (`currentBatteryProfile->capacity.warning`)", + "units": "mAh/mWh" + }, + { + "name": "capacityCritical", + "ctype": "uint32_t", + "desc": "Capacity critical threshold (`currentBatteryProfile->capacity.critical`)", + "units": "mAh/mWh" + }, + { + "name": "capacityUnit", + "ctype": "uint8_t", + "desc": "Enum `batCapacityUnit_e` Capacity unit (`batteryMetersConfig()->capacity_unit`)", + "units": "Enum", + "enum": "batCapacityUnit_e" + } + ] + }, + "notes": "Fields are 0 if `USE_ADC` is not defined.", + "description": "Retrieves the configuration specific to the battery voltage and current sensors and capacity settings for the current battery profile." + }, + "MSP2_INAV_SET_BATTERY_CONFIG": { + "code": 8198, + "mspv": 2, + "request": { + "payload": [ + { + "name": "vbatScale", + "ctype": "uint16_t", + "desc": "Sets `batteryMetersConfigMutable()->voltage.scale` (if `USE_ADC`)", + "units": "Scale" + }, + { + "name": "vbatSource", + "ctype": "uint8_t", + "desc": "Enum `batVoltageSource_e` Sets `batteryMetersConfigMutable()->voltageSource` (if `USE_ADC`, validated)", + "units": "Enum", + "enum": "batVoltageSource_e" + }, + { + "name": "cellCount", + "ctype": "uint8_t", + "desc": "Sets `currentBatteryProfileMutable->cells` (if `USE_ADC`)", + "units": "Count" + }, + { + "name": "vbatCellDetect", + "ctype": "uint16_t", + "desc": "Sets `currentBatteryProfileMutable->voltage.cellDetect` (if `USE_ADC`)", + "units": "0.01V" + }, + { + "name": "vbatMinCell", + "ctype": "uint16_t", + "desc": "Sets `currentBatteryProfileMutable->voltage.cellMin` (if `USE_ADC`)", + "units": "0.01V" + }, + { + "name": "vbatMaxCell", + "ctype": "uint16_t", + "desc": "Sets `currentBatteryProfileMutable->voltage.cellMax` (if `USE_ADC`)", + "units": "0.01V" + }, + { + "name": "vbatWarningCell", + "ctype": "uint16_t", + "desc": "Sets `currentBatteryProfileMutable->voltage.cellWarning` (if `USE_ADC`)", + "units": "0.01V" + }, + { + "name": "currentOffset", + "ctype": "int16_t", + "desc": "Sets `batteryMetersConfigMutable()->current.offset`", + "units": "mV" + }, + { + "name": "currentScale", + "ctype": "int16_t", + "desc": "Sets `batteryMetersConfigMutable()->current.scale`", + "units": "0.1 mV/A" + }, + { + "name": "capacityValue", + "ctype": "uint32_t", + "desc": "Sets `currentBatteryProfileMutable->capacity.value`", + "units": "mAh/mWh" + }, + { + "name": "capacityWarning", + "ctype": "uint32_t", + "desc": "Sets `currentBatteryProfileMutable->capacity.warning`", + "units": "mAh/mWh" + }, + { + "name": "capacityCritical", + "ctype": "uint32_t", + "desc": "Sets `currentBatteryProfileMutable->capacity.critical`", + "units": "mAh/mWh" + }, + { + "name": "capacityUnit", + "ctype": "uint8_t", + "desc": "Enum `batCapacityUnit_e` Sets `batteryMetersConfigMutable()->capacity_unit` (validated, updates OSD energy unit if changed)", + "units": "Enum", + "enum": "batCapacityUnit_e" + } + ] + }, + "reply": null, + "notes": "Expects 29 bytes. Performs validation on `vbatSource` and `capacityUnit`.", + "description": "Sets the battery voltage/current sensor configuration and capacity settings for the current battery profile." + }, + "MSP2_INAV_RATE_PROFILE": { + "code": 8199, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "throttleMid", + "ctype": "uint8_t", + "desc": "Throttle Midpoint (`currentControlRateProfile->throttle.rcMid8`)", + "units": "" + }, + { + "name": "throttleExpo", + "ctype": "uint8_t", + "desc": "Throttle Expo (`currentControlRateProfile->throttle.rcExpo8`)", + "units": "" + }, + { + "name": "dynamicThrottlePID", + "ctype": "uint8_t", + "desc": "TPA value (`currentControlRateProfile->throttle.dynPID`)", + "units": "" + }, + { + "name": "tpaBreakpoint", + "ctype": "uint16_t", + "desc": "TPA breakpoint (`currentControlRateProfile->throttle.pa_breakpoint`)", + "units": "" + }, + { + "name": "stabRcExpo", + "ctype": "uint8_t", + "desc": "Stabilized Roll/Pitch Expo (`currentControlRateProfile->stabilized.rcExpo8`)", + "units": "" + }, + { + "name": "stabRcYawExpo", + "ctype": "uint8_t", + "desc": "Stabilized Yaw Expo (`currentControlRateProfile->stabilized.rcYawExpo8`)", + "units": "" + }, + { + "name": "stabRollRate", + "ctype": "uint8_t", + "desc": "Stabilized Roll Rate (`currentControlRateProfile->stabilized.rates[FD_ROLL]`)", + "units": "" + }, + { + "name": "stabPitchRate", + "ctype": "uint8_t", + "desc": "Stabilized Pitch Rate (`currentControlRateProfile->stabilized.rates[FD_PITCH]`)", + "units": "" + }, + { + "name": "stabYawRate", + "ctype": "uint8_t", + "desc": "Stabilized Yaw Rate (`currentControlRateProfile->stabilized.rates[FD_YAW]`)", + "units": "" + }, + { + "name": "manualRcExpo", + "ctype": "uint8_t", + "desc": "Manual Roll/Pitch Expo (`currentControlRateProfile->manual.rcExpo8`)", + "units": "" + }, + { + "name": "manualRcYawExpo", + "ctype": "uint8_t", + "desc": "Manual Yaw Expo (`currentControlRateProfile->manual.rcYawExpo8`)", + "units": "" + }, + { + "name": "manualRollRate", + "ctype": "uint8_t", + "desc": "Manual Roll Rate (`currentControlRateProfile->manual.rates[FD_ROLL]`)", + "units": "" + }, + { + "name": "manualPitchRate", + "ctype": "uint8_t", + "desc": "Manual Pitch Rate (`currentControlRateProfile->manual.rates[FD_PITCH]`)", + "units": "" + }, + { + "name": "manualYawRate", + "ctype": "uint8_t", + "desc": "Manual Yaw Rate (`currentControlRateProfile->manual.rates[FD_YAW]`)", + "units": "" + } + ] + }, + "notes": "", + "description": "Retrieves the rates and expos for the current control rate profile, including both stabilized and manual flight modes. Supersedes `MSP_RC_TUNING`." + }, + "MSP2_INAV_SET_RATE_PROFILE": { + "code": 8200, + "mspv": 2, + "request": { + "payload": [ + { + "name": "throttleMid", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->throttle.rcMid8`", + "units": "" + }, + { + "name": "throttleExpo", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->throttle.rcExpo8`", + "units": "" + }, + { + "name": "dynamicThrottlePID", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->throttle.dynPID`", + "units": "" + }, + { + "name": "tpaBreakpoint", + "ctype": "uint16_t", + "desc": "Sets `currentControlRateProfile->throttle.pa_breakpoint`", + "units": "" + }, + { + "name": "stabRcExpo", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->stabilized.rcExpo8`", + "units": "" + }, + { + "name": "stabRcYawExpo", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->stabilized.rcYawExpo8`", + "units": "" + }, + { + "name": "stabRollRate", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->stabilized.rates[FD_ROLL]` (constrained)", + "units": "" + }, + { + "name": "stabPitchRate", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->stabilized.rates[FD_PITCH]` (constrained)", + "units": "" + }, + { + "name": "stabYawRate", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->stabilized.rates[FD_YAW]` (constrained)", + "units": "" + }, + { + "name": "manualRcExpo", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->manual.rcExpo8`", + "units": "" + }, + { + "name": "manualRcYawExpo", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->manual.rcYawExpo8`", + "units": "" + }, + { + "name": "manualRollRate", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->manual.rates[FD_ROLL]` (constrained)", + "units": "" + }, + { + "name": "manualPitchRate", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->manual.rates[FD_PITCH]` (constrained)", + "units": "" + }, + { + "name": "manualYawRate", + "ctype": "uint8_t", + "desc": "Sets `currentControlRateProfile->manual.rates[FD_YAW]` (constrained)", + "units": "" + } + ] + }, + "reply": null, + "notes": "Expects 15 bytes. Constraints applied to rates based on axis.", + "description": "Sets the rates and expos for the current control rate profile (stabilized and manual). Supersedes `MSP_SET_RC_TUNING`." + }, + "MSP2_INAV_AIR_SPEED": { + "code": 8201, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "airspeed", + "ctype": "uint32_t", + "desc": "Estimated/measured airspeed (`getAirspeedEstimate()`, cm/s). 0 if unavailable", + "units": "cm/s" + } + ] + }, + "notes": "Requires `USE_PITOT`; returns 0 when pitot functionality is not enabled or calibrated.", + "description": "Retrieves the estimated or measured airspeed." + }, + "MSP2_INAV_OUTPUT_MAPPING": { + "code": 8202, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "usageFlags", + "ctype": "uint8_t", + "desc": "Timer usage flags (lower 8 bits of `timerHardware[i].usageFlags`, e.g. `TIM_USE_MOTOR`, `TIM_USE_SERVO`)", + "units": "" + } + ], + "repeating": "for each Motor/Servo timer" + }, + "variable_len": "for each Motor/Servo timer", + "notes": "Superseded by `MSP2_INAV_OUTPUT_MAPPING_EXT2`. Only includes timers *not* used for PPM/PWM input.", + "description": "Retrieves the output mapping configuration (identifies which timer outputs are used for Motors/Servos). Legacy version sending only 8-bit usage flags." + }, + "MSP2_INAV_MC_BRAKING": { + "code": 8203, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "brakingSpeedThreshold", + "ctype": "uint16_t", + "desc": "Speed above which braking engages (`navConfig()->mc.braking_speed_threshold`)", + "units": "cm/s" + }, + { + "name": "brakingDisengageSpeed", + "ctype": "uint16_t", + "desc": "Speed below which braking disengages (`navConfig()->mc.braking_disengage_speed`)", + "units": "cm/s" + }, + { + "name": "brakingTimeout", + "ctype": "uint16_t", + "desc": "Timeout before braking force reduces (`navConfig()->mc.braking_timeout`)", + "units": "ms" + }, + { + "name": "brakingBoostFactor", + "ctype": "uint8_t", + "desc": "Boost factor applied during braking (`navConfig()->mc.braking_boost_factor`)", + "units": "%" + }, + { + "name": "brakingBoostTimeout", + "ctype": "uint16_t", + "desc": "Timeout for the boost factor (`navConfig()->mc.braking_boost_timeout`)", + "units": "ms" + }, + { + "name": "brakingBoostSpeedThreshold", + "ctype": "uint16_t", + "desc": "Speed threshold for boost engagement (`navConfig()->mc.braking_boost_speed_threshold`)", + "units": "cm/s" + }, + { + "name": "brakingBoostDisengageSpeed", + "ctype": "uint16_t", + "desc": "Speed threshold for boost disengagement (`navConfig()->mc.braking_boost_disengage_speed`)", + "units": "cm/s" + }, + { + "name": "brakingBankAngle", + "ctype": "uint8_t", + "desc": "Maximum bank angle allowed during braking (`navConfig()->mc.braking_bank_angle`)", + "units": "degrees" + } + ] + }, + "notes": "Payload is empty if `USE_MR_BRAKING_MODE` is not defined.", + "description": "Retrieves configuration parameters for the multirotor braking mode feature." + }, + "MSP2_INAV_SET_MC_BRAKING": { + "code": 8204, + "mspv": 2, + "request": { + "payload": [ + { + "name": "brakingSpeedThreshold", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->mc.braking_speed_threshold`", + "units": "cm/s" + }, + { + "name": "brakingDisengageSpeed", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->mc.braking_disengage_speed`", + "units": "cm/s" + }, + { + "name": "brakingTimeout", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->mc.braking_timeout`", + "units": "ms" + }, + { + "name": "brakingBoostFactor", + "ctype": "uint8_t", + "desc": "Sets `navConfigMutable()->mc.braking_boost_factor`", + "units": "%" + }, + { + "name": "brakingBoostTimeout", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->mc.braking_boost_timeout`", + "units": "ms" + }, + { + "name": "brakingBoostSpeedThreshold", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->mc.braking_boost_speed_threshold`", + "units": "cm/s" + }, + { + "name": "brakingBoostDisengageSpeed", + "ctype": "uint16_t", + "desc": "Sets `navConfigMutable()->mc.braking_boost_disengage_speed`", + "units": "cm/s" + }, + { + "name": "brakingBankAngle", + "ctype": "uint8_t", + "desc": "Sets `navConfigMutable()->mc.braking_bank_angle`", + "units": "degrees" + } + ] + }, + "reply": null, + "notes": "Expects 14 bytes. Returns error if `USE_MR_BRAKING_MODE` is not defined.", + "description": "Sets configuration parameters for the multirotor braking mode feature." + }, + "MSP2_INAV_OUTPUT_MAPPING_EXT": { + "code": 8205, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "timerId", + "ctype": "uint8_t", + "desc": "Hardware timer identifier (e.g., `TIM1`, `TIM2`). Value depends on target", + "units": "" + }, + { + "name": "usageFlags", + "ctype": "uint8_t", + "desc": "Timer usage flags (lower 8 bits of `timerHardware[i].usageFlags`, e.g. `TIM_USE_MOTOR`, `TIM_USE_SERVO`)", + "units": "" + } + ], + "repeating": "for each Motor/Servo timer" + }, + "variable_len": "for each Motor/Servo timer", + "notes": "Usage flags are truncated to 8 bits. `timerId` mapping is target-specific.", + "description": "Retrieves extended output mapping configuration (timer ID and usage flags). Obsolete, use `MSP2_INAV_OUTPUT_MAPPING_EXT2`." + }, + "MSP2_INAV_TIMER_OUTPUT_MODE": { + "code": 8206, + "mspv": 2, + "request": null, + "reply": null, + "variable_len": true, + "notes": "Non-SITL only. HARDWARE_TIMER_DEFINITION_COUNT is target specific. Returns MSP_RESULT_ACK on success, MSP_RESULT_ERROR on invalid timer index.", + "description": "Reads timer output mode overrides.", + "variants": { + "dataSize == 0": { + "description": "List all timers", + "request": null, + "reply": { + "payload": [ + { + "name": "timerIndex", + "ctype": "uint8_t", + "desc": "Timer index" + }, + { + "name": "outputMode", + "ctype": "uint8_t", + "enum": "outputMode_e", + "desc": "OUTPUT_MODE_*" + } + ], + "repeating": "HARDWARE_TIMER_DEFINITION_COUNT" + } + }, + "dataSize == 1": { + "description": "Query one timer", + "request": { + "payload": [ + { + "name": "timerIndex", + "ctype": "uint8_t", + "desc": "0..HARDWARE_TIMER_DEFINITION_COUNT-1" + } + ] + }, + "reply": { + "payload": [ + { + "name": "timerIndex", + "ctype": "uint8_t", + "desc": "Echoed timer index" + }, + { + "name": "outputMode", + "ctype": "uint8_t", + "enum": "outputMode_e", + "desc": "OUTPUT_MODE_*" + } + ] + } + } + } + }, + "MSP2_INAV_SET_TIMER_OUTPUT_MODE": { + "code": 8207, + "mspv": 2, + "request": { + "payload": [ + { + "name": "timerIndex", + "ctype": "uint8_t", + "desc": "Index of the hardware timer definition", + "units": "" + }, + { + "name": "outputMode", + "ctype": "uint8_t", + "desc": "Output mode override (`outputMode_e` enum) to set", + "units": "", + "enum": "outputMode_e" + } + ] + }, + "reply": null, + "notes": "Only available on non-SITL builds. Expects 2 bytes. Returns error if `timerIndex` is invalid.", + "description": "Set the output mode override for a specific hardware timer." + }, + "MSP2_INAV_MIXER": { + "code": 8208, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "motorDirectionInverted", + "ctype": "uint8_t", + "desc": "Boolean: 1 if motor direction is reversed globally (`mixerConfig()->motorDirectionInverted`)", + "units": "" + }, + { + "name": "reserved1", + "ctype": "uint8_t", + "desc": "Always 0 (Was yaw jump prevention limit)", + "units": "", + "value": 0 + }, + { + "name": "motorStopOnLow", + "ctype": "uint8_t", + "desc": "Boolean: 1 if motors stop at minimum throttle (`mixerConfig()->motorstopOnLow`)", + "units": "" + }, + { + "name": "platformType", + "ctype": "uint8_t", + "desc": "Enum (`mixerConfig()->platformType`)", + "units": "", + "enum": "flyingPlatformType_e" + }, + { + "name": "hasFlaps", + "ctype": "uint8_t", + "desc": "Boolean: 1 if the current mixer configuration includes flaps (`mixerConfig()->hasFlaps`)", + "units": "" + }, + { + "name": "appliedMixerPreset", + "ctype": "int16_t", + "desc": "Enum (`mixerPreset_e`): Mixer preset currently applied (`mixerConfig()->appliedMixerPreset`) WARNING: cannot figure where this is", + "units": "", + "enum": "mixerPreset_e" + }, + { + "name": "maxMotors", + "ctype": "uint8_t", + "desc": "Constant: Maximum motors supported (`MAX_SUPPORTED_MOTORS`)", + "units": "" + }, + { + "name": "maxServos", + "ctype": "uint8_t", + "desc": "Constant: Maximum servos supported (`MAX_SUPPORTED_SERVOS`)", + "units": "" + } + ] + }, + "notes": "", + "description": "Retrieves INAV-specific mixer configuration details." + }, + "MSP2_INAV_SET_MIXER": { + "code": 8209, + "mspv": 2, + "request": { + "payload": [ + { + "name": "motorDirectionInverted", + "ctype": "uint8_t", + "desc": "Sets `mixerConfigMutable()->motorDirectionInverted`", + "units": "" + }, + { + "name": "reserved1", + "ctype": "uint8_t", + "desc": "Ignored", + "units": "" + }, + { + "name": "motorStopOnLow", + "ctype": "uint8_t", + "desc": "Sets `mixerConfigMutable()->motorstopOnLow`", + "units": "" + }, + { + "name": "platformType", + "ctype": "uint8_t", + "desc": "Sets `mixerConfigMutable()->platformType`", + "units": "", + "enum": "flyingPlatformType_e" + }, + { + "name": "hasFlaps", + "ctype": "uint8_t", + "desc": "Sets `mixerConfigMutable()->hasFlaps`", + "units": "" + }, + { + "name": "appliedMixerPreset", + "ctype": "int16_t", + "desc": "Sets `mixerConfigMutable()->appliedMixerPreset`", + "units": "" + }, + { + "name": "maxMotors", + "ctype": "uint8_t", + "desc": "Ignored", + "units": "" + }, + { + "name": "maxServos", + "ctype": "uint8_t", + "desc": "Ignored", + "units": "" + } + ] + }, + "reply": null, + "notes": "Expects 9 bytes. Calls `mixerUpdateStateFlags()`.", + "description": "Sets INAV-specific mixer configuration details." + }, + "MSP2_INAV_OSD_LAYOUTS": { + "code": 8210, + "mspv": 2, + "request": null, + "reply": null, + "variants": { + "dataSize == 0": { + "description": "Query layout/item counts", + "request": null, + "reply": { + "payload": [ + { + "name": "layoutCount", + "ctype": "uint8_t", + "desc": "Number of OSD layouts (`OSD_LAYOUT_COUNT`)" + }, + { + "name": "itemCount", + "ctype": "uint8_t", + "desc": "Number of OSD items per layout (`OSD_ITEM_COUNT`)" + } + ] + } + }, + "dataSize == 1": { + "description": "Fetch all item positions for a layout", + "request": { + "payload": [ + { + "name": "layoutIndex", + "ctype": "uint8_t", + "desc": "Layout index (0 to `OSD_LAYOUT_COUNT - 1`)" + } + ] + }, + "reply": { + "payload": [ + { + "name": "itemPosition", + "ctype": "uint16_t", + "desc": "Packed X/Y position (`osdLayoutsConfig()->item_pos[layoutIndex][item]`)", + "units": "packed coords" + } + ], + "repeating": "OSD_ITEM_COUNT" + } + }, + "dataSize == 3": { + "description": "Fetch a single item position", + "request": { + "payload": [ + { + "name": "layoutIndex", + "ctype": "uint8_t", + "desc": "Layout index (0 to `OSD_LAYOUT_COUNT - 1`)" + }, + { + "name": "itemIndex", + "ctype": "uint16_t", + "desc": "OSD item index (0 to `OSD_ITEM_COUNT - 1`)" + } + ] + }, + "reply": { + "payload": [ + { + "name": "itemPosition", + "ctype": "uint16_t", + "desc": "Packed X/Y position (`osdLayoutsConfig()->item_pos[layoutIndex][itemIndex]`)", + "units": "packed coords" + } + ] + } + } + }, + "notes": "Requires `USE_OSD`. Returns `MSP_RESULT_ACK` on success, `MSP_RESULT_ERROR` if indexes are out of range.", + "description": "Retrieves OSD layout metadata or item positions for specific layouts/items." + }, + "MSP2_INAV_OSD_SET_LAYOUT_ITEM": { + "code": 8211, + "mspv": 2, + "request": { + "payload": [ + { + "name": "layoutIndex", + "ctype": "uint8_t", + "desc": "Index of the OSD layout (0 to `OSD_LAYOUT_COUNT - 1`)", + "units": "Index" + }, + { + "name": "itemIndex", + "ctype": "uint8_t", + "desc": "Index of the OSD item", + "units": "Index" + }, + { + "name": "itemPosition", + "ctype": "uint16_t", + "desc": "Packed X/Y position using `OSD_POS(x, y)` with `OSD_VISIBLE_FLAG` bit", + "units": "Coordinates" + } + ] + }, + "reply": null, + "notes": "Requires `USE_OSD`. Expects 4 bytes. Returns error if indexes are invalid. If the modified layout is not the currently active one, it temporarily overrides the active layout for 10 seconds to show the change. Otherwise, triggers a full OSD redraw.", + "description": "Sets the position of a single OSD item within a specific layout." + }, + "MSP2_INAV_OSD_ALARMS": { + "code": 8212, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "rssiAlarm", + "ctype": "uint8_t", + "desc": "RSSI alarm threshold (`osdConfig()->rssi_alarm`)", + "units": "%" + }, + { + "name": "timerAlarm", + "ctype": "uint16_t", + "desc": "Timer alarm threshold (`osdConfig()->time_alarm`)", + "units": "seconds" + }, + { + "name": "altAlarm", + "ctype": "uint16_t", + "desc": "Altitude alarm threshold (`osdConfig()->alt_alarm`)", + "units": "meters" + }, + { + "name": "distAlarm", + "ctype": "uint16_t", + "desc": "Distance alarm threshold (`osdConfig()->dist_alarm`)", + "units": "meters" + }, + { + "name": "negAltAlarm", + "ctype": "uint16_t", + "desc": "Negative altitude alarm threshold (`osdConfig()->neg_alt_alarm`)", + "units": "meters" + }, + { + "name": "gForceAlarm", + "ctype": "uint16_t", + "desc": "G-force alarm threshold (`osdConfig()->gforce_alarm * 1000`)", + "units": "G * 1000" + }, + { + "name": "gForceAxisMinAlarm", + "ctype": "int16_t", + "desc": "Min G-force per-axis alarm (`osdConfig()->gforce_axis_alarm_min * 1000`)", + "units": "G * 1000" + }, + { + "name": "gForceAxisMaxAlarm", + "ctype": "int16_t", + "desc": "Max G-force per-axis alarm (`osdConfig()->gforce_axis_alarm_max * 1000`)", + "units": "G * 1000" + }, + { + "name": "currentAlarm", + "ctype": "uint8_t", + "desc": "Current draw alarm threshold (`osdConfig()->current_alarm`)", + "units": "A" + }, + { + "name": "imuTempMinAlarm", + "ctype": "int16_t", + "desc": "Min IMU temperature alarm (`osdConfig()->imu_temp_alarm_min`)", + "units": "degrees C" + }, + { + "name": "imuTempMaxAlarm", + "ctype": "int16_t", + "desc": "Max IMU temperature alarm (`osdConfig()->imu_temp_alarm_max`)", + "units": "degrees C" + }, + { + "name": "baroTempMinAlarm", + "ctype": "int16_t", + "desc": "Min Baro temperature alarm (`osdConfig()->baro_temp_alarm_min`). 0 if `USE_BARO` disabled", + "units": "degrees C" + }, + { + "name": "baroTempMaxAlarm", + "ctype": "int16_t", + "desc": "Max Baro temperature alarm (`osdConfig()->baro_temp_alarm_max`). 0 if `USE_BARO` disabled", + "units": "degrees C" + }, + { + "name": "adsbWarnDistance", + "ctype": "uint16_t", + "desc": "ADSB warning distance (`osdConfig()->adsb_distance_warning`). 0 if `USE_ADSB` disabled", + "units": "meters" + }, + { + "name": "adsbAlertDistance", + "ctype": "uint16_t", + "desc": "ADSB alert distance (`osdConfig()->adsb_distance_alert`). 0 if `USE_ADSB` disabled", + "units": "meters" + } + ] + }, + "notes": "Requires `USE_OSD`.", + "description": "Retrieves OSD alarm threshold settings." + }, + "MSP2_INAV_OSD_SET_ALARMS": { + "code": 8213, + "mspv": 2, + "request": { + "payload": [ + { + "name": "rssiAlarm", + "ctype": "uint8_t", + "desc": "Sets `osdConfigMutable()->rssi_alarm", + "units": "%" + }, + { + "name": "timerAlarm", + "ctype": "uint16_t", + "desc": "Sets `osdConfigMutable()->time_alarm", + "units": "seconds" + }, + { + "name": "altAlarm", + "ctype": "uint16_t", + "desc": "Sets `osdConfigMutable()->alt_alarm", + "units": "meters" + }, + { + "name": "distAlarm", + "ctype": "uint16_t", + "desc": "Sets `osdConfigMutable()->dist_alarm", + "units": "meters" + }, + { + "name": "negAltAlarm", + "ctype": "uint16_t", + "desc": "Sets `osdConfigMutable()->neg_alt_alarm`", + "units": "meters" + }, + { + "name": "gForceAlarm", + "ctype": "uint16_t", + "desc": "Sets `osdConfigMutable()->gforce_alarm = value / 1000.0f`", + "units": "G * 1000" + }, + { + "name": "gForceAxisMinAlarm", + "ctype": "int16_t", + "desc": "Sets `osdConfigMutable()->gforce_axis_alarm_min = value / 1000.0f`", + "units": "G * 1000" + }, + { + "name": "gForceAxisMaxAlarm", + "ctype": "int16_t", + "desc": "Sets `osdConfigMutable()->gforce_axis_alarm_max = value / 1000.0f`", + "units": "G * 1000" + }, + { + "name": "currentAlarm", + "ctype": "uint8_t", + "desc": "Sets `osdConfigMutable()->current_alarm`", + "units": "A" + }, + { + "name": "imuTempMinAlarm", + "ctype": "int16_t", + "desc": "Sets `osdConfigMutable()->imu_temp_alarm_min`", + "units": "degrees C" + }, + { + "name": "imuTempMaxAlarm", + "ctype": "int16_t", + "desc": "Sets `osdConfigMutable()->imu_temp_alarm_max`", + "units": "degrees C" + }, + { + "name": "baroTempMinAlarm", + "ctype": "int16_t", + "desc": "Sets `osdConfigMutable()->baro_temp_alarm_min` (if `USE_BARO`)", + "units": "degrees C" + }, + { + "name": "baroTempMaxAlarm", + "ctype": "int16_t", + "desc": "Sets `osdConfigMutable()->baro_temp_alarm_max` (if `USE_BARO`)", + "units": "degrees C" + } + ] + }, + "reply": null, + "notes": "Requires `USE_OSD`. Expects 24 bytes. ADSB alarms are not settable via this message.", + "description": "Sets OSD alarm threshold settings." + }, + "MSP2_INAV_OSD_PREFERENCES": { + "code": 8214, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "videoSystem", + "ctype": "uint8_t", + "desc": "Enum `videoSystem_e`: Video system (Auto/PAL/NTSC) (`osdConfig()->video_system`)", + "units": "", + "enum": "videoSystem_e" + }, + { + "name": "mainVoltageDecimals", + "ctype": "uint8_t", + "desc": "Count: Decimal places for main voltage display (`osdConfig()->main_voltage_decimals`)", + "units": "" + }, + { + "name": "ahiReverseRoll", + "ctype": "uint8_t", + "desc": "Boolean: Reverse roll direction on Artificial Horizon (`osdConfig()->ahi_reverse_roll`)", + "units": "" + }, + { + "name": "crosshairsStyle", + "ctype": "uint8_t", + "desc": "Enum `osd_crosshairs_style_e`: Style of the center crosshairs (`osdConfig()->crosshairs_style`)", + "units": "", + "enum": "osd_crosshairs_style_e" + }, + { + "name": "leftSidebarScroll", + "ctype": "uint8_t", + "desc": "Enum `osd_sidebar_scroll_e`: Left sidebar scroll behavior (`osdConfig()->left_sidebar_scroll`)", + "units": "", + "enum": "osd_sidebar_scroll_e" + }, + { + "name": "rightSidebarScroll", + "ctype": "uint8_t", + "desc": "Enum `osd_sidebar_scroll_e`: Right sidebar scroll behavior (`osdConfig()->right_sidebar_scroll`)", + "units": "", + "enum": "osd_sidebar_scroll_e" + }, + { + "name": "sidebarScrollArrows", + "ctype": "uint8_t", + "desc": "Boolean: Show arrows for scrollable sidebars (`osdConfig()->sidebar_scroll_arrows`)", + "units": "" + }, + { + "name": "units", + "ctype": "uint8_t", + "desc": "Enum: `osd_unit_e` Measurement units (Metric/Imperial) (`osdConfig()->units`)", + "units": "", + "enum": "osd_unit_e" + }, + { + "name": "statsEnergyUnit", + "ctype": "uint8_t", + "desc": "Enum `osd_stats_energy_unit_e`: Unit for energy display in post-flight stats (`osdConfig()->stats_energy_unit`)", + "units": "", + "enum": "osd_stats_energy_unit_e" + } + ] + }, + "notes": "Requires `USE_OSD`.", + "description": "Retrieves OSD display preferences (video system, units, styles, etc.)." + }, + "MSP2_INAV_OSD_SET_PREFERENCES": { + "code": 8215, + "mspv": 2, + "request": { + "payload": [ + { + "name": "videoSystem", + "ctype": "uint8_t", + "desc": "Sets `osdConfigMutable()->video_system`", + "units": "", + "enum": "videoSystem_e" + }, + { + "name": "mainVoltageDecimals", + "ctype": "uint8_t", + "desc": "Sets `osdConfigMutable()->main_voltage_decimals`", + "units": "" + }, + { + "name": "ahiReverseRoll", + "ctype": "uint8_t", + "desc": "Sets `osdConfigMutable()->ahi_reverse_roll`", + "units": "" + }, + { + "name": "crosshairsStyle", + "ctype": "uint8_t", + "desc": "Sets `osdConfigMutable()->crosshairs_style`", + "units": "", + "enum": "osd_crosshairs_style_e" + }, + { + "name": "leftSidebarScroll", + "ctype": "uint8_t", + "desc": "Sets `osdConfigMutable()->left_sidebar_scroll`", + "units": "", + "enum": "osd_sidebar_scroll_e" + }, + { + "name": "rightSidebarScroll", + "ctype": "uint8_t", + "desc": "Sets `osdConfigMutable()->right_sidebar_scroll`", + "units": "", + "enum": "osd_sidebar_scroll_e" + }, + { + "name": "sidebarScrollArrows", + "ctype": "uint8_t", + "desc": "Sets `osdConfigMutable()->sidebar_scroll_arrows`", + "units": "" + }, + { + "name": "units", + "ctype": "uint8_t", + "desc": "Sets `osdConfigMutable()->units` (enum `osd_unit_e`)", + "units": "", + "enum": "osd_unit_e" + }, + { + "name": "statsEnergyUnit", + "ctype": "uint8_t", + "desc": "Sets `osdConfigMutable()->stats_energy_unit`", + "units": "", + "enum": "osd_stats_energy_unit_e" + } + ] + }, + "reply": null, + "notes": "Requires `USE_OSD`. Expects 9 bytes. Triggers a full OSD redraw.", + "description": "Sets OSD display preferences." + }, + "MSP2_INAV_SELECT_BATTERY_PROFILE": { + "code": 8216, + "mspv": 2, + "request": { + "payload": [ + { + "name": "batteryProfileIndex", + "ctype": "uint8_t", + "desc": "Index of the battery profile to activate (0-based)", + "units": "" + } + ] + }, + "reply": null, + "notes": "Expects 1 byte. Will fail if armed. Calls `setConfigBatteryProfileAndWriteEEPROM()`.", + "description": "Selects the active battery profile and saves configuration." + }, + "MSP2_INAV_DEBUG": { + "code": 8217, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "debugValues", + "desc": "Values from the `debug` array (signed, typically 8 entries)", + "ctype": "int32_t", + "array": true, + "array_size": 8, + "array_size_define": "DEBUG32_VALUE_COUNT", + "units": "" + } + ] + }, + "variable_len": true, + "notes": "`DEBUG32_VALUE_COUNT` is usually 8.", + "description": "Retrieves values from the firmware's 32-bit `debug[]` array. Supersedes `MSP_DEBUG`." + }, + "MSP2_BLACKBOX_CONFIG": { + "code": 8218, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "blackboxSupported", + "ctype": "uint8_t", + "desc": "Boolean: 1 if Blackbox is supported (`USE_BLACKBOX`), 0 otherwise", + "units": "" + }, + { + "name": "blackboxDevice", + "ctype": "uint8_t", + "desc": "Enum `BlackboxDevice`: Target device for logging (`blackboxConfig()->device`). 0 if not supported", + "units": "", + "enum": "BlackboxDevice" + }, + { + "name": "blackboxRateNum", + "ctype": "uint16_t", + "desc": "Numerator for logging rate divider (`blackboxConfig()->rate_num`). 0 if not supported", + "units": "" + }, + { + "name": "blackboxRateDenom", + "ctype": "uint16_t", + "desc": "Denominator for logging rate divider (`blackboxConfig()->rate_denom`). 0 if not supported", + "units": "" + }, + { + "name": "blackboxIncludeFlags", + "ctype": "uint32_t", + "desc": "Bitmask: Flags for fields included/excluded from logging (`blackboxConfig()->includeFlags`)", + "units": "", + "bitmask": true + } + ] + }, + "notes": "If `USE_BLACKBOX` is disabled, only the first four fields are returned (all zero).", + "description": "Retrieves the Blackbox configuration. Supersedes `MSP_BLACKBOX_CONFIG`." + }, + "MSP2_SET_BLACKBOX_CONFIG": { + "code": 8219, + "mspv": 2, + "request": { + "payload": [ + { + "name": "blackboxDevice", + "ctype": "uint8_t", + "desc": "Sets `blackboxConfigMutable()->device`", + "units": "", + "enum": "BlackboxDevice" + }, + { + "name": "blackboxRateNum", + "ctype": "uint16_t", + "desc": "Sets `blackboxConfigMutable()->rate_num`", + "units": "" + }, + { + "name": "blackboxRateDenom", + "ctype": "uint16_t", + "desc": "Sets `blackboxConfigMutable()->rate_denom`", + "units": "" + }, + { + "name": "blackboxIncludeFlags", + "ctype": "uint32_t", + "desc": "Sets `blackboxConfigMutable()->includeFlags`", + "units": "" + } + ] + }, + "reply": null, + "notes": "Requires `USE_BLACKBOX`. Expects 9 bytes. Returns error if Blackbox is currently logging (`!blackboxMayEditConfig()`).", + "description": "Sets the Blackbox configuration. Supersedes `MSP_SET_BLACKBOX_CONFIG`." + }, + "MSP2_INAV_TEMP_SENSOR_CONFIG": { + "code": 8220, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "type", + "ctype": "uint8_t", + "desc": "Enum (`tempSensorType_e`): Type of the temperature sensor", + "units": "", + "enum": "tempSensorType_e" + }, + { + "name": "address", + "ctype": "uint64_t", + "desc": "Sensor address/ID (e.g., for 1-Wire sensors)", + "units": "" + }, + { + "name": "alarmMin", + "ctype": "int16_t", + "desc": "Min temperature alarm threshold (`sensorConfig->alarm_min`)", + "units": "0.1°C" + }, + { + "name": "alarmMax", + "ctype": "int16_t", + "desc": "Max temperature alarm threshold (`sensorConfig->alarm_max`)", + "units": "0.1°C" + }, + { + "name": "osdSymbol", + "ctype": "uint8_t", + "desc": "Index: OSD symbol to use for this sensor (0 to `TEMP_SENSOR_SYM_COUNT`)", + "units": "" + }, + { + "name": "label", + "desc": "User-defined label for the sensor", + "ctype": "char", + "array": true, + "array_size": 4, + "array_size_define": "TEMPERATURE_LABEL_LEN", + "units": "" + } + ], + "repeating": "MAX_TEMP_SENSORS" + }, + "variable_len": "MAX_TEMP_SENSORS", + "notes": "Requires `USE_TEMPERATURE_SENSOR`.", + "description": "Retrieves the configuration for all onboard temperature sensors." + }, + "MSP2_INAV_SET_TEMP_SENSOR_CONFIG": { + "code": 8221, + "mspv": 2, + "request": { + "payload": [ + { + "name": "type", + "ctype": "uint8_t", + "desc": "Sets sensor type (`tempSensorType_e`)", + "units": "", + "enum": "tempSensorType_e" + }, + { + "name": "address", + "ctype": "uint64_t", + "desc": "Sets sensor address/ID", + "units": "" + }, + { + "name": "alarmMin", + "ctype": "int16_t", + "desc": "Sets min alarm threshold (`tempSensorConfigMutable(index)->alarm_min`)", + "units": "0.1°C" + }, + { + "name": "alarmMax", + "ctype": "int16_t", + "desc": "Sets max alarm threshold (`tempSensorConfigMutable(index)->alarm_max`)", + "units": "0.1°C" + }, + { + "name": "osdSymbol", + "ctype": "uint8_t", + "desc": "Sets OSD symbol index (validated)", + "units": "" + }, + { + "name": "label", + "desc": "Sets sensor label (converted to uppercase)", + "ctype": "char", + "array": true, + "array_size": 4, + "array_size_define": "TEMPERATURE_LABEL_LEN", + "units": "" + } + ], + "repeating": "MAX_TEMP_SENSORS" + }, + "reply": null, + "notes": "Requires `USE_TEMPERATURE_SENSOR`. Payload must include `MAX_TEMP_SENSORS` consecutive `tempSensorConfig_t` structures (labels are uppercased).", + "description": "Sets the configuration for all onboard temperature sensors." + }, + "MSP2_INAV_TEMPERATURES": { + "code": 8222, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "temperature", + "ctype": "int16_t", + "desc": "Current temperature reading. -1000 if sensor is invalid or reading failed", + "units": "0.1°C" + } + ], + "repeating": "MAX_TEMP_SENSORS" + }, + "variable_len": "MAX_TEMP_SENSORS", + "notes": "Requires `USE_TEMPERATURE_SENSOR`.", + "description": "Retrieves the current readings from all configured temperature sensors." + }, + "MSP_SIMULATOR": { + "code": 8223, + "mspv": 2, + "request": { + "payload": [ + { + "name": "simulatorVersion", + "ctype": "uint8_t", + "desc": "Version of the simulator protocol (`SIMULATOR_MSP_VERSION`)", + "units": "" + }, + { + "name": "simulatorFlags_t", + "ctype": "uint8_t", + "desc": "Bitmask: Options for HITL (`HITL_*` flags)", + "units": "Bitmask", + "bitmask": true + }, + { + "name": "gpsFixType", + "ctype": "uint8_t", + "desc": "Enum `gpsFixType_e` Type of GPS fix (If `HITL_HAS_NEW_GPS_DATA`)", + "units": "Enum", + "enum": "gpsFixType_e" + }, + { + "name": "gpsNumSat", + "ctype": "uint8_t", + "desc": "(If `HITL_HAS_NEW_GPS_DATA`) Simulated satellite count", + "units": "" + }, + { + "name": "gpsLat", + "ctype": "int32_t", + "desc": "(If `HITL_HAS_NEW_GPS_DATA`) Simulated latitude (1e7 deg)", + "units": "" + }, + { + "name": "gpsLon", + "ctype": "int32_t", + "desc": "(If `HITL_HAS_NEW_GPS_DATA`) Simulated longitude (1e7 deg)", + "units": "" + }, + { + "name": "gpsAlt", + "ctype": "int32_t", + "desc": "(If `HITL_HAS_NEW_GPS_DATA`) Simulated altitude (cm)", + "units": "" + }, + { + "name": "gpsSpeed", + "ctype": "uint16_t", + "desc": "(If `HITL_HAS_NEW_GPS_DATA`) Simulated ground speed (cm/s)", + "units": "" + }, + { + "name": "gpsCourse", + "ctype": "uint16_t", + "desc": "(If `HITL_HAS_NEW_GPS_DATA`) Simulated ground course (deci-deg)", + "units": "" + }, + { + "name": "gpsVelN", + "ctype": "int16_t", + "desc": "(If `HITL_HAS_NEW_GPS_DATA`) Simulated North velocity (cm/s)", + "units": "" + }, + { + "name": "gpsVelE", + "ctype": "int16_t", + "desc": "(If `HITL_HAS_NEW_GPS_DATA`) Simulated East velocity (cm/s)", + "units": "" + }, + { + "name": "gpsVelD", + "ctype": "int16_t", + "desc": "(If `HITL_HAS_NEW_GPS_DATA`) Simulated Down velocity (cm/s)", + "units": "" + }, + { + "name": "imuRoll", + "ctype": "int16_t", + "desc": "(If NOT `HITL_USE_IMU`) Simulated Roll (deci-deg)", + "units": "" + }, + { + "name": "imuPitch", + "ctype": "int16_t", + "desc": "(If NOT `HITL_USE_IMU`) Simulated Pitch (deci-deg)", + "units": "" + }, + { + "name": "imuYaw", + "ctype": "int16_t", + "desc": "(If NOT `HITL_USE_IMU`) Simulated Yaw (deci-deg)", + "units": "" + }, + { + "name": "accX", + "ctype": "int16_t", + "desc": "mG (G * 1000)", + "units": "" + }, + { + "name": "accY", + "ctype": "int16_t", + "desc": "mG (G * 1000)", + "units": "" + }, + { + "name": "accZ", + "ctype": "int16_t", + "desc": "mG (G * 1000)", + "units": "" + }, + { + "name": "gyroX", + "ctype": "int16_t", + "desc": "dps * 16", + "units": "" + }, + { + "name": "gyroY", + "ctype": "int16_t", + "desc": "dps * 16", + "units": "" + }, + { + "name": "gyroZ", + "ctype": "int16_t", + "desc": "dps * 16", + "units": "" + }, + { + "name": "baroPressure", + "ctype": "uint32_t", + "desc": "Pa", + "units": "" + }, + { + "name": "magX", + "ctype": "int16_t", + "desc": "Scaled", + "units": "" + }, + { + "name": "magY", + "ctype": "int16_t", + "desc": "Scaled", + "units": "" + }, + { + "name": "magZ", + "ctype": "int16_t", + "desc": "Scaled", + "units": "" + }, + { + "name": "vbat", + "ctype": "uint8_t", + "desc": "(If `HITL_EXT_BATTERY_VOLTAGE`) Simulated battery voltage (0.1V units)", + "units": "" + }, + { + "name": "airspeed", + "ctype": "uint16_t", + "desc": "(If `HITL_AIRSPEED`) Simulated airspeed (cm/s)", + "units": "" + }, + { + "name": "extFlags", + "ctype": "uint8_t", + "desc": "(If `HITL_EXTENDED_FLAGS`) Additional flags (upper 8 bits)", + "units": "" + } + ] + }, + "reply": { + "payload": [ + { + "name": "stabilizedRoll", + "ctype": "uint16_t", + "desc": "Stabilized Roll command output (-500 to 500)", + "units": "" + }, + { + "name": "stabilizedPitch", + "ctype": "uint16_t", + "desc": "Stabilized Pitch command output (-500 to 500)", + "units": "" + }, + { + "name": "stabilizedYaw", + "ctype": "uint16_t", + "desc": "Stabilized Yaw command output (-500 to 500)", + "units": "" + }, + { + "name": "stabilizedThrottle", + "ctype": "uint16_t", + "desc": "Stabilized Throttle command output (-500 to 500 if armed, else -500)", + "units": "" + }, + { + "name": "debugFlags", + "ctype": "uint8_t", + "desc": "Packed flags: Debug index (0-7), Platform type, Armed state, OSD feature status", + "units": "" + }, + { + "name": "debugValue", + "ctype": "uint32_t", + "desc": "Current debug value (`debug[simulatorData.debugIndex]`)", + "units": "" + }, + { + "name": "attitudeRoll", + "ctype": "int16_t", + "desc": "Current estimated Roll (deci-deg)", + "units": "" + }, + { + "name": "attitudePitch", + "ctype": "int16_t", + "desc": "Current estimated Pitch (deci-deg)", + "units": "" + }, + { + "name": "attitudeYaw", + "ctype": "int16_t", + "desc": "Current estimated Yaw (deci-deg)", + "units": "" + }, + { + "name": "osdHeader", + "ctype": "uint8_t", + "desc": "OSD RLE Header (255)", + "units": "", + "optional": true + }, + { + "name": "osdRows", + "ctype": "uint8_t", + "desc": "(If OSD supported) Number of OSD rows", + "units": "", + "optional": true + }, + { + "name": "osdCols", + "ctype": "uint8_t", + "desc": "(If OSD supported) Number of OSD columns", + "units": "", + "optional": true + }, + { + "name": "osdStartY", + "ctype": "uint8_t", + "desc": "(If OSD supported) Starting row for RLE data", + "units": "", + "optional": true + }, + { + "name": "osdStartX", + "ctype": "uint8_t", + "desc": "(If OSD supported) Starting column for RLE data", + "units": "", + "optional": true + }, + { + "name": "osdRleData", + "desc": "(If OSD supported) Run-length encoded OSD character data. Terminated by `[0, 0]`", + "ctype": "uint8_t", + "array": true, + "array_size": 0, + "units": "", + "optional": true + } + ] + }, + "variable_len": true, + "notes": "Requires `USE_SIMULATOR`. Complex message handling state changes for enabling/disabling HITL. Sensor data is injected directly. OSD data is sent using a custom RLE scheme. See `simulatorData` struct and associated code for details.", + "description": "Handles Hardware-in-the-Loop (HITL) simulation data exchange. Receives simulated sensor data and options, sends back control outputs and debug info." + }, + "MSP2_INAV_SERVO_MIXER": { + "code": 8224, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "targetChannel", + "ctype": "uint8_t", + "desc": "Servo output channel index (0-based)", + "units": "" + }, + { + "name": "inputSource", + "ctype": "uint8_t", + "desc": "Enum `inputSource_e` Input source", + "units": "", + "enum": "inputSource_e" + }, + { + "name": "rate", + "ctype": "int16_t", + "desc": "Mixing rate/weight", + "units": "" + }, + { + "name": "speed", + "ctype": "uint8_t", + "desc": "Speed/Slew rate limit (0-100)", + "units": "" + }, + { + "name": "conditionId", + "ctype": "int8_t", + "desc": "Logic Condition ID (0 to `MAX_LOGIC_CONDITIONS - 1`, or 255/-1 if none/disabled)", + "units": "" + }, + { + "name": "p2TargetChannel", + "ctype": "uint8_t", + "desc": "(Optional) Profile 2 Target channel", + "units": "", + "optional": true + }, + { + "name": "p2InputSource", + "ctype": "uint8_t", + "desc": "(Optional) Profile 2 Enum `inputSource_e` Input source", + "units": "", + "optional": true, + "enum": "inputSource_e" + }, + { + "name": "p2Rate", + "ctype": "int16_t", + "desc": "(Optional) Profile 2 Rate", + "units": "", + "optional": true + }, + { + "name": "p2Speed", + "ctype": "uint8_t", + "desc": "(Optional) Profile 2 Speed", + "units": "", + "optional": true + }, + { + "name": "p2ConditionId", + "ctype": "int8_t", + "desc": "(Optional) Profile 2 Logic Condition ID", + "units": "", + "optional": true + } + ], + "repeating": "MAX_SERVO_RULES" + }, + "variable_len": "MAX_SERVO_RULES", + "notes": "`conditionId` requires `USE_PROGRAMMING_FRAMEWORK`.", + "description": "Retrieves the custom servo mixer rules, including programming framework condition IDs, for primary and secondary mixer profiles. Supersedes `MSP_SERVO_MIX_RULES`." + }, + "MSP2_INAV_SET_SERVO_MIXER": { + "code": 8225, + "mspv": 2, + "request": { + "payload": [ + { + "name": "ruleIndex", + "ctype": "uint8_t", + "desc": "Index of the rule to set (0 to `MAX_SERVO_RULES - 1`)", + "units": "" + }, + { + "name": "targetChannel", + "ctype": "uint8_t", + "desc": "Servo output channel index", + "units": "" + }, + { + "name": "inputSource", + "ctype": "uint8_t", + "desc": "Enum `inputSource_e` Input source", + "units": "", + "enum": "inputSource_e" + }, + { + "name": "rate", + "ctype": "int16_t", + "desc": "Mixing rate/weight", + "units": "" + }, + { + "name": "speed", + "ctype": "uint8_t", + "desc": "Speed/Slew rate limit (0-100)", + "units": "" + }, + { + "name": "conditionId", + "ctype": "int8_t", + "desc": "Logic Condition ID (255/-1 if none). Ignored if `USE_PROGRAMMING_FRAMEWORK` is disabled", + "units": "" + } + ] + }, + "reply": null, + "notes": "Expects 7 bytes. Returns error if index invalid. Calls `loadCustomServoMixer()`.", + "description": "Sets a single custom servo mixer rule, including programming framework condition ID. Supersedes `MSP_SET_SERVO_MIX_RULE`." + }, + "MSP2_INAV_LOGIC_CONDITIONS": { + "code": 8226, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "enabled", + "ctype": "uint8_t", + "desc": "Boolean: 1 if the condition is enabled", + "units": "" + }, + { + "name": "activatorId", + "ctype": "int8_t", + "desc": "Activator condition ID (-1/255 if none)", + "units": "" + }, + { + "name": "operation", + "ctype": "uint8_t", + "desc": "Enum `logicOperation_e` Logical operation (AND, OR, XOR, etc.)", + "units": "", + "enum": "logicOperation_e" + }, + { + "name": "operandAType", + "ctype": "uint8_t", + "desc": "Enum `logicOperandType_e` Type of the first operand (Flight Mode, GVAR, etc.)", + "units": "", + "enum": "logicOperandType_e" + }, + { + "name": "operandAValue", + "ctype": "int32_t", + "desc": "Value/ID of the first operand", + "units": "" + }, + { + "name": "operandBType", + "ctype": "uint8_t", + "desc": "Enum `logicOperandType_e`: Type of the second operand", + "units": "", + "enum": "logicOperandType_e" + }, + { + "name": "operandBValue", + "ctype": "int32_t", + "desc": "Value/ID of the second operand", + "units": "" + }, + { + "name": "flags", + "ctype": "uint8_t", + "desc": "Bitmask: Condition flags (`logicConditionFlags_e`)", + "units": "Bitmask", + "bitmask": true + } + ], + "repeating": "MAX_LOGIC_CONDITIONS" + }, + "variable_len": "MAX_LOGIC_CONDITIONS", + "notes": "Requires `USE_PROGRAMMING_FRAMEWORK`. See `logicCondition_t` structure.", + "description": "Retrieves the configuration of all defined Logic Conditions." + }, + "MSP2_INAV_SET_LOGIC_CONDITIONS": { + "code": 8227, + "mspv": 2, + "request": { + "payload": [ + { + "name": "conditionIndex", + "ctype": "uint8_t", + "desc": "Index of the condition to set (0 to `MAX_LOGIC_CONDITIONS - 1`)", + "units": "" + }, + { + "name": "enabled", + "ctype": "uint8_t", + "desc": "Boolean: 1 to enable the condition", + "units": "" + }, + { + "name": "activatorId", + "ctype": "int8_t", + "desc": "Activator condition ID (-1/255 if none)", + "units": "" + }, + { + "name": "operation", + "ctype": "uint8_t", + "desc": "Enum `logicOperation_e` Logical operation", + "units": "", + "enum": "logicOperation_e" + }, + { + "name": "operandAType", + "ctype": "uint8_t", + "desc": "Enum `logicOperandType_e` Type of operand A", + "units": "", + "enum": "logicOperandType_e" + }, + { + "name": "operandAValue", + "ctype": "int32_t", + "desc": "Value/ID of operand A", + "units": "" + }, + { + "name": "operandBType", + "ctype": "uint8_t", + "desc": "Enum `logicOperandType_e` Type of operand B", + "units": "", + "enum": "logicOperandType_e" + }, + { + "name": "operandBValue", + "ctype": "int32_t", + "desc": "Value/ID of operand B", + "units": "" + }, + { + "name": "flags", + "ctype": "uint8_t", + "desc": "Bitmask: Condition flags (`logicConditionFlags_e`)", + "units": "Bitmask", + "bitmask": true + } + ] + }, + "reply": null, + "notes": "Requires `USE_PROGRAMMING_FRAMEWORK`. Expects 15 bytes. Returns error if index is invalid.", + "description": "Sets the configuration for a single Logic Condition by its index." + }, + "MSP2_INAV_GLOBAL_FUNCTIONS": { + "code": 8228, + "mspv": 2, + "not_implemented": true + }, + "MSP2_INAV_SET_GLOBAL_FUNCTIONS": { + "code": 8229, + "mspv": 2, + "not_implemented": true + }, + "MSP2_INAV_LOGIC_CONDITIONS_STATUS": { + "code": 8230, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "conditionValues", + "ctype": "int32_t", + "desc": "Array of current values for each logic condition (`logicConditionGetValue(i)`). 1 for true, 0 for false, or numerical value depending on operation", + "array": true, + "array_size": 64, + "array_size_define": "MAX_LOGIC_CONDITIONS", + "units": "" + } + ] + }, + "variable_len": true, + "notes": "Requires `USE_PROGRAMMING_FRAMEWORK`.", + "description": "Retrieves the current evaluated status (true/false or numerical value) of all logic conditions." + }, + "MSP2_INAV_GVAR_STATUS": { + "code": 8231, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "gvarValues", + "desc": "Array of current values for each global variable (`gvGet(i)`)", + "ctype": "int32_t", + "array": true, + "array_size": 8, + "array_size_define": "MAX_GLOBAL_VARIABLES", + "units": "" + } + ] + }, + "variable_len": true, + "notes": "Requires `USE_PROGRAMMING_FRAMEWORK`.", + "description": "Retrieves the current values of all Global Variables (GVARS)." + }, + "MSP2_INAV_PROGRAMMING_PID": { + "code": 8232, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "enabled", + "ctype": "uint8_t", + "desc": "Boolean: 1 if the PID is enabled", + "units": "" + }, + { + "name": "setpointType", + "ctype": "uint8_t", + "desc": "Enum (`logicOperandType_e`) Type of the setpoint source", + "units": "", + "enum": "logicOperandType_e" + }, + { + "name": "setpointValue", + "ctype": "int32_t", + "desc": "Value/ID of the setpoint source", + "units": "" + }, + { + "name": "measurementType", + "ctype": "uint8_t", + "desc": "Enum (`logicOperandType_e`) Type of the measurement source", + "units": "", + "enum": "logicOperandType_e" + }, + { + "name": "measurementValue", + "ctype": "int32_t", + "desc": "Value/ID of the measurement source", + "units": "" + }, + { + "name": "gainP", + "ctype": "uint16_t", + "desc": "Proportional gain", + "units": "" + }, + { + "name": "gainI", + "ctype": "uint16_t", + "desc": "Integral gain", + "units": "" + }, + { + "name": "gainD", + "ctype": "uint16_t", + "desc": "Derivative gain", + "units": "" + }, + { + "name": "gainFF", + "ctype": "uint16_t", + "desc": "Feed-forward gain", + "units": "" + } + ], + "repeating": "MAX_PROGRAMMING_PID_COUNT" + }, + "variable_len": "MAX_PROGRAMMING_PID_COUNT", + "notes": "Requires `USE_PROGRAMMING_FRAMEWORK`. See `programmingPid_t` structure.", + "description": "Retrieves the configuration of all Programming PIDs." + }, + "MSP2_INAV_SET_PROGRAMMING_PID": { + "code": 8233, + "mspv": 2, + "request": { + "payload": [ + { + "name": "pidIndex", + "ctype": "uint8_t", + "desc": "Index of the Programming PID to set (0 to `MAX_PROGRAMMING_PID_COUNT - 1`)", + "units": "" + }, + { + "name": "enabled", + "ctype": "uint8_t", + "desc": "Boolean: 1 to enable the PID", + "units": "" + }, + { + "name": "setpointType", + "ctype": "uint8_t", + "desc": "Enum (`logicOperandType_e`) Type of the setpoint source", + "units": "", + "enum": "logicOperandType_e" + }, + { + "name": "setpointValue", + "ctype": "int32_t", + "desc": "Value/ID of the setpoint source", + "units": "" + }, + { + "name": "measurementType", + "ctype": "uint8_t", + "desc": "Enum (`logicOperandType_e`) Type of the measurement source", + "units": "", + "enum": "logicOperandType_e" + }, + { + "name": "measurementValue", + "ctype": "int32_t", + "desc": "Value/ID of the measurement source", + "units": "" + }, + { + "name": "gainP", + "ctype": "uint16_t", + "desc": "Proportional gain", + "units": "" + }, + { + "name": "gainI", + "ctype": "uint16_t", + "desc": "Integral gain", + "units": "" + }, + { + "name": "gainD", + "ctype": "uint16_t", + "desc": "Derivative gain", + "units": "" + }, + { + "name": "gainFF", + "ctype": "uint16_t", + "desc": "Feed-forward gain", + "units": "" + } + ] + }, + "reply": null, + "notes": "Requires `USE_PROGRAMMING_FRAMEWORK`. Expects 20 bytes. Returns error if index is invalid.", + "description": "Sets the configuration for a single Programming PID by its index." + }, + "MSP2_INAV_PROGRAMMING_PID_STATUS": { + "code": 8234, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "pidOutputs", + "desc": "Array of current output values for each Programming PID (`programmingPidGetOutput(i)`, signed)", + "array": true, + "ctype": "int32_t", + "array_size": 4, + "array_size_define": "MAX_PROGRAMMING_PID_COUNT", + "units": "" + } + ] + }, + "variable_len": true, + "notes": "Requires `USE_PROGRAMMING_FRAMEWORK`.", + "description": "Retrieves the current output value of all Programming PIDs." + }, + "MSP2_PID": { + "code": 8240, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "P", + "ctype": "uint8_t", + "desc": "Proportional gain (`pidBank()->pid[i].P`), constrained 0-255", + "units": "" + }, + { + "name": "I", + "ctype": "uint8_t", + "desc": "Integral gain (`pidBank()->pid[i].I`), constrained 0-255", + "units": "" + }, + { + "name": "D", + "ctype": "uint8_t", + "desc": "Derivative gain (`pidBank()->pid[i].D`), constrained 0-255", + "units": "" + }, + { + "name": "FF", + "ctype": "uint8_t", + "desc": "Feed-forward gain (`pidBank()->pid[i].FF`), constrained 0-255", + "units": "" + } + ], + "repeating": "PID_ITEM_COUNT" + }, + "variable_len": "PID_ITEM_COUNT", + "notes": "`PID_ITEM_COUNT` defines the number of standard PID controllers (Roll, Pitch, Yaw, Alt, Vel, etc.). Updates from EZ-Tune if enabled.", + "description": "Retrieves the standard PID controller gains (P, I, D, FF) for the current PID profile." + }, + "MSP2_SET_PID": { + "code": 8241, + "mspv": 2, + "request": { + "payload": [ + { + "name": "P", + "ctype": "uint8_t", + "desc": "Sets Proportional gain (`pidBankMutable()->pid[i].P`)", + "units": "" + }, + { + "name": "I", + "ctype": "uint8_t", + "desc": "Sets Integral gain (`pidBankMutable()->pid[i].I`)", + "units": "" + }, + { + "name": "D", + "ctype": "uint8_t", + "desc": "Sets Derivative gain (`pidBankMutable()->pid[i].D`)", + "units": "" + }, + { + "name": "FF", + "ctype": "uint8_t", + "desc": "Sets Feed-forward gain (`pidBankMutable()->pid[i].FF`)", + "units": "" + } + ], + "repeating": "PID_ITEM_COUNT" + }, + "reply": null, + "variable_len": "PID_ITEM_COUNT", + "notes": "Expects `PID_ITEM_COUNT * 4` bytes. Calls `schedulePidGainsUpdate()` and `navigationUsePIDs()`.", + "description": "Sets the standard PID controller gains (P, I, D, FF) for the current PID profile." + }, + "MSP2_INAV_OPFLOW_CALIBRATION": { + "code": 8242, + "mspv": 2, + "request": null, + "reply": null, + "notes": "Requires `USE_OPFLOW`. Will fail if armed. Calls `opflowStartCalibration()`.", + "description": "Starts the optical flow sensor calibration procedure." + }, + "MSP2_INAV_FWUPDT_PREPARE": { + "code": 8243, + "mspv": 2, + "request": { + "payload": [ + { + "name": "firmwareSize", + "ctype": "uint32_t", + "desc": "Total size of the incoming firmware file in bytes", + "units": "" + } + ] + }, + "reply": null, + "notes": "Requires `MSP_FIRMWARE_UPDATE`. Expects 4 bytes. Returns error if preparation fails (e.g., no storage, invalid size). Calls `firmwareUpdatePrepare()`.", + "description": "Prepares the flight controller to receive a firmware update via MSP." + }, + "MSP2_INAV_FWUPDT_STORE": { + "code": 8244, + "mspv": 2, + "request": { + "payload": [ + { + "name": "firmwareChunk", + "desc": "Chunk of firmware data", + "ctype": "uint8_t", + "array": true, + "array_size": 0, + "units": "" + } + ] + }, + "reply": null, + "variable_len": true, + "notes": "Requires `MSP_FIRMWARE_UPDATE`. Returns error if storage fails (e.g., out of space, checksum error). Called repeatedly until the entire firmware is transferred. Calls `firmwareUpdateStore()`.", + "description": "Stores a chunk of firmware data received via MSP." + }, + "MSP2_INAV_FWUPDT_EXEC": { + "code": 8245, + "mspv": 2, + "request": { + "payload": [ + { + "name": "updateType", + "ctype": "uint8_t", + "desc": "Type of update (e.g., full flash, specific section - currently ignored/unused)", + "units": "" + } + ] + }, + "reply": null, + "notes": "Requires `MSP_FIRMWARE_UPDATE`. Expects 1 byte. Returns error if update cannot start (e.g., not fully received). Calls `firmwareUpdateExec()`. If successful, the device will reboot into the new firmware.", + "description": "Executes the firmware update process (flashes the stored firmware and reboots)." + }, + "MSP2_INAV_FWUPDT_ROLLBACK_PREPARE": { + "code": 8246, + "mspv": 2, + "request": null, + "reply": null, + "notes": "Requires `MSP_FIRMWARE_UPDATE`. Returns error if rollback preparation fails (e.g., no rollback image available). Calls `firmwareUpdateRollbackPrepare()`.", + "description": "Prepares the flight controller to perform a firmware rollback to the previously stored version." + }, + "MSP2_INAV_FWUPDT_ROLLBACK_EXEC": { + "code": 8247, + "mspv": 2, + "request": null, + "reply": null, + "notes": "Requires `MSP_FIRMWARE_UPDATE`. Returns error if rollback cannot start. Calls `firmwareUpdateRollbackExec()`. If successful, the device will reboot into the backup firmware.", + "description": "Executes the firmware rollback process (flashes the stored backup firmware and reboots)." + }, + "MSP2_INAV_SAFEHOME": { + "code": 8248, + "mspv": 2, + "request": { + "payload": [ + { + "name": "safehomeIndex", + "ctype": "uint8_t", + "desc": "Index of the safe home location (0 to `MAX_SAFE_HOMES - 1`)", + "units": "" + } + ] + }, + "reply": { + "payload": [ + { + "name": "safehomeIndex", + "ctype": "uint8_t", + "desc": "Index requested", + "units": "" + }, + { + "name": "enabled", + "ctype": "uint8_t", + "desc": "Boolean: 1 if this safe home is enabled", + "units": "" + }, + { + "name": "latitude", + "ctype": "int32_t", + "desc": "Latitude (1e7 deg)", + "units": "" + }, + { + "name": "longitude", + "ctype": "int32_t", + "desc": "Longitude (1e7 deg)", + "units": "" + } + ] + }, + "notes": "Requires `USE_SAFE_HOME`. Used by `mspFcSafeHomeOutCommand`. See `MSP2_INAV_SET_SAFEHOME` for setting.", + "description": "Get or Set configuration for a specific Safe Home location." + }, + "MSP2_INAV_SET_SAFEHOME": { + "code": 8249, + "mspv": 2, + "request": { + "payload": [ + { + "name": "safehomeIndex", + "ctype": "uint8_t", + "desc": "Index of the safe home location (0 to `MAX_SAFE_HOMES - 1`)", + "units": "" + }, + { + "name": "enabled", + "ctype": "uint8_t", + "desc": "Boolean: 1 to enable this safe home", + "units": "" + }, + { + "name": "latitude", + "ctype": "int32_t", + "desc": "Latitude (1e7 deg)", + "units": "" + }, + { + "name": "longitude", + "ctype": "int32_t", + "desc": "Longitude (1e7 deg)", + "units": "" + } + ] + }, + "reply": null, + "notes": "Requires `USE_SAFE_HOME`. Expects 10 bytes. Returns error if index invalid. Resets corresponding FW autoland approach if `USE_FW_AUTOLAND` is enabled.", + "description": "Sets the configuration for a specific Safe Home location." + }, + "MSP2_INAV_MISC2": { + "code": 8250, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "uptimeSeconds", + "ctype": "uint32_t", + "desc": "Time since boot (`micros() / 1000000`)", + "units": "Seconds" + }, + { + "name": "flightTimeSeconds", + "ctype": "uint32_t", + "desc": "Accumulated flight time (`getFlightTime()`)", + "units": "Seconds" + }, + { + "name": "throttlePercent", + "ctype": "uint8_t", + "desc": "Current throttle output percentage (`getThrottlePercent(true)`)", + "units": "%" + }, + { + "name": "autoThrottleFlag", + "ctype": "uint8_t", + "desc": "1 if navigation is controlling throttle, 0 otherwise (`navigationIsControllingThrottle()`)", + "units": "Boolean" + } + ] + }, + "notes": "", + "description": "Retrieves miscellaneous runtime information including timers and throttle status." + }, + "MSP2_INAV_LOGIC_CONDITIONS_SINGLE": { + "code": 8251, + "mspv": 2, + "request": { + "payload": [ + { + "name": "conditionIndex", + "ctype": "uint8_t", + "desc": "Index of the condition to retrieve (0 to `MAX_LOGIC_CONDITIONS - 1`)", + "units": "" + } + ] + }, + "reply": { + "payload": [ + { + "name": "enabled", + "ctype": "uint8_t", + "desc": "Boolean: 1 if enabled", + "units": "" + }, + { + "name": "activatorId", + "ctype": "int8_t", + "desc": "Activator ID (-1/255 if none)", + "units": "" + }, + { + "name": "operation", + "ctype": "uint8_t", + "desc": "Enum `logicOperation_e` Logical operation", + "units": "", + "enum": "logicOperation_e" + }, + { + "name": "operandAType", + "ctype": "uint8_t", + "desc": "Enum `logicOperandType_e` Type of operand A", + "units": "", + "enum": "logicOperandType_e" + }, + { + "name": "operandAValue", + "ctype": "int32_t", + "desc": "Value/ID of operand A", + "units": "" + }, + { + "name": "operandBType", + "ctype": "uint8_t", + "desc": "Enum `logicOperandType_e` Type of operand B", + "units": "", + "enum": "logicOperandType_e" + }, + { + "name": "operandBValue", + "ctype": "int32_t", + "desc": "Value/ID of operand B", + "units": "" + }, + { + "name": "flags", + "ctype": "uint8_t", + "desc": "Bitmask: Condition flags (`logicConditionFlags_e`)", + "units": "Bitmask", + "bitmask": true + } + ] + }, + "notes": "Requires `USE_PROGRAMMING_FRAMEWORK`. Used by `mspFcLogicConditionCommand`.", + "description": "Gets the configuration for a single Logic Condition by its index." + }, + "MSP2_INAV_ESC_RPM": { + "code": 8256, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "escRpm", + "ctype": "uint32_t", + "desc": "RPM reported by the ESC", + "units": "RPM" + } + ], + "repeating": "getMotorCount()" + }, + "variable_len": "getMotorCount()", + "notes": "Requires `USE_ESC_SENSOR`. Payload size depends on the number of detected motors with telemetry.", + "description": "Retrieves the RPM reported by each ESC via telemetry." + }, + "MSP2_INAV_ESC_TELEM": { + "code": 8257, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "motorCount", + "ctype": "uint8_t", + "desc": "Number of motors reporting telemetry (`getMotorCount()`)", + "units": "" + }, + { + "name": "escData", + "desc": "Array of `escSensorData_t` structures containing voltage, current, temp, RPM, errors etc. for each ESC", + "ctype": "escSensorData_t", + "array": true, + "array_size": 0, + "units": "" + } + ] + }, + "variable_len": true, + "notes": "Requires `USE_ESC_SENSOR`. See `escSensorData_t` in `sensors/esc_sensor.h` for the exact structure fields.", + "description": "Retrieves the full telemetry data structure reported by each ESC." + }, + "MSP2_INAV_LED_STRIP_CONFIG_EX": { + "code": 8264, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "ledConfig", + "ctype": "ledConfig_t", + "desc": "Raw `ledConfig_t` structure (6 bytes) holding position, function, overlay, color, direction, and params bitfields (`io/ledstrip.h`).", + "units": "" + } + ], + "repeating": "LED_MAX_STRIP_LENGTH" + }, + "variable_len": "LED_MAX_STRIP_LENGTH", + "notes": "Requires `USE_LED_STRIP`. See `ledConfig_t` in `io/ledstrip.h` for structure fields (position, function, overlay, color, direction, params).", + "description": "Retrieves the full configuration for each LED on the strip using the `ledConfig_t` structure. Supersedes `MSP_LED_STRIP_CONFIG`." + }, + "MSP2_INAV_SET_LED_STRIP_CONFIG_EX": { + "code": 8265, + "mspv": 2, + "request": { + "payload": [ + { + "name": "ledIndex", + "ctype": "uint8_t", + "desc": "Index of the LED to configure (0 to `LED_MAX_STRIP_LENGTH - 1`)", + "units": "" + }, + { + "name": "ledConfig", + "ctype": "ledConfig_t", + "desc": "Raw `ledConfig_t` structure (6 bytes) mirroring the firmware layout.", + "units": "" + } + ] + }, + "reply": null, + "notes": "Requires `USE_LED_STRIP`. Expects `1 + sizeof(ledConfig_t)` bytes. Returns error if index invalid. Calls `reevaluateLedConfig()`.", + "description": "Sets the configuration for a single LED on the strip using the `ledConfig_t` structure. Supersedes `MSP_SET_LED_STRIP_CONFIG`." + }, + "MSP2_INAV_FW_APPROACH": { + "code": 8266, + "mspv": 2, + "request": { + "payload": [ + { + "name": "approachIndex", + "ctype": "uint8_t", + "desc": "Index of the approach setting (0 to `MAX_FW_LAND_APPOACH_SETTINGS - 1`)", + "units": "" + } + ] + }, + "reply": { + "payload": [ + { + "name": "approachIndex", + "ctype": "uint8_t", + "desc": "Index requested", + "units": "Index" + }, + { + "name": "approachAlt", + "ctype": "int32_t", + "desc": "Signed altitude for the approach phase (`navFwAutolandApproach_t.approachAlt`)", + "units": "cm" + }, + { + "name": "landAlt", + "ctype": "int32_t", + "desc": "Signed altitude for the final landing phase (`navFwAutolandApproach_t.landAlt`)", + "units": "cm" + }, + { + "name": "approachDirection", + "ctype": "uint8_t", + "desc": "Enum `fwAutolandApproachDirection_e`: Direction of approach (From WP, Specific Heading)", + "units": "Enum", + "enum": "fwAutolandApproachDirection_e" + }, + { + "name": "landHeading1", + "ctype": "int16_t", + "desc": "Primary landing heading (if approachDirection requires it)", + "units": "degrees" + }, + { + "name": "landHeading2", + "ctype": "int16_t", + "desc": "Secondary landing heading (if approachDirection requires it)", + "units": "degrees" + }, + { + "name": "isSeaLevelRef", + "ctype": "uint8_t", + "desc": "1 if altitudes are relative to sea level, 0 if relative to home", + "units": "Boolean" + } + ] + }, + "notes": "Requires `USE_FW_AUTOLAND`. Used by `mspFwApproachOutCommand`. See `MSP2_INAV_SET_FW_APPROACH` for setting.", + "description": "Get or Set configuration for a specific Fixed Wing Autoland approach." + }, + "MSP2_INAV_SET_FW_APPROACH": { + "code": 8267, + "mspv": 2, + "request": { + "payload": [ + { + "name": "approachIndex", + "ctype": "uint8_t", + "desc": "Index of the approach setting (0 to `MAX_FW_LAND_APPOACH_SETTINGS - 1`)", + "units": "Index" + }, + { + "name": "approachAlt", + "ctype": "int32_t", + "desc": "Signed approach altitude (`navFwAutolandApproach_t.approachAlt`)", + "units": "cm" + }, + { + "name": "landAlt", + "ctype": "int32_t", + "desc": "Signed landing altitude (`navFwAutolandApproach_t.landAlt`)", + "units": "cm" + }, + { + "name": "approachDirection", + "ctype": "uint8_t", + "desc": "Enum `fwAutolandApproachDirection_e` Sets approach direction", + "units": "Enum", + "enum": "fwAutolandApproachDirection_e" + }, + { + "name": "landHeading1", + "ctype": "int16_t", + "desc": "Sets primary landing heading", + "units": "degrees" + }, + { + "name": "landHeading2", + "ctype": "int16_t", + "desc": "Sets secondary landing heading", + "units": "degrees" + }, + { + "name": "isSeaLevelRef", + "ctype": "uint8_t", + "desc": "Sets altitude reference", + "units": "Boolean" + } + ] + }, + "reply": null, + "notes": "Requires `USE_FW_AUTOLAND`. Expects 15 bytes. Returns error if index invalid.", + "description": "Sets the configuration for a specific Fixed Wing Autoland approach." + }, + "MSP2_INAV_GPS_UBLOX_COMMAND": { + "code": 8272, + "mspv": 2, + "request": { + "payload": [ + { + "name": "ubxCommand", + "desc": "Raw U-Blox UBX protocol command frame (including header, class, ID, length, payload, checksum)", + "ctype": "uint8_t", + "array": true, + "array_size": 0, + "units": "" + } + ] + }, + "reply": null, + "variable_len": true, + "notes": "Requires GPS feature enabled (`FEATURE_GPS`) and the GPS driver to be U-Blox (`isGpsUblox()`). Payload must be at least 8 bytes (minimum UBX frame size). Use with extreme caution, incorrect commands can misconfigure the GPS module. Calls `gpsUbloxSendCommand()`.", + "description": "Sends a raw command directly to a U-Blox GPS module connected to the FC." + }, + "MSP2_INAV_RATE_DYNAMICS": { + "code": 8288, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "sensitivityCenter", + "ctype": "uint8_t", + "desc": "Sensitivity at stick center (`currentControlRateProfile->rateDynamics.sensitivityCenter`)", + "units": "%" + }, + { + "name": "sensitivityEnd", + "ctype": "uint8_t", + "desc": "Sensitivity at stick ends (`currentControlRateProfile->rateDynamics.sensitivityEnd`)", + "units": "%" + }, + { + "name": "correctionCenter", + "ctype": "uint8_t", + "desc": "Correction strength at stick center (`currentControlRateProfile->rateDynamics.correctionCenter`)", + "units": "%" + }, + { + "name": "correctionEnd", + "ctype": "uint8_t", + "desc": "Correction strength at stick ends (`currentControlRateProfile->rateDynamics.correctionEnd`)", + "units": "%" + }, + { + "name": "weightCenter", + "ctype": "uint8_t", + "desc": "Transition weight at stick center (`currentControlRateProfile->rateDynamics.weightCenter`)", + "units": "%" + }, + { + "name": "weightEnd", + "ctype": "uint8_t", + "desc": "Transition weight at stick ends (`currentControlRateProfile->rateDynamics.weightEnd`)", + "units": "%" + } + ] + }, + "notes": "Requires `USE_RATE_DYNAMICS`.", + "description": "Retrieves Rate Dynamics configuration parameters for the current control rate profile." + }, + "MSP2_INAV_SET_RATE_DYNAMICS": { + "code": 8289, + "mspv": 2, + "request": { + "payload": [ + { + "name": "sensitivityCenter", + "ctype": "uint8_t", + "desc": "Sets sensitivity at center", + "units": "%" + }, + { + "name": "sensitivityEnd", + "ctype": "uint8_t", + "desc": "Sets sensitivity at ends", + "units": "%" + }, + { + "name": "correctionCenter", + "ctype": "uint8_t", + "desc": "Sets correction at center", + "units": "%" + }, + { + "name": "correctionEnd", + "ctype": "uint8_t", + "desc": "Sets correction at ends", + "units": "%" + }, + { + "name": "weightCenter", + "ctype": "uint8_t", + "desc": "Sets weight at center", + "units": "%" + }, + { + "name": "weightEnd", + "ctype": "uint8_t", + "desc": "Sets weight at ends", + "units": "%" + } + ] + }, + "reply": null, + "notes": "Requires `USE_RATE_DYNAMICS`. Expects 6 bytes.", + "description": "Sets Rate Dynamics configuration parameters for the current control rate profile." + }, + "MSP2_INAV_EZ_TUNE": { + "code": 8304, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "enabled", + "ctype": "uint8_t", + "desc": "Boolean: 1 if EZ-Tune is enabled (`ezTune()->enabled`)", + "units": "" + }, + { + "name": "filterHz", + "ctype": "uint16_t", + "desc": "Filter frequency used during tuning (`ezTune()->filterHz`)", + "units": "" + }, + { + "name": "axisRatio", + "ctype": "uint8_t", + "desc": "Roll vs Pitch axis tuning ratio (`ezTune()->axisRatio`)", + "units": "" + }, + { + "name": "response", + "ctype": "uint8_t", + "desc": "Desired response characteristic (`ezTune()->response`)", + "units": "" + }, + { + "name": "damping", + "ctype": "uint8_t", + "desc": "Desired damping characteristic (`ezTune()->damping`)", + "units": "" + }, + { + "name": "stability", + "ctype": "uint8_t", + "desc": "Stability preference (`ezTune()->stability`)", + "units": "" + }, + { + "name": "aggressiveness", + "ctype": "uint8_t", + "desc": "Aggressiveness preference (`ezTune()->aggressiveness`)", + "units": "" + }, + { + "name": "rate", + "ctype": "uint8_t", + "desc": "Resulting rate setting (`ezTune()->rate`)", + "units": "" + }, + { + "name": "expo", + "ctype": "uint8_t", + "desc": "Resulting expo setting (`ezTune()->expo`)", + "units": "" + }, + { + "name": "snappiness", + "ctype": "uint8_t", + "desc": "Snappiness preference (`ezTune()->snappiness`)", + "units": "" + } + ] + }, + "notes": "Requires `USE_EZ_TUNE`. Calls `ezTuneUpdate()` before sending.", + "description": "Retrieves the current EZ-Tune parameters." + }, + "MSP2_INAV_EZ_TUNE_SET": { + "code": 8305, + "mspv": 2, + "request": { + "payload": [ + { + "name": "enabled", + "ctype": "uint8_t", + "desc": "Sets enabled state", + "units": "" + }, + { + "name": "filterHz", + "ctype": "uint16_t", + "desc": "Sets filter frequency", + "units": "" + }, + { + "name": "axisRatio", + "ctype": "uint8_t", + "desc": "Sets axis ratio", + "units": "" + }, + { + "name": "response", + "ctype": "uint8_t", + "desc": "Sets response characteristic", + "units": "" + }, + { + "name": "damping", + "ctype": "uint8_t", + "desc": "Sets damping characteristic", + "units": "" + }, + { + "name": "stability", + "ctype": "uint8_t", + "desc": "Sets stability preference", + "units": "" + }, + { + "name": "aggressiveness", + "ctype": "uint8_t", + "desc": "Sets aggressiveness preference", + "units": "" + }, + { + "name": "rate", + "ctype": "uint8_t", + "desc": "Sets rate setting", + "units": "" + }, + { + "name": "expo", + "ctype": "uint8_t", + "desc": "Sets expo setting", + "units": "" + }, + { + "name": "snappiness", + "ctype": "uint8_t", + "desc": "(Optional) Sets snappiness preference", + "units": "", + "optional": true + } + ] + }, + "reply": null, + "notes": "Requires `USE_EZ_TUNE`. Expects 10 or 11 bytes. Calls `ezTuneUpdate()` after setting parameters.", + "description": "Sets the EZ-Tune parameters and triggers an update." + }, + "MSP2_INAV_SELECT_MIXER_PROFILE": { + "code": 8320, + "mspv": 2, + "request": { + "payload": [ + { + "name": "mixerProfileIndex", + "ctype": "uint8_t", + "desc": "Index of the mixer profile to activate (0-based)", + "units": "" + } + ] + }, + "reply": null, + "notes": "Expects 1 byte. Will fail if armed. Calls `setConfigMixerProfileAndWriteEEPROM()`. Only applicable if `MAX_MIXER_PROFILE_COUNT` > 1.", + "description": "Selects the active mixer profile and saves configuration." + }, + "MSP2_ADSB_VEHICLE_LIST": { + "code": 8336, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "maxVehicles", + "ctype": "uint8_t", + "desc": "Maximum number of vehicles tracked (`MAX_ADSB_VEHICLES`). 0 if `USE_ADSB` disabled", + "units": "" + }, + { + "name": "callsignLength", + "ctype": "uint8_t", + "desc": "Maximum length of callsign string (`ADSB_CALL_SIGN_MAX_LENGTH`). 0 if `USE_ADSB` disabled", + "units": "" + }, + { + "name": "totalVehicleMsgs", + "ctype": "uint32_t", + "desc": "Total vehicle messages received (`getAdsbStatus()->vehiclesMessagesTotal`). 0 if `USE_ADSB` disabled", + "units": "" + }, + { + "name": "totalHeartbeatMsgs", + "ctype": "uint32_t", + "desc": "Total heartbeat messages received (`getAdsbStatus()->heartbeatMessagesTotal`). 0 if `USE_ADSB` disabled", + "units": "" + }, + { + "repeating": "maxVehicles", + "payload": [ + { + "name": "callsign", + "desc": "Fixed-length callsign from `adsbVehicle->vehicleValues.callsign` (padded with NULs if shorter).", + "ctype": "char", + "array": true, + "array_size": 9, + "array_size_define": "ADSB_CALL_SIGN_MAX_LENGTH", + "units": "" + }, + { + "name": "icao", + "ctype": "uint32_t", + "desc": "ICAO address (`adsbVehicle->vehicleValues.icao`).", + "units": "" + }, + { + "name": "lat", + "ctype": "int32_t", + "desc": "Latitude in degrees * 1e7 (`adsbVehicle->vehicleValues.lat`).", + "units": "1e-7 deg" + }, + { + "name": "lon", + "ctype": "int32_t", + "desc": "Longitude in degrees * 1e7 (`adsbVehicle->vehicleValues.lon`).", + "units": "1e-7 deg" + }, + { + "name": "alt", + "ctype": "int32_t", + "desc": "Altitude above sea level (`adsbVehicle->vehicleValues.alt`).", + "units": "cm" + }, + { + "name": "headingDeg", + "ctype": "uint16_t", + "desc": "Course over ground in whole degrees (`CENTIDEGREES_TO_DEGREES(vehicleValues.heading)`).", + "units": "deg" + }, + { + "name": "tslc", + "ctype": "uint8_t", + "desc": "Time since last communication (`adsbVehicle->vehicleValues.tslc`).", + "units": "s" + }, + { + "name": "emitterType", + "ctype": "uint8_t", + "desc": "Emitter category (`adsbVehicle->vehicleValues.emitterType`) (refers to enum 'ADSB_EMITTER_TYPE', but none found)", + "units": "" + }, + { + "name": "ttl", + "ctype": "uint8_t", + "desc": "TTL counter used for list maintenance (`adsbVehicle->ttl`).", + "units": "" + } + ] + } + ] + }, + "variable_len": "maxVehicles", + "notes": "Requires `USE_ADSB`. Only a subset of `adsbVehicle_t` is transmitted (callsign, core values, heading in whole degrees, TSLC, emitter type, TTL).", + "description": "Retrieves the list of currently tracked ADSB (Automatic Dependent Surveillance–Broadcast) vehicles. See `adsbVehicle_t` and `adsbVehicleValues_t` in `io/adsb.h` for the exact structure fields." + }, + "MSP2_INAV_CUSTOM_OSD_ELEMENTS": { + "code": 8448, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "maxElements", + "ctype": "uint8_t", + "desc": "Maximum number of custom elements (`MAX_CUSTOM_ELEMENTS`)", + "units": "" + }, + { + "name": "maxTextLength", + "ctype": "uint8_t", + "desc": "Maximum length of the text part (`OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1`)", + "units": "" + }, + { + "name": "maxParts", + "ctype": "uint8_t", + "desc": "Maximum number of parts per element (`CUSTOM_ELEMENTS_PARTS`)", + "units": "" + } + ] + }, + "notes": "Requires `USE_PROGRAMMING_FRAMEWORK`.", + "description": "Retrieves counts related to custom OSD elements defined by the programming framework." + }, + "MSP2_INAV_CUSTOM_OSD_ELEMENT": { + "code": 8449, + "mspv": 2, + "request": { + "payload": [ + { + "name": "elementIndex", + "ctype": "uint8_t", + "desc": "Index of the custom element (0 to `MAX_CUSTOM_ELEMENTS - 1`)" + } + ] + }, + "reply": { + "payload": [ + { + "repeating": "CUSTOM_ELEMENTS_PARTS", + "payload": [ + { + "name": "partType", + "ctype": "uint8_t", + "enum": "osdCustomElementType_e", + "desc": "Type of this part" + }, + { + "name": "partValue", + "ctype": "uint16_t", + "desc": "Value/ID associated with this part" + } + ] + }, + { + "name": "visibilityType", + "ctype": "uint8_t", + "enum": "osdCustomElementTypeVisibility_e", + "desc": "Visibility condition source" + }, + { + "name": "visibilityValue", + "ctype": "uint16_t", + "desc": "Value/ID of the visibility condition source" + }, + { + "name": "elementText", + "desc": "Static text bytes", + "ctype": "char", + "array": true, + "array_size": 0, + "array_size_define": "OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1" + } + ] + }, + "notes": "Reply emitted only if idx < MAX_CUSTOM_ELEMENTS; otherwise no body is written.", + "description": "Gets the configuration of a single custom OSD element defined by the programming framework." + }, + "MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS": { + "code": 8450, + "mspv": 2, + "request": { + "payload": [ + { + "name": "elementIndex", + "ctype": "uint8_t", + "desc": "Index of the custom element (0 to `MAX_CUSTOM_ELEMENTS - 1`)" + }, + { + "repeating": "CUSTOM_ELEMENTS_PARTS", + "payload": [ + { + "name": "partType", + "ctype": "uint8_t", + "enum": "osdCustomElementType_e", + "desc": "Type of this part" + }, + { + "name": "partValue", + "ctype": "uint16_t", + "desc": "Value/ID associated with this part" + } + ] + }, + { + "name": "visibilityType", + "ctype": "uint8_t", + "enum": "osdCustomElementTypeVisibility_e", + "desc": "Visibility condition source" + }, + { + "name": "visibilityValue", + "ctype": "uint16_t", + "desc": "Value/ID of the visibility condition source" + }, + { + "name": "elementText", + "desc": "Raw bytes", + "ctype": "char", + "array": true, + "array_size": 0, + "array_size_define": "OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1" + } + ] + }, + "reply": null, + "notes": "Payload length must be (OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1) + (CUSTOM_ELEMENTS_PARTS * 3) + 4 bytes including elementIndex. elementIndex must be < MAX_CUSTOM_ELEMENTS. Each partType must be < CUSTOM_ELEMENT_TYPE_END. Firmware NUL-terminates elementText internally.", + "description": "Sets the configuration of one custom OSD element." + }, + "MSP2_INAV_OUTPUT_MAPPING_EXT2": { + "code": 8461, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "timerId", + "ctype": "uint8_t", + "desc": "Hardware timer identifier (e.g., `TIM1`, `TIM2`). SITL uses index", + "units": "" + }, + { + "name": "usageFlags", + "ctype": "uint32_t", + "desc": "Full 32-bit timer usage flags (`TIM_USE_*`)", + "units": "" + }, + { + "name": "pinLabel", + "ctype": "uint8_t", + "desc": "Label for special pin usage (`PIN_LABEL_*` enum, e.g., `PIN_LABEL_LED`). 0 (`PIN_LABEL_NONE`) otherwise", + "units": "", + "enum": "pinLabel_e" + } + ], + "repeating": "for each Motor/Servo timer" + }, + "variable_len": "for each Motor/Servo timer", + "notes": "Provides complete usage flags and helps identify pins repurposed for functions like LED strip.", + "description": "Retrieves the full extended output mapping configuration (timer ID, full 32-bit usage flags, and pin label). Supersedes `MSP2_INAV_OUTPUT_MAPPING_EXT`." + }, + "MSP2_INAV_SERVO_CONFIG": { + "code": 8704, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "min", + "ctype": "int16_t", + "desc": "Minimum servo endpoint (`servoParams(i)->min`)", + "units": "PWM" + }, + { + "name": "max", + "ctype": "int16_t", + "desc": "Maximum servo endpoint (`servoParams(i)->max`)", + "units": "PWM" + }, + { + "name": "middle", + "ctype": "int16_t", + "desc": "Middle/Neutral servo position (`servoParams(i)->middle`)", + "units": "PWM" + }, + { + "name": "rate", + "ctype": "int8_t", + "desc": "Servo rate/scaling (`servoParams(i)->rate`)", + "units": "% (-125 to 125)" + } + ], + "repeating": "MAX_SUPPORTED_SERVOS" + }, + "variable_len": "MAX_SUPPORTED_SERVOS", + "notes": "", + "description": "Retrieves the configuration parameters for all supported servos (min, max, middle, rate). Supersedes `MSP_SERVO_CONFIGURATIONS`." + }, + "MSP2_INAV_SET_SERVO_CONFIG": { + "code": 8705, + "mspv": 2, + "request": { + "payload": [ + { + "name": "servoIndex", + "ctype": "uint8_t", + "desc": "Index of the servo to configure (0 to `MAX_SUPPORTED_SERVOS - 1`)", + "units": "Index" + }, + { + "name": "min", + "ctype": "int16_t", + "desc": "Sets minimum servo endpoint", + "units": "PWM" + }, + { + "name": "max", + "ctype": "int16_t", + "desc": "Sets maximum servo endpoint", + "units": "PWM" + }, + { + "name": "middle", + "ctype": "int16_t", + "desc": "Sets middle/neutral servo position", + "units": "PWM" + }, + { + "name": "rate", + "ctype": "int8_t", + "desc": "Sets servo rate/scaling", + "units": "% (-125 to 125)" + } + ] + }, + "reply": null, + "notes": "Expects 8 bytes. Returns error if index invalid. Calls `servoComputeScalingFactors()`.", + "description": "Sets the configuration parameters for a single servo. Supersedes `MSP_SET_SERVO_CONFIGURATION`." + }, + "MSP2_INAV_GEOZONE": { + "code": 8720, + "mspv": 2, + "request": { + "payload": [ + { + "name": "geozoneIndex", + "ctype": "uint8_t", + "desc": "Index of the geozone (0 to `MAX_GEOZONES_IN_CONFIG - 1`)", + "units": "" + } + ] + }, + "reply": { + "payload": [ + { + "name": "geozoneIndex", + "ctype": "uint8_t", + "desc": "Index requested", + "units": "" + }, + { + "name": "type", + "ctype": "uint8_t", + "desc": "Define (`GEOZONE_TYPE_EXCLUSIVE/INCLUSIVE`): Zone type (Inclusion/Exclusion)", + "units": "" + }, + { + "name": "shape", + "ctype": "uint8_t", + "desc": "Define (`GEOZONE_SHAPE_CIRCULAR/POLYGON`): Zone shape (Polygon/Circular)", + "units": "" + }, + { + "name": "minAltitude", + "ctype": "int32_t", + "desc": "Minimum allowed altitude within the zone (`geoZonesConfig(idx)->minAltitude`)", + "units": "cm" + }, + { + "name": "maxAltitude", + "ctype": "int32_t", + "desc": "Maximum allowed altitude within the zone (`geoZonesConfig(idx)->maxAltitude`)", + "units": "cm" + }, + { + "name": "isSeaLevelRef", + "ctype": "uint8_t", + "desc": "Boolean: 1 if altitudes are relative to sea level, 0 if relative to home", + "units": "" + }, + { + "name": "fenceAction", + "ctype": "uint8_t", + "desc": "Enum (`fenceAction_e`): Action to take upon boundary violation", + "units": "", + "enum": "fenceAction_e" + }, + { + "name": "vertexCount", + "ctype": "uint8_t", + "desc": "Number of vertices defined for this zone", + "units": "" + } + ] + }, + "notes": "Requires `USE_GEOZONE`. Used by `mspFcGeozoneOutCommand`.", + "description": "Get configuration for a specific Geozone." + }, + "MSP2_INAV_SET_GEOZONE": { + "code": 8721, + "mspv": 2, + "request": { + "payload": [ + { + "name": "geozoneIndex", + "ctype": "uint8_t", + "desc": "Index of the geozone (0 to `MAX_GEOZONES_IN_CONFIG - 1`)", + "units": "" + }, + { + "name": "type", + "ctype": "uint8_t", + "desc": "Define (`GEOZONE_TYPE_EXCLUSIVE/INCLUSIVE`): Zone type (Inclusion/Exclusion)", + "units": "" + }, + { + "name": "shape", + "ctype": "uint8_t", + "desc": "Define (`GEOZONE_SHAPE_CIRCULAR/POLYGON`): Zone shape (Polygon/Circular)", + "units": "" + }, + { + "name": "minAltitude", + "ctype": "int32_t", + "desc": "Minimum allowed altitude (`geoZonesConfigMutable()->minAltitude`)", + "units": "cm" + }, + { + "name": "maxAltitude", + "ctype": "int32_t", + "desc": "Maximum allowed altitude (`geoZonesConfigMutable()->maxAltitude`)", + "units": "cm" + }, + { + "name": "isSeaLevelRef", + "ctype": "uint8_t", + "desc": "Boolean: Altitude reference", + "units": "" + }, + { + "name": "fenceAction", + "ctype": "uint8_t", + "desc": "Enum (`fenceAction_e`): Action to take upon boundary violation", + "units": "", + "enum": "fenceAction_e" + }, + { + "name": "vertexCount", + "ctype": "uint8_t", + "desc": "Number of vertices to be defined (used for validation later)", + "units": "" + } + ] + }, + "reply": null, + "notes": "Requires `USE_GEOZONE`. Expects 14 bytes. Returns error if index invalid. Calls `geozoneResetVertices()`. Vertices must be set subsequently using `MSP2_INAV_SET_GEOZONE_VERTEX`.", + "description": "Sets the main configuration for a specific Geozone (type, shape, altitude, action). **This command resets (clears) all vertices associated with the zone.**" + }, + "MSP2_INAV_GEOZONE_VERTEX": { + "code": 8722, + "mspv": 2, + "request": { + "payload": [ + { + "name": "geozoneIndex", + "ctype": "uint8_t", + "desc": "Index of the geozone", + "units": "" + }, + { + "name": "vertexId", + "ctype": "uint8_t", + "desc": "Index of the vertex within the zone (0-based). For circles, 0 = center", + "units": "" + } + ] + }, + "reply": { + "payload": [ + { + "name": "geozoneIndex", + "ctype": "uint8_t", + "desc": "Geozone index requested", + "units": "Index" + }, + { + "name": "vertexId", + "ctype": "uint8_t", + "desc": "Vertex index requested", + "units": "Index" + }, + { + "name": "latitude", + "ctype": "int32_t", + "desc": "Vertex latitude", + "units": "deg * 1e7" + }, + { + "name": "longitude", + "ctype": "int32_t", + "desc": "Vertex longitude", + "units": "deg * 1e7" + }, + { + "name": "radius", + "ctype": "int32_t", + "desc": "If vertex is circle, Radius of the circular zone", + "units": "cm", + "optional": true + } + ] + }, + "variable_len": true, + "notes": "Requires `USE_GEOZONE`. Returns error if indexes are invalid or vertex doesn't exist. For circular zones, the radius is stored internally as the 'latitude' of the vertex with index 1.", + "description": "Get a specific vertex (or center+radius for circular zones) of a Geozone." + }, + "MSP2_INAV_SET_GEOZONE_VERTEX": { + "code": 8723, + "mspv": 2, + "variants": { + "polygon": { + "description": "Polygonal Geozone", + "request": { + "payload": [ + { + "name": "geozoneIndex", + "ctype": "uint8_t", + "desc": "Geozone index requested", + "units": "Index" + }, + { + "name": "vertexId", + "ctype": "uint8_t", + "desc": "Vertex index requested", + "units": "Index" + }, + { + "name": "latitude", + "ctype": "int32_t", + "desc": "Vertex latitude", + "units": "deg * 1e7" + }, + { + "name": "longitude", + "ctype": "int32_t", + "desc": "Vertex longitude", + "units": "deg * 1e7" + } + ] + }, + "reply": { + "payload": null + } + }, + "circle": { + "description": "Circular Geozone", + "request": { + "payload": [ + { + "name": "geozoneIndex", + "ctype": "uint8_t", + "desc": "Geozone index requested", + "units": "Index" + }, + { + "name": "vertexId", + "ctype": "uint8_t", + "desc": "Vertex index requested", + "units": "Index" + }, + { + "name": "latitude", + "ctype": "int32_t", + "desc": "Vertex/Center latitude", + "units": "deg * 1e7" + }, + { + "name": "longitude", + "ctype": "int32_t", + "desc": "Vertex/Center longitude", + "units": "deg * 1e7" + }, + { + "name": "radius", + "ctype": "int32_t", + "desc": "Radius of the circular zone", + "units": "cm" + } + ] + }, + "reply": { + "payload": null + } + } + }, + "notes": "Requires `USE_GEOZONE`. Expects 10 bytes (Polygon) or 14 bytes (Circular). Returns error if indexes invalid or if trying to set vertex beyond `vertexCount` defined in `MSP2_INAV_SET_GEOZONE`. Calls `geozoneSetVertex()`. For circular zones, sets center (vertex 0) and radius (vertex 1's latitude).", + "description": "Sets a specific vertex (or center+radius for circular zones) for a Geozone." + }, + "MSP2_INAV_SET_GVAR": { + "code": 8724, + "mspv": 2, + "request": { + "payload": [ + { + "name": "gvarIndex", + "ctype": "uint8_t", + "desc": "Index of the Global Variable to set", + "units": "Index" + }, + { + "name": "value", + "ctype": "int32_t", + "desc": "New value to store (clamped to configured min/max by `gvSet()`)", + "units": "" + } + ] + }, + "reply": null, + "notes": "Requires `USE_PROGRAMMING_FRAMEWORK`. Expects 5 bytes. Returns error if index is outside `MAX_GLOBAL_VARIABLES`.", + "description": "Sets the specified Global Variable (GVAR) to the provided value." + }, + "MSP2_INAV_FULL_LOCAL_POSE": { + "code": 8736, + "mspv": 2, + "request": null, + "reply": { + "payload": [ + { + "name": "roll", + "ctype": "int16_t", + "desc": "Roll angle (`attitude.values.roll`)", + "units": "deci-degrees" + }, + { + "name": "pitch", + "ctype": "int16_t", + "desc": "Pitch angle (`attitude.values.pitch`)", + "units": "deci-degrees" + }, + { + "name": "yaw", + "ctype": "int16_t", + "desc": "Yaw/Heading angle (`attitude.values.yaw`)", + "units": "deci-degrees" + }, + { + "name": "localPositionNorth", + "ctype": "int32_t", + "desc": "Estimated North coordinate in local NEU frame (`posControl.actualState.abs.pos.x`)", + "units": "cm" + }, + { + "name": "localVelocityNorth", + "ctype": "int16_t", + "desc": "Estimated North component of velocity in local NEU frame (`posControl.actualState.abs.vel.x`)", + "units": "cm/s" + }, + { + "name": "localPositionEast", + "ctype": "int32_t", + "desc": "Estimated East coordinate in local NEU frame (`posControl.actualState.abs.pos.y`)", + "units": "cm" + }, + { + "name": "localVelocityEast", + "ctype": "int16_t", + "desc": "Estimated East component of velocity in local NEU frame (`posControl.actualState.abs.vel.y`)", + "units": "cm/s" + }, + { + "name": "localPositionUp", + "ctype": "int32_t", + "desc": "Estimated Up coordinate in local NEU frame (`posControl.actualState.abs.pos.z`)", + "units": "cm" + }, + { + "name": "localVelocityUp", + "ctype": "int16_t", + "desc": "Estimated Up component of velocity in local NEU frame (`posControl.actualState.abs.vel.z`)", + "units": "cm/s" + } + ] + }, + "notes": "All attitude angles are in deci-degrees.", + "description": "Provides estimates of current attitude, local NEU position, and velocity." + }, + "MSP2_BETAFLIGHT_BIND": { + "code": 12288, + "mspv": 2, + "request": null, + "reply": null, + "notes": "Requires `rxConfig()->receiverType == RX_TYPE_SERIAL`. Requires `USE_SERIALRX_CRSF` or `USE_SERIALRX_SRXL2`. Calls `crsfBind()` or `srxl2Bind()` respectively. Returns error if receiver type or provider is not supported for binding.", + "description": "Initiates the receiver binding procedure for supported serial protocols (CRSF, SRXL2)." + } +} diff --git a/docs/development/msp/msp_ref.md b/docs/development/msp/msp_ref.md new file mode 100644 index 00000000000..5a413a1506c --- /dev/null +++ b/docs/development/msp/msp_ref.md @@ -0,0 +1,4538 @@ + +# INAV MSP Messages reference + +**This page is auto-generated from the [master INAV MSP definitions file](https://github.com/iNavFlight/inav/blob/master/docs/development/msp/msp_messages.json)** + +For details on the structure of MSP, see [The wiki page](https://github.com/iNavFlight/inav/wiki/MSP-V2) + +For list of enums, see [Enum documentation page](https://github.com/iNavFlight/inav/wiki/Enums-reference) + +For current generation code, see [documentation project](https://github.com/xznhj8129/msp_documentation) (temporary until official implementation) + + +**JSON file rev: 3 +** + +**Warning: Verification needed, exercise caution until completely verified for accuracy and cleared, especially for integer signs. Source-based generation/validation is forthcoming. Refer to source for absolute certainty** + +**If you find an error, it must be corrected in the JSON spec, not this markdown.** + +**Guide:** + +* **MSP Versions:** + * **MSPv1:** The original protocol. Uses command IDs from 0 to 254. + * **MSPv2:** An extended version. Uses command IDs from 0x1000 onwards. +* **Request Payload:** The request payload sent to the destination (usually the flight controller). May be empty or hold data for setting or requesting data from the FC. +* **Reply Payload:** The reply sent from the FC to the sender. May be empty or hold data. +* **Notes:** Pay attention to message notes and description. + +# Format: +## JSON format example: +``` + "MSP_API_VERSION": { + "code": 1, + "mspv": 1, + "request": null, + "reply": { + "payload": [ + { + "name": "mspProtocolVersion", + "ctype": "uint8_t", + "units": "", + "desc": "MSP Protocol version (`MSP_PROTOCOL_VERSION`, typically 0)." + }, + { + "name": "apiVersionMajor", + "ctype": "uint8_t", + "units": "", + "desc": "INAV API Major version (`API_VERSION_MAJOR`)." + }, + { + "name": "apiVersionMinor", + "ctype": "uint8_t", + "units": "", + "desc": "INAV API Minor version (`API_VERSION_MINOR`)." + } + ], + }, + "notes": "Used by configurators to check compatibility.", + "description": "Provides the MSP protocol version and the INAV API version." + }, +``` +## Message fields: +**name**: MSP message name\ +**code**: Integer message code\ +**description**: String with description of message\ +**request**: null or dict of data sent\ +**reply**: null or dict of data received\ +**variable_len**: Optional boolean, if true, message does not have a predefined fixed length and needs appropriate handling\ +**variants**: Optional special case, message has different cases of reply/request. Key/description is not a strict expression or code; just a readable condition\ +**not_implemented**: Optional special case, message is not implemented\ +**notes**: String with details of message + +## Data dict fields: +**payload**: Array of payload fields\ +**repeating**: Optional Special Case, integer or string of how many times the *entire* payload is repeated + +## Payload fields: +### Fields: +**name**: field name from code\ +**ctype**: Base C type of the value. Arrays list their element type here as well\ +**desc**: Optional string with description and details of field\ +**units**: Optional defined units\ +**enum**: Optional string of enum struct if value is an enum +**array**: Optional boolean to denote field is array of more values\ +**array_size**: If array, integer count of elements. Use `0` when the length is indeterminate/variable\ +**array_size_define**: Optional string naming the source `#define` that provides the size (informational only)\ +**repeating**: Optional Special case, contains array of more payload fields that are added Times * Key\ +**payload**: If repeating, contains more payload fields\ +**polymorph**: Optional boolean special case, field does not have a defined C type and could be anything + +**Simple value** +``` +{ + "name": "mspProtocolVersion", + "ctype": "uint8_t", + "units": "", + "desc": "MSP Protocol version (`MSP_PROTOCOL_VERSION`, typically 0)." +}, +``` +**Fixed length array** +``` +{ + "name": "fcVariantIdentifier", + "ctype": "char", + "desc": "4-character identifier string (e.g., \"INAV\"). Defined by `flightControllerIdentifier", + "array": true, + "array_size": 4, + "units": "" +} +``` +**Array sized via define** +``` +{ + "name": "buildDate", + "ctype": "char", + "desc": "Build date string (e.g., \"Dec 31 2023\").", + "array": true, + "array_size": 11, + "array_size_define": "BUILD_DATE_LENGTH", + "units": "" +} +``` +**Undefined length array** +``` +{ + "name": "firmwareChunk", + "ctype": "uint8_t", + "desc": "Chunk of firmware data", + "array": true, + "array_size": 0, +} +``` +**As of yet unknown length array** +``` +{ + "name": "elementText", + "ctype": "char", + "desc": "Static text bytes, not NUL-terminated and not yet sized.", + "array": true, + "array_size": 0 +} +``` +**Nested array with struct** +``` +{ + "repeating": "maxVehicles", + "payload": [ + { + "name": "adsbVehicle", + "ctype": "adsbVehicle_t", + "desc": "Array of `adsbVehicle_t` Repeated `maxVehicles` times", + "repeating": "maxVehicles", + "array": true, + "array_size": 0, + "units": "" + } + ] +} +``` + + +--- + +## Index +### MSPv1 +[1 - MSP_API_VERSION](#msp_api_version) +[2 - MSP_FC_VARIANT](#msp_fc_variant) +[3 - MSP_FC_VERSION](#msp_fc_version) +[4 - MSP_BOARD_INFO](#msp_board_info) +[5 - MSP_BUILD_INFO](#msp_build_info) +[6 - MSP_INAV_PID](#msp_inav_pid) +[7 - MSP_SET_INAV_PID](#msp_set_inav_pid) +[10 - MSP_NAME](#msp_name) +[11 - MSP_SET_NAME](#msp_set_name) +[12 - MSP_NAV_POSHOLD](#msp_nav_poshold) +[13 - MSP_SET_NAV_POSHOLD](#msp_set_nav_poshold) +[14 - MSP_CALIBRATION_DATA](#msp_calibration_data) +[15 - MSP_SET_CALIBRATION_DATA](#msp_set_calibration_data) +[16 - MSP_POSITION_ESTIMATION_CONFIG](#msp_position_estimation_config) +[17 - MSP_SET_POSITION_ESTIMATION_CONFIG](#msp_set_position_estimation_config) +[18 - MSP_WP_MISSION_LOAD](#msp_wp_mission_load) +[19 - MSP_WP_MISSION_SAVE](#msp_wp_mission_save) +[20 - MSP_WP_GETINFO](#msp_wp_getinfo) +[21 - MSP_RTH_AND_LAND_CONFIG](#msp_rth_and_land_config) +[22 - MSP_SET_RTH_AND_LAND_CONFIG](#msp_set_rth_and_land_config) +[23 - MSP_FW_CONFIG](#msp_fw_config) +[24 - MSP_SET_FW_CONFIG](#msp_set_fw_config) +[34 - MSP_MODE_RANGES](#msp_mode_ranges) +[35 - MSP_SET_MODE_RANGE](#msp_set_mode_range) +[36 - MSP_FEATURE](#msp_feature) +[37 - MSP_SET_FEATURE](#msp_set_feature) +[38 - MSP_BOARD_ALIGNMENT](#msp_board_alignment) +[39 - MSP_SET_BOARD_ALIGNMENT](#msp_set_board_alignment) +[40 - MSP_CURRENT_METER_CONFIG](#msp_current_meter_config) +[41 - MSP_SET_CURRENT_METER_CONFIG](#msp_set_current_meter_config) +[42 - MSP_MIXER](#msp_mixer) +[43 - MSP_SET_MIXER](#msp_set_mixer) +[44 - MSP_RX_CONFIG](#msp_rx_config) +[45 - MSP_SET_RX_CONFIG](#msp_set_rx_config) +[46 - MSP_LED_COLORS](#msp_led_colors) +[47 - MSP_SET_LED_COLORS](#msp_set_led_colors) +[48 - MSP_LED_STRIP_CONFIG](#msp_led_strip_config) +[49 - MSP_SET_LED_STRIP_CONFIG](#msp_set_led_strip_config) +[50 - MSP_RSSI_CONFIG](#msp_rssi_config) +[51 - MSP_SET_RSSI_CONFIG](#msp_set_rssi_config) +[52 - MSP_ADJUSTMENT_RANGES](#msp_adjustment_ranges) +[53 - MSP_SET_ADJUSTMENT_RANGE](#msp_set_adjustment_range) +[54 - MSP_CF_SERIAL_CONFIG](#msp_cf_serial_config) +[55 - MSP_SET_CF_SERIAL_CONFIG](#msp_set_cf_serial_config) +[56 - MSP_VOLTAGE_METER_CONFIG](#msp_voltage_meter_config) +[57 - MSP_SET_VOLTAGE_METER_CONFIG](#msp_set_voltage_meter_config) +[58 - MSP_SONAR_ALTITUDE](#msp_sonar_altitude) +[64 - MSP_RX_MAP](#msp_rx_map) +[65 - MSP_SET_RX_MAP](#msp_set_rx_map) +[68 - MSP_REBOOT](#msp_reboot) +[70 - MSP_DATAFLASH_SUMMARY](#msp_dataflash_summary) +[71 - MSP_DATAFLASH_READ](#msp_dataflash_read) +[72 - MSP_DATAFLASH_ERASE](#msp_dataflash_erase) +[73 - MSP_LOOP_TIME](#msp_loop_time) +[74 - MSP_SET_LOOP_TIME](#msp_set_loop_time) +[75 - MSP_FAILSAFE_CONFIG](#msp_failsafe_config) +[76 - MSP_SET_FAILSAFE_CONFIG](#msp_set_failsafe_config) +[79 - MSP_SDCARD_SUMMARY](#msp_sdcard_summary) +[80 - MSP_BLACKBOX_CONFIG](#msp_blackbox_config) +[81 - MSP_SET_BLACKBOX_CONFIG](#msp_set_blackbox_config) +[82 - MSP_TRANSPONDER_CONFIG](#msp_transponder_config) +[83 - MSP_SET_TRANSPONDER_CONFIG](#msp_set_transponder_config) +[84 - MSP_OSD_CONFIG](#msp_osd_config) +[85 - MSP_SET_OSD_CONFIG](#msp_set_osd_config) +[86 - MSP_OSD_CHAR_READ](#msp_osd_char_read) +[87 - MSP_OSD_CHAR_WRITE](#msp_osd_char_write) +[88 - MSP_VTX_CONFIG](#msp_vtx_config) +[89 - MSP_SET_VTX_CONFIG](#msp_set_vtx_config) +[90 - MSP_ADVANCED_CONFIG](#msp_advanced_config) +[91 - MSP_SET_ADVANCED_CONFIG](#msp_set_advanced_config) +[92 - MSP_FILTER_CONFIG](#msp_filter_config) +[93 - MSP_SET_FILTER_CONFIG](#msp_set_filter_config) +[94 - MSP_PID_ADVANCED](#msp_pid_advanced) +[95 - MSP_SET_PID_ADVANCED](#msp_set_pid_advanced) +[96 - MSP_SENSOR_CONFIG](#msp_sensor_config) +[97 - MSP_SET_SENSOR_CONFIG](#msp_set_sensor_config) +[98 - MSP_SPECIAL_PARAMETERS](#msp_special_parameters) +[99 - MSP_SET_SPECIAL_PARAMETERS](#msp_set_special_parameters) +[100 - MSP_IDENT](#msp_ident) +[101 - MSP_STATUS](#msp_status) +[102 - MSP_RAW_IMU](#msp_raw_imu) +[103 - MSP_SERVO](#msp_servo) +[104 - MSP_MOTOR](#msp_motor) +[105 - MSP_RC](#msp_rc) +[106 - MSP_RAW_GPS](#msp_raw_gps) +[107 - MSP_COMP_GPS](#msp_comp_gps) +[108 - MSP_ATTITUDE](#msp_attitude) +[109 - MSP_ALTITUDE](#msp_altitude) +[110 - MSP_ANALOG](#msp_analog) +[111 - MSP_RC_TUNING](#msp_rc_tuning) +[113 - MSP_ACTIVEBOXES](#msp_activeboxes) +[114 - MSP_MISC](#msp_misc) +[116 - MSP_BOXNAMES](#msp_boxnames) +[117 - MSP_PIDNAMES](#msp_pidnames) +[118 - MSP_WP](#msp_wp) +[119 - MSP_BOXIDS](#msp_boxids) +[120 - MSP_SERVO_CONFIGURATIONS](#msp_servo_configurations) +[121 - MSP_NAV_STATUS](#msp_nav_status) +[122 - MSP_NAV_CONFIG](#msp_nav_config) +[124 - MSP_3D](#msp_3d) +[125 - MSP_RC_DEADBAND](#msp_rc_deadband) +[126 - MSP_SENSOR_ALIGNMENT](#msp_sensor_alignment) +[127 - MSP_LED_STRIP_MODECOLOR](#msp_led_strip_modecolor) +[130 - MSP_BATTERY_STATE](#msp_battery_state) +[137 - MSP_VTXTABLE_BAND](#msp_vtxtable_band) +[138 - MSP_VTXTABLE_POWERLEVEL](#msp_vtxtable_powerlevel) +[150 - MSP_STATUS_EX](#msp_status_ex) +[151 - MSP_SENSOR_STATUS](#msp_sensor_status) +[160 - MSP_UID](#msp_uid) +[164 - MSP_GPSSVINFO](#msp_gpssvinfo) +[166 - MSP_GPSSTATISTICS](#msp_gpsstatistics) +[180 - MSP_OSD_VIDEO_CONFIG](#msp_osd_video_config) +[181 - MSP_SET_OSD_VIDEO_CONFIG](#msp_set_osd_video_config) +[182 - MSP_DISPLAYPORT](#msp_displayport) +[186 - MSP_SET_TX_INFO](#msp_set_tx_info) +[187 - MSP_TX_INFO](#msp_tx_info) +[200 - MSP_SET_RAW_RC](#msp_set_raw_rc) +[201 - MSP_SET_RAW_GPS](#msp_set_raw_gps) +[203 - MSP_SET_BOX](#msp_set_box) +[204 - MSP_SET_RC_TUNING](#msp_set_rc_tuning) +[205 - MSP_ACC_CALIBRATION](#msp_acc_calibration) +[206 - MSP_MAG_CALIBRATION](#msp_mag_calibration) +[207 - MSP_SET_MISC](#msp_set_misc) +[208 - MSP_RESET_CONF](#msp_reset_conf) +[209 - MSP_SET_WP](#msp_set_wp) +[210 - MSP_SELECT_SETTING](#msp_select_setting) +[211 - MSP_SET_HEAD](#msp_set_head) +[212 - MSP_SET_SERVO_CONFIGURATION](#msp_set_servo_configuration) +[214 - MSP_SET_MOTOR](#msp_set_motor) +[215 - MSP_SET_NAV_CONFIG](#msp_set_nav_config) +[217 - MSP_SET_3D](#msp_set_3d) +[218 - MSP_SET_RC_DEADBAND](#msp_set_rc_deadband) +[219 - MSP_SET_RESET_CURR_PID](#msp_set_reset_curr_pid) +[220 - MSP_SET_SENSOR_ALIGNMENT](#msp_set_sensor_alignment) +[221 - MSP_SET_LED_STRIP_MODECOLOR](#msp_set_led_strip_modecolor) +[239 - MSP_SET_ACC_TRIM](#msp_set_acc_trim) +[240 - MSP_ACC_TRIM](#msp_acc_trim) +[241 - MSP_SERVO_MIX_RULES](#msp_servo_mix_rules) +[242 - MSP_SET_SERVO_MIX_RULE](#msp_set_servo_mix_rule) +[245 - MSP_SET_PASSTHROUGH](#msp_set_passthrough) +[246 - MSP_RTC](#msp_rtc) +[247 - MSP_SET_RTC](#msp_set_rtc) +[250 - MSP_EEPROM_WRITE](#msp_eeprom_write) +[251 - MSP_RESERVE_1](#msp_reserve_1) +[252 - MSP_RESERVE_2](#msp_reserve_2) +[253 - MSP_DEBUGMSG](#msp_debugmsg) +[254 - MSP_DEBUG](#msp_debug) + +### MSPv2 +[4097 - MSP2_COMMON_TZ](#msp2_common_tz) +[4098 - MSP2_COMMON_SET_TZ](#msp2_common_set_tz) +[4099 - MSP2_COMMON_SETTING](#msp2_common_setting) +[4100 - MSP2_COMMON_SET_SETTING](#msp2_common_set_setting) +[4101 - MSP2_COMMON_MOTOR_MIXER](#msp2_common_motor_mixer) +[4102 - MSP2_COMMON_SET_MOTOR_MIXER](#msp2_common_set_motor_mixer) +[4103 - MSP2_COMMON_SETTING_INFO](#msp2_common_setting_info) +[4104 - MSP2_COMMON_PG_LIST](#msp2_common_pg_list) +[4105 - MSP2_COMMON_SERIAL_CONFIG](#msp2_common_serial_config) +[4106 - MSP2_COMMON_SET_SERIAL_CONFIG](#msp2_common_set_serial_config) +[4107 - MSP2_COMMON_SET_RADAR_POS](#msp2_common_set_radar_pos) +[4108 - MSP2_COMMON_SET_RADAR_ITD](#msp2_common_set_radar_itd) +[4109 - MSP2_COMMON_SET_MSP_RC_LINK_STATS](#msp2_common_set_msp_rc_link_stats) +[4110 - MSP2_COMMON_SET_MSP_RC_INFO](#msp2_common_set_msp_rc_info) +[4111 - MSP2_COMMON_GET_RADAR_GPS](#msp2_common_get_radar_gps) +[7937 - MSP2_SENSOR_RANGEFINDER](#msp2_sensor_rangefinder) +[7938 - MSP2_SENSOR_OPTIC_FLOW](#msp2_sensor_optic_flow) +[7939 - MSP2_SENSOR_GPS](#msp2_sensor_gps) +[7940 - MSP2_SENSOR_COMPASS](#msp2_sensor_compass) +[7941 - MSP2_SENSOR_BAROMETER](#msp2_sensor_barometer) +[7942 - MSP2_SENSOR_AIRSPEED](#msp2_sensor_airspeed) +[7943 - MSP2_SENSOR_HEADTRACKER](#msp2_sensor_headtracker) +[8192 - MSP2_INAV_STATUS](#msp2_inav_status) +[8193 - MSP2_INAV_OPTICAL_FLOW](#msp2_inav_optical_flow) +[8194 - MSP2_INAV_ANALOG](#msp2_inav_analog) +[8195 - MSP2_INAV_MISC](#msp2_inav_misc) +[8196 - MSP2_INAV_SET_MISC](#msp2_inav_set_misc) +[8197 - MSP2_INAV_BATTERY_CONFIG](#msp2_inav_battery_config) +[8198 - MSP2_INAV_SET_BATTERY_CONFIG](#msp2_inav_set_battery_config) +[8199 - MSP2_INAV_RATE_PROFILE](#msp2_inav_rate_profile) +[8200 - MSP2_INAV_SET_RATE_PROFILE](#msp2_inav_set_rate_profile) +[8201 - MSP2_INAV_AIR_SPEED](#msp2_inav_air_speed) +[8202 - MSP2_INAV_OUTPUT_MAPPING](#msp2_inav_output_mapping) +[8203 - MSP2_INAV_MC_BRAKING](#msp2_inav_mc_braking) +[8204 - MSP2_INAV_SET_MC_BRAKING](#msp2_inav_set_mc_braking) +[8205 - MSP2_INAV_OUTPUT_MAPPING_EXT](#msp2_inav_output_mapping_ext) +[8206 - MSP2_INAV_TIMER_OUTPUT_MODE](#msp2_inav_timer_output_mode) +[8207 - MSP2_INAV_SET_TIMER_OUTPUT_MODE](#msp2_inav_set_timer_output_mode) +[8208 - MSP2_INAV_MIXER](#msp2_inav_mixer) +[8209 - MSP2_INAV_SET_MIXER](#msp2_inav_set_mixer) +[8210 - MSP2_INAV_OSD_LAYOUTS](#msp2_inav_osd_layouts) +[8211 - MSP2_INAV_OSD_SET_LAYOUT_ITEM](#msp2_inav_osd_set_layout_item) +[8212 - MSP2_INAV_OSD_ALARMS](#msp2_inav_osd_alarms) +[8213 - MSP2_INAV_OSD_SET_ALARMS](#msp2_inav_osd_set_alarms) +[8214 - MSP2_INAV_OSD_PREFERENCES](#msp2_inav_osd_preferences) +[8215 - MSP2_INAV_OSD_SET_PREFERENCES](#msp2_inav_osd_set_preferences) +[8216 - MSP2_INAV_SELECT_BATTERY_PROFILE](#msp2_inav_select_battery_profile) +[8217 - MSP2_INAV_DEBUG](#msp2_inav_debug) +[8218 - MSP2_BLACKBOX_CONFIG](#msp2_blackbox_config) +[8219 - MSP2_SET_BLACKBOX_CONFIG](#msp2_set_blackbox_config) +[8220 - MSP2_INAV_TEMP_SENSOR_CONFIG](#msp2_inav_temp_sensor_config) +[8221 - MSP2_INAV_SET_TEMP_SENSOR_CONFIG](#msp2_inav_set_temp_sensor_config) +[8222 - MSP2_INAV_TEMPERATURES](#msp2_inav_temperatures) +[8223 - MSP_SIMULATOR](#msp_simulator) +[8224 - MSP2_INAV_SERVO_MIXER](#msp2_inav_servo_mixer) +[8225 - MSP2_INAV_SET_SERVO_MIXER](#msp2_inav_set_servo_mixer) +[8226 - MSP2_INAV_LOGIC_CONDITIONS](#msp2_inav_logic_conditions) +[8227 - MSP2_INAV_SET_LOGIC_CONDITIONS](#msp2_inav_set_logic_conditions) +[8228 - MSP2_INAV_GLOBAL_FUNCTIONS](#msp2_inav_global_functions) +[8229 - MSP2_INAV_SET_GLOBAL_FUNCTIONS](#msp2_inav_set_global_functions) +[8230 - MSP2_INAV_LOGIC_CONDITIONS_STATUS](#msp2_inav_logic_conditions_status) +[8231 - MSP2_INAV_GVAR_STATUS](#msp2_inav_gvar_status) +[8232 - MSP2_INAV_PROGRAMMING_PID](#msp2_inav_programming_pid) +[8233 - MSP2_INAV_SET_PROGRAMMING_PID](#msp2_inav_set_programming_pid) +[8234 - MSP2_INAV_PROGRAMMING_PID_STATUS](#msp2_inav_programming_pid_status) +[8240 - MSP2_PID](#msp2_pid) +[8241 - MSP2_SET_PID](#msp2_set_pid) +[8242 - MSP2_INAV_OPFLOW_CALIBRATION](#msp2_inav_opflow_calibration) +[8243 - MSP2_INAV_FWUPDT_PREPARE](#msp2_inav_fwupdt_prepare) +[8244 - MSP2_INAV_FWUPDT_STORE](#msp2_inav_fwupdt_store) +[8245 - MSP2_INAV_FWUPDT_EXEC](#msp2_inav_fwupdt_exec) +[8246 - MSP2_INAV_FWUPDT_ROLLBACK_PREPARE](#msp2_inav_fwupdt_rollback_prepare) +[8247 - MSP2_INAV_FWUPDT_ROLLBACK_EXEC](#msp2_inav_fwupdt_rollback_exec) +[8248 - MSP2_INAV_SAFEHOME](#msp2_inav_safehome) +[8249 - MSP2_INAV_SET_SAFEHOME](#msp2_inav_set_safehome) +[8250 - MSP2_INAV_MISC2](#msp2_inav_misc2) +[8251 - MSP2_INAV_LOGIC_CONDITIONS_SINGLE](#msp2_inav_logic_conditions_single) +[8256 - MSP2_INAV_ESC_RPM](#msp2_inav_esc_rpm) +[8257 - MSP2_INAV_ESC_TELEM](#msp2_inav_esc_telem) +[8264 - MSP2_INAV_LED_STRIP_CONFIG_EX](#msp2_inav_led_strip_config_ex) +[8265 - MSP2_INAV_SET_LED_STRIP_CONFIG_EX](#msp2_inav_set_led_strip_config_ex) +[8266 - MSP2_INAV_FW_APPROACH](#msp2_inav_fw_approach) +[8267 - MSP2_INAV_SET_FW_APPROACH](#msp2_inav_set_fw_approach) +[8272 - MSP2_INAV_GPS_UBLOX_COMMAND](#msp2_inav_gps_ublox_command) +[8288 - MSP2_INAV_RATE_DYNAMICS](#msp2_inav_rate_dynamics) +[8289 - MSP2_INAV_SET_RATE_DYNAMICS](#msp2_inav_set_rate_dynamics) +[8304 - MSP2_INAV_EZ_TUNE](#msp2_inav_ez_tune) +[8305 - MSP2_INAV_EZ_TUNE_SET](#msp2_inav_ez_tune_set) +[8320 - MSP2_INAV_SELECT_MIXER_PROFILE](#msp2_inav_select_mixer_profile) +[8336 - MSP2_ADSB_VEHICLE_LIST](#msp2_adsb_vehicle_list) +[8448 - MSP2_INAV_CUSTOM_OSD_ELEMENTS](#msp2_inav_custom_osd_elements) +[8449 - MSP2_INAV_CUSTOM_OSD_ELEMENT](#msp2_inav_custom_osd_element) +[8450 - MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS](#msp2_inav_set_custom_osd_elements) +[8461 - MSP2_INAV_OUTPUT_MAPPING_EXT2](#msp2_inav_output_mapping_ext2) +[8704 - MSP2_INAV_SERVO_CONFIG](#msp2_inav_servo_config) +[8705 - MSP2_INAV_SET_SERVO_CONFIG](#msp2_inav_set_servo_config) +[8720 - MSP2_INAV_GEOZONE](#msp2_inav_geozone) +[8721 - MSP2_INAV_SET_GEOZONE](#msp2_inav_set_geozone) +[8722 - MSP2_INAV_GEOZONE_VERTEX](#msp2_inav_geozone_vertex) +[8723 - MSP2_INAV_SET_GEOZONE_VERTEX](#msp2_inav_set_geozone_vertex) +[8724 - MSP2_INAV_SET_GVAR](#msp2_inav_set_gvar) +[8736 - MSP2_INAV_FULL_LOCAL_POSE](#msp2_inav_full_local_pose) +[12288 - MSP2_BETAFLIGHT_BIND](#msp2_betaflight_bind) + +## `MSP_API_VERSION (1 / 0x1)` +**Description:** Provides the MSP protocol version and the INAV API version. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `mspProtocolVersion` | `uint8_t` | 1 | MSP Protocol version (`MSP_PROTOCOL_VERSION`, typically 0) | +| `apiVersionMajor` | `uint8_t` | 1 | INAV API Major version (`API_VERSION_MAJOR`) | +| `apiVersionMinor` | `uint8_t` | 1 | INAV API Minor version (`API_VERSION_MINOR`) | + +**Notes:** Used by configurators to check compatibility. + +## `MSP_FC_VARIANT (2 / 0x2)` +**Description:** Identifies the flight controller firmware variant (e.g., INAV, Betaflight). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `fcVariantIdentifier` | `char[4]` | 4 | 4-character identifier string (e.g., "INAV"). Defined by `flightControllerIdentifier`. | + +**Notes:** See `FLIGHT_CONTROLLER_IDENTIFIER_LENGTH`. + +## `MSP_FC_VERSION (3 / 0x3)` +**Description:** Provides the specific version number of the flight controller firmware. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `fcVersionMajor` | `uint8_t` | 1 | Firmware Major version (`FC_VERSION_MAJOR`) | +| `fcVersionMinor` | `uint8_t` | 1 | Firmware Minor version (`FC_VERSION_MINOR`) | +| `fcVersionPatch` | `uint8_t` | 1 | Firmware Patch level (`FC_VERSION_PATCH_LEVEL`) | + +## `MSP_BOARD_INFO (4 / 0x4)` +**Description:** Provides information about the specific hardware board and its capabilities. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `boardIdentifier` | `char[4]` | 4 | - | 4-character UPPER CASE board identifier (`TARGET_BOARD_IDENTIFIER`) | +| `hardwareRevision` | `uint16_t` | 2 | - | Hardware revision number. 0 if not detected (`USE_HARDWARE_REVISION_DETECTION`) | +| `osdSupport` | `uint8_t` | 1 | - | OSD chip type: 0=None, 2=Onboard (`USE_OSD`). INAV does not support slave OSD (1) | +| `commCapabilities` | `uint8_t` | 1 | Bitmask | Bitmask: Communication capabilities: Bit 0=VCP support (`USE_VCP`), Bit 1=SoftSerial support (`USE_SOFTSERIAL1`/`2`) | +| `targetNameLength` | `uint8_t` | 1 | - | Length of the target name string that follows | +| `targetName` | `char[]` | array | - | Target name string (e.g., "MATEKF405"). Length given by previous field | + +**Notes:** `BOARD_IDENTIFIER_LENGTH` is 4. + +## `MSP_BUILD_INFO (5 / 0x5)` +**Description:** Provides build date, time, and Git revision of the firmware. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `buildDate` | `char[BUILD_DATE_LENGTH]` | 11 (BUILD_DATE_LENGTH) | Build date string (e.g., "Dec 31 2023"). `BUILD_DATE_LENGTH`. | +| `buildTime` | `char[BUILD_TIME_LENGTH]` | 8 (BUILD_TIME_LENGTH) | Build time string (e.g., "23:59:59"). `BUILD_TIME_LENGTH`. | +| `gitRevision` | `char[GIT_SHORT_REVISION_LENGTH]` | 8 (GIT_SHORT_REVISION_LENGTH) | Short Git revision string. `GIT_SHORT_REVISION_LENGTH`. | + +## `MSP_INAV_PID (6 / 0x6)` +**Description:** Retrieves legacy INAV-specific PID controller related settings. Many fields are now obsolete or placeholders. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `legacyAsyncProcessing` | `uint8_t` | 1 | - | Legacy, unused. Always 0 | +| `legacyAsyncValue1` | `uint16_t` | 2 | - | Legacy, unused. Always 0 | +| `legacyAsyncValue2` | `int16_t` | 2 | - | Legacy, unused. Always 0 | +| `headingHoldRateLimit` | `uint8_t` | 1 | deg/s | Max rate for heading hold P term (`pidProfile()->heading_hold_rate_limit`) | +| `headingHoldLpfFreq` | `uint8_t` | 1 | Hz | Fixed LPF frequency for heading hold error (`HEADING_HOLD_ERROR_LPF_FREQ`) | +| `legacyYawJumpLimit` | `int16_t` | 2 | - | Legacy, unused. Always 0 | +| `legacyGyroLpf` | `uint8_t` | 1 | Hz | Fixed value `GYRO_LPF_256HZ` | +| `accLpfHz` | `uint8_t` | 1 | Hz | Accelerometer LPF frequency (`accelerometerConfig()->acc_lpf_hz`) cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz | +| `reserved1` | `uint8_t` | 1 | - | Reserved. Always 0 | +| `reserved2` | `uint8_t` | 1 | - | Reserved. Always 0 | +| `reserved3` | `uint8_t` | 1 | - | Reserved. Always 0 | +| `reserved4` | `uint8_t` | 1 | - | Reserved. Always 0 | + +**Notes:** Superseded by `MSP2_PID` for core PIDs and other specific messages for filter settings. + +## `MSP_SET_INAV_PID (7 / 0x7)` +**Description:** Sets legacy INAV-specific PID controller related settings. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `legacyAsyncProcessing` | `uint8_t` | 1 | - | Legacy, ignored | +| `legacyAsyncValue1` | `int16_t` | 2 | - | Legacy, ignored | +| `legacyAsyncValue2` | `int16_t` | 2 | - | Legacy, ignored | +| `headingHoldRateLimit` | `uint8_t` | 1 | deg/s | Sets `pidProfileMutable()->heading_hold_rate_limit`. | +| `headingHoldLpfFreq` | `uint8_t` | 1 | Hz | Ignored (fixed value `HEADING_HOLD_ERROR_LPF_FREQ` used) | +| `legacyYawJumpLimit` | `int16_t` | 2 | - | Legacy, ignored | +| `legacyGyroLpf` | `uint8_t` | 1 | - | Ignored (historically mapped to `gyro_lpf_e` values). | +| `accLpfHz` | `uint8_t` | 1 | Hz | Sets `accelerometerConfigMutable()->acc_lpf_hz`. | +| `reserved1` | `uint8_t` | 1 | - | Ignored | +| `reserved2` | `uint8_t` | 1 | - | Ignored | +| `reserved3` | `uint8_t` | 1 | - | Ignored | +| `reserved4` | `uint8_t` | 1 | - | Ignored | + +**Reply Payload:** **None** + +**Notes:** Expects 15 bytes. + +## `MSP_NAME (10 / 0xa)` +**Description:** Returns the user-defined craft name. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `craftName` | `char[]` | array | The craft name string (`systemConfig()->craftName`). Null termination is *not* explicitly sent, the length is determined by the payload size | + +## `MSP_SET_NAME (11 / 0xb)` +**Description:** Sets the user-defined craft name. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `craftName` | `char[]` | array | The new craft name string. Automatically null-terminated by the FC | + +**Reply Payload:** **None** + +**Notes:** Maximum length is `MAX_NAME_LENGTH`. + +## `MSP_NAV_POSHOLD (12 / 0xc)` +**Description:** Retrieves navigation position hold and general manual/auto flight parameters. Some parameters depend on the platform type (Multirotor vs Fixed Wing). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `userControlMode` | `uint8_t` | 1 | - | Navigation user control mode NAV_GPS_ATTI (0) or NAV_GPS_CRUISE (1) | +| `maxAutoSpeed` | `uint16_t` | 2 | cm/s | Max speed in autonomous modes (`navConfig()->general.max_auto_speed`) | +| `maxAutoClimbRate` | `uint16_t` | 2 | cm/s | Max climb rate in autonomous modes (uses `fw.max_auto_climb_rate` or `mc.max_auto_climb_rate` based on platform) | +| `maxManualSpeed` | `uint16_t` | 2 | cm/s | Max speed in manual modes with GPS aiding (`navConfig()->general.max_manual_speed`) | +| `maxManualClimbRate` | `uint16_t` | 2 | cm/s | Max climb rate in manual modes with GPS aiding (uses `fw.max_manual_climb_rate` or `mc.max_manual_climb_rate`) | +| `mcMaxBankAngle` | `uint8_t` | 1 | degrees | Max bank angle for multirotor position hold (`navConfig()->mc.max_bank_angle`) | +| `mcAltHoldThrottleType` | `uint8_t` | 1 | [navMcAltHoldThrottle_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-navmcaltholdthrottle_e) | Enum `navMcAltHoldThrottle_e` mirrored from `navConfig()->mc.althold_throttle_type`. | +| `mcHoverThrottle` | `uint16_t` | 2 | PWM | Multirotor hover throttle PWM value (`currentBatteryProfile->nav.mc.hover_throttle`). | + +## `MSP_SET_NAV_POSHOLD (13 / 0xd)` +**Description:** Sets navigation position hold and general manual/auto flight parameters. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `userControlMode` | `uint8_t` | 1 | [navUserControlMode_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-navusercontrolmode_e) | Sets `navConfigMutable()->general.flags.user_control_mode`. WARNING: uses unnamed enum in navigation.h 'NAV_GPS_ATTI/NAV_GPS_CRUISE' | +| `maxAutoSpeed` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->general.max_auto_speed`. | +| `maxAutoClimbRate` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->fw.max_auto_climb_rate` or `navConfigMutable()->mc.max_auto_climb_rate` based on `mixerConfig()->platformType`. | +| `maxManualSpeed` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->general.max_manual_speed`. | +| `maxManualClimbRate` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->fw.max_manual_climb_rate` or `navConfigMutable()->mc.max_manual_climb_rate`. | +| `mcMaxBankAngle` | `uint8_t` | 1 | degrees | Sets `navConfigMutable()->mc.max_bank_angle`. | +| `mcAltHoldThrottleType` | `uint8_t` | 1 | [navMcAltHoldThrottle_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-navmcaltholdthrottle_e) | Enum `navMcAltHoldThrottle_e`; updates `navConfigMutable()->mc.althold_throttle_type`. | +| `mcHoverThrottle` | `uint16_t` | 2 | PWM | Sets `currentBatteryProfileMutable->nav.mc.hover_throttle`. | + +**Reply Payload:** **None** + +**Notes:** Expects 13 bytes. + +## `MSP_CALIBRATION_DATA (14 / 0xe)` +**Description:** Retrieves sensor calibration data (Accelerometer zero/gain, Magnetometer zero/gain, Optical Flow scale). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `accCalibAxisFlags` | `uint8_t` | 1 | Bitmask | Bitmask: Flags indicating which axes of the accelerometer have been calibrated (`accGetCalibrationAxisFlags()`) | +| `accZeroX` | `int16_t` | 2 | Raw ADC | Accelerometer zero offset for X-axis (`accelerometerConfig()->accZero.raw[X]`) | +| `accZeroY` | `int16_t` | 2 | Raw ADC | Accelerometer zero offset for Y-axis (`accelerometerConfig()->accZero.raw[Y]`) | +| `accZeroZ` | `int16_t` | 2 | Raw ADC | Accelerometer zero offset for Z-axis (`accelerometerConfig()->accZero.raw[Z]`) | +| `accGainX` | `int16_t` | 2 | Raw ADC | Accelerometer gain/scale for X-axis (`accelerometerConfig()->accGain.raw[X]`) | +| `accGainY` | `int16_t` | 2 | Raw ADC | Accelerometer gain/scale for Y-axis (`accelerometerConfig()->accGain.raw[Y]`) | +| `accGainZ` | `int16_t` | 2 | Raw ADC | Accelerometer gain/scale for Z-axis (`accelerometerConfig()->accGain.raw[Z]`) | +| `magZeroX` | `int16_t` | 2 | Raw ADC | Magnetometer zero offset for X-axis (`compassConfig()->magZero.raw[X]`). 0 if `USE_MAG` disabled | +| `magZeroY` | `int16_t` | 2 | Raw ADC | Magnetometer zero offset for Y-axis (`compassConfig()->magZero.raw[Y]`). 0 if `USE_MAG` disabled | +| `magZeroZ` | `int16_t` | 2 | Raw ADC | Magnetometer zero offset for Z-axis (`compassConfig()->magZero.raw[Z]`). 0 if `USE_MAG` disabled | +| `opflowScale` | `uint16_t` | 2 | Scale * 256 | Optical flow scale factor (`opticalFlowConfig()->opflow_scale * 256`). 0 if `USE_OPFLOW` disabled | +| `magGainX` | `int16_t` | 2 | Raw ADC | Magnetometer gain/scale for X-axis (`compassConfig()->magGain[X]`). 0 if `USE_MAG` disabled | +| `magGainY` | `int16_t` | 2 | Raw ADC | Magnetometer gain/scale for Y-axis (`compassConfig()->magGain[Y]`). 0 if `USE_MAG` disabled | +| `magGainZ` | `int16_t` | 2 | Raw ADC | Magnetometer gain/scale for Z-axis (`compassConfig()->magGain[Z]`). 0 if `USE_MAG` disabled | + +**Notes:** Total size 27 bytes. Fields related to optional sensors are zero if the sensor is not used. + +## `MSP_SET_CALIBRATION_DATA (15 / 0xf)` +**Description:** Sets sensor calibration data. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `accZeroX` | `int16_t` | 2 | Raw ADC | Sets `accelerometerConfigMutable()->accZero.raw[X]`. | +| `accZeroY` | `int16_t` | 2 | Raw ADC | Sets `accelerometerConfigMutable()->accZero.raw[Y]`. | +| `accZeroZ` | `int16_t` | 2 | Raw ADC | Sets `accelerometerConfigMutable()->accZero.raw[Z]`. | +| `accGainX` | `int16_t` | 2 | Raw ADC | Sets `accelerometerConfigMutable()->accGain.raw[X]`. | +| `accGainY` | `int16_t` | 2 | Raw ADC | Sets `accelerometerConfigMutable()->accGain.raw[Y]`. | +| `accGainZ` | `int16_t` | 2 | Raw ADC | Sets `accelerometerConfigMutable()->accGain.raw[Z]`. | +| `magZeroX` | `int16_t` | 2 | Raw ADC | Sets `compassConfigMutable()->magZero.raw[X]` (if `USE_MAG`) | +| `magZeroY` | `int16_t` | 2 | Raw ADC | Sets `compassConfigMutable()->magZero.raw[Y]` (if `USE_MAG`) | +| `magZeroZ` | `int16_t` | 2 | Raw ADC | Sets `compassConfigMutable()->magZero.raw[Z]` (if `USE_MAG`) | +| `opflowScale` | `uint16_t` | 2 | Scale * 256 | Sets `opticalFlowConfigMutable()->opflow_scale = value / 256.0f` (if `USE_OPFLOW`) | +| `magGainX` | `int16_t` | 2 | Raw ADC | Sets `compassConfigMutable()->magGain[X]` (if `USE_MAG`) | +| `magGainY` | `int16_t` | 2 | Raw ADC | Sets `compassConfigMutable()->magGain[Y]` (if `USE_MAG`) | +| `magGainZ` | `int16_t` | 2 | Raw ADC | Sets `compassConfigMutable()->magGain[Z]` (if `USE_MAG`) | + +**Reply Payload:** **None** + +**Notes:** Minimum payload 18 bytes. Adds +6 bytes for magnetometer zeros, +2 for optical flow scale, and +6 for magnetometer gains when those features (`USE_MAG`, `USE_OPFLOW`) are compiled in. + +## `MSP_POSITION_ESTIMATION_CONFIG (16 / 0x10)` +**Description:** Retrieves parameters related to the INAV position estimation fusion weights and GPS minimum satellite count. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `weightZBaroP` | `uint16_t` | 2 | Weight * 100 | Barometer Z position fusion weight (`positionEstimationConfig()->w_z_baro_p * 100`) | +| `weightZGPSP` | `uint16_t` | 2 | Weight * 100 | GPS Z position fusion weight (`positionEstimationConfig()->w_z_gps_p * 100`) | +| `weightZGPSV` | `uint16_t` | 2 | Weight * 100 | GPS Z velocity fusion weight (`positionEstimationConfig()->w_z_gps_v * 100`) | +| `weightXYGPSP` | `uint16_t` | 2 | Weight * 100 | GPS XY position fusion weight (`positionEstimationConfig()->w_xy_gps_p * 100`) | +| `weightXYGPSV` | `uint16_t` | 2 | Weight * 100 | GPS XY velocity fusion weight (`positionEstimationConfig()->w_xy_gps_v * 100`) | +| `minSats` | `uint8_t` | 1 | Count | Minimum satellites required for GPS use (`gpsConfigMutable()->gpsMinSats`) | +| `useGPSVelNED` | `uint8_t` | 1 | Boolean | Legacy flag, always 1 (GPS velocity is always used if available) | + +## `MSP_SET_POSITION_ESTIMATION_CONFIG (17 / 0x11)` +**Description:** Sets parameters related to the INAV position estimation fusion weights and GPS minimum satellite count. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `weightZBaroP` | `uint16_t` | 2 | Weight * 100 | Sets `positionEstimationConfigMutable()->w_z_baro_p = value / 100.0f` (constrained 0.0-10.0) | +| `weightZGPSP` | `uint16_t` | 2 | Weight * 100 | Sets `positionEstimationConfigMutable()->w_z_gps_p = value / 100.0f` (constrained 0.0-10.0) | +| `weightZGPSV` | `uint16_t` | 2 | Weight * 100 | Sets `positionEstimationConfigMutable()->w_z_gps_v = value / 100.0f` (constrained 0.0-10.0) | +| `weightXYGPSP` | `uint16_t` | 2 | Weight * 100 | Sets `positionEstimationConfigMutable()->w_xy_gps_p = value / 100.0f` (constrained 0.0-10.0) | +| `weightXYGPSV` | `uint16_t` | 2 | Weight * 100 | Sets `positionEstimationConfigMutable()->w_xy_gps_v = value / 100.0f` (constrained 0.0-10.0) | +| `minSats` | `uint8_t` | 1 | Count | Sets `gpsConfigMutable()->gpsMinSats` (constrained 5-10) | +| `useGPSVelNED` | `uint8_t` | 1 | Boolean | Legacy flag, ignored | + +**Reply Payload:** **None** + +**Notes:** Expects 12 bytes. + +## `MSP_WP_MISSION_LOAD (18 / 0x12)` +**Description:** Commands the FC to load the waypoint mission stored in non-volatile memory (e.g., EEPROM or FlashFS) into the active mission buffer. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `missionID` | `uint8_t` | 1 | Reserved for future use, currently ignored | + +**Reply Payload:** **None** + +**Notes:** Only functional if `NAV_NON_VOLATILE_WAYPOINT_STORAGE` is defined. Requires 1 byte payload. Returns error if loading fails. + +## `MSP_WP_MISSION_SAVE (19 / 0x13)` +**Description:** Commands the FC to save the currently active waypoint mission from RAM to non-volatile memory (e.g., EEPROM or FlashFS). + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `missionID` | `uint8_t` | 1 | Reserved for future use, currently ignored | + +**Reply Payload:** **None** + +**Notes:** Only functional if `NAV_NON_VOLATILE_WAYPOINT_STORAGE` is defined. Requires 1 byte payload. Returns error if saving fails. + +## `MSP_WP_GETINFO (20 / 0x14)` +**Description:** Retrieves information about the waypoint mission capabilities and the status of the currently loaded mission. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `wpCapabilities` | `uint8_t` | 1 | Reserved for future waypoint capabilities flags. Currently always 0 | +| `maxWaypoints` | `uint8_t` | 1 | Maximum number of waypoints supported (`NAV_MAX_WAYPOINTS`) | +| `missionValid` | `uint8_t` | 1 | Boolean flag indicating if the current mission in RAM is valid (`isWaypointListValid()`) | +| `waypointCount` | `uint8_t` | 1 | Number of waypoints currently defined in the mission (`getWaypointCount()`) | + +## `MSP_RTH_AND_LAND_CONFIG (21 / 0x15)` +**Description:** Retrieves configuration parameters related to Return-to-Home (RTH) and automatic landing behaviors. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `minRthDistance` | `uint16_t` | 2 | cm | Minimum distance from home required for RTH to engage (`navConfig()->general.min_rth_distance`) | +| `rthClimbFirst` | `uint8_t` | 1 | Boolean | Flag: Climb to RTH altitude before returning (`navConfig()->general.flags.rth_climb_first`) | +| `rthClimbIgnoreEmerg` | `uint8_t` | 1 | Boolean | Flag: Climb even in emergency RTH (`navConfig()->general.flags.rth_climb_ignore_emerg`) | +| `rthTailFirst` | `uint8_t` | 1 | Boolean | Flag: Return tail-first during RTH (`navConfig()->general.flags.rth_tail_first`) | +| `rthAllowLanding` | `uint8_t` | 1 | Boolean | Flag: Allow automatic landing after RTH (`navConfig()->general.flags.rth_allow_landing`) | +| `rthAltControlMode` | `uint8_t` | 1 | [navRthAltControlMode_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-navrthaltcontrolmode_e) | RTH altitude control mode (`navConfig()->general.flags.rth_alt_control_mode`). WARNING: uses unnamed enum in navigation.h:253 'NAV_RTH_NO_ALT...' | +| `rthAbortThreshold` | `uint16_t` | 2 | cm | Distance increase threshold to abort RTH (`navConfig()->general.rth_abort_threshold`) | +| `rthAltitude` | `uint16_t` | 2 | cm | Target RTH altitude (`navConfig()->general.rth_altitude`) | +| `landMinAltVspd` | `uint16_t` | 2 | cm/s | Landing vertical speed at minimum slowdown altitude (`navConfig()->general.land_minalt_vspd`) | +| `landMaxAltVspd` | `uint16_t` | 2 | cm/s | Landing vertical speed at maximum slowdown altitude (`navConfig()->general.land_maxalt_vspd`) | +| `landSlowdownMinAlt` | `uint16_t` | 2 | cm | Altitude below which `landMinAltVspd` applies (`navConfig()->general.land_slowdown_minalt`) | +| `landSlowdownMaxAlt` | `uint16_t` | 2 | cm | Altitude above which `landMaxAltVspd` applies (`navConfig()->general.land_slowdown_maxalt`) | +| `emergDescentRate` | `uint16_t` | 2 | cm/s | Vertical speed during emergency landing descent (`navConfig()->general.emerg_descent_rate`) | + +## `MSP_SET_RTH_AND_LAND_CONFIG (22 / 0x16)` +**Description:** Sets configuration parameters related to Return-to-Home (RTH) and automatic landing behaviors. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `minRthDistance` | `uint16_t` | 2 | cm | Sets `navConfigMutable()->general.min_rth_distance`. | +| `rthClimbFirst` | `uint8_t` | 1 | Boolean | Sets `navConfigMutable()->general.flags.rth_climb_first`. | +| `rthClimbIgnoreEmerg` | `uint8_t` | 1 | Boolean | Sets `navConfigMutable()->general.flags.rth_climb_ignore_emerg`. | +| `rthTailFirst` | `uint8_t` | 1 | Boolean | Sets `navConfigMutable()->general.flags.rth_tail_first`. | +| `rthAllowLanding` | `uint8_t` | 1 | Boolean | Sets `navConfigMutable()->general.flags.rth_allow_landing`. | +| `rthAltControlMode` | `uint8_t` | 1 | [navRthAltControlMode_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-navrthaltcontrolmode_e) | Sets `navConfigMutable()->general.flags.rth_alt_control_mode`. WARNING: uses unnamed enum in navigation.h:253 | +| `rthAbortThreshold` | `uint16_t` | 2 | cm | Sets `navConfigMutable()->general.rth_abort_threshold`. | +| `rthAltitude` | `uint16_t` | 2 | cm | Sets `navConfigMutable()->general.rth_altitude`. | +| `landMinAltVspd` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->general.land_minalt_vspd`. | +| `landMaxAltVspd` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->general.land_maxalt_vspd`. | +| `landSlowdownMinAlt` | `uint16_t` | 2 | cm | Sets `navConfigMutable()->general.land_slowdown_minalt`. | +| `landSlowdownMaxAlt` | `uint16_t` | 2 | cm | Sets `navConfigMutable()->general.land_slowdown_maxalt`. | +| `emergDescentRate` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->general.emerg_descent_rate`. | + +**Reply Payload:** **None** + +**Notes:** Expects 21 bytes. + +## `MSP_FW_CONFIG (23 / 0x17)` +**Description:** Retrieves configuration parameters specific to Fixed Wing navigation. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `cruiseThrottle` | `uint16_t` | 2 | PWM | Cruise throttle command (`currentBatteryProfile->nav.fw.cruise_throttle`). | +| `minThrottle` | `uint16_t` | 2 | PWM | Minimum throttle during autonomous flight (`currentBatteryProfile->nav.fw.min_throttle`). | +| `maxThrottle` | `uint16_t` | 2 | PWM | Maximum throttle during autonomous flight (`currentBatteryProfile->nav.fw.max_throttle`). | +| `maxBankAngle` | `uint8_t` | 1 | degrees | Maximum bank angle allowed (`navConfig()->fw.max_bank_angle`) | +| `maxClimbAngle` | `uint8_t` | 1 | degrees | Maximum pitch angle during climb (`navConfig()->fw.max_climb_angle`) | +| `maxDiveAngle` | `uint8_t` | 1 | degrees | Maximum negative pitch angle during descent (`navConfig()->fw.max_dive_angle`) | +| `pitchToThrottle` | `uint8_t` | 1 | us/deg | Pitch-to-throttle gain (`currentBatteryProfile->nav.fw.pitch_to_throttle`); PWM microseconds per degree (10 units ≈ 1% throttle). | +| `loiterRadius` | `uint16_t` | 2 | cm | Default loiter radius (`navConfig()->fw.loiter_radius`). | + +## `MSP_SET_FW_CONFIG (24 / 0x18)` +**Description:** Sets configuration parameters specific to Fixed Wing navigation. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `cruiseThrottle` | `uint16_t` | 2 | PWM | Sets `currentBatteryProfileMutable->nav.fw.cruise_throttle`. | +| `minThrottle` | `uint16_t` | 2 | PWM | Sets `currentBatteryProfileMutable->nav.fw.min_throttle`. | +| `maxThrottle` | `uint16_t` | 2 | PWM | Sets `currentBatteryProfileMutable->nav.fw.max_throttle`. | +| `maxBankAngle` | `uint8_t` | 1 | degrees | Sets `navConfigMutable()->fw.max_bank_angle`. | +| `maxClimbAngle` | `uint8_t` | 1 | degrees | Sets `navConfigMutable()->fw.max_climb_angle`. | +| `maxDiveAngle` | `uint8_t` | 1 | degrees | Sets `navConfigMutable()->fw.max_dive_angle`. | +| `pitchToThrottle` | `uint8_t` | 1 | us/deg | Sets `currentBatteryProfileMutable->nav.fw.pitch_to_throttle` (PWM microseconds per degree; 10 units ≈ 1% throttle). | +| `loiterRadius` | `uint16_t` | 2 | cm | Sets `navConfigMutable()->fw.loiter_radius`. | + +**Reply Payload:** **None** + +**Notes:** Expects 12 bytes. + +## `MSP_MODE_RANGES (34 / 0x22)` +**Description:** Returns all defined mode activation ranges (aux channel assignments for flight modes). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `modePermanentId` | `uint8_t` | 1 | ID | Permanent ID of the flight mode (maps to `boxId` via `findBoxByActiveBoxId`). 0 if entry unused | +| `auxChannelIndex` | `uint8_t` | 1 | Index | 0-based index of the AUX channel used for activation | +| `rangeStartStep` | `uint8_t` | 1 | step | Start step (0-48). Each step is 25 PWM units; 0 is <=900 and 48 is >=2100. | +| `rangeEndStep` | `uint8_t` | 1 | step | End step (0-48). Uses the same 25-PWM step mapping as rangeStartStep. | + +**Notes:** The number of steps and mapping to PWM values depends on internal range calculations. + +## `MSP_SET_MODE_RANGE (35 / 0x23)` +**Description:** Sets a single mode activation range by its index. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `rangeIndex` | `uint8_t` | 1 | Index | Index of the mode range to set (0 to `MAX_MODE_ACTIVATION_CONDITION_COUNT - 1`) | +| `modePermanentId` | `uint8_t` | 1 | ID | Permanent ID of the flight mode to assign | +| `auxChannelIndex` | `uint8_t` | 1 | Index | 0-based index of the AUX channel | +| `rangeStartStep` | `uint8_t` | 1 | step | Start step (0-48). Each step is 25 PWM units; 0 is <=900 and 48 is >=2100. | +| `rangeEndStep` | `uint8_t` | 1 | step | End step (0-48). Uses the same 25-PWM step mapping as rangeStartStep. | + +**Reply Payload:** **None** + +**Notes:** Expects 5 bytes. Updates the mode configuration and recalculates used mode flags. Returns error if `rangeIndex` or `modePermanentId` is invalid. + +## `MSP_FEATURE (36 / 0x24)` +**Description:** Returns a bitmask of enabled features. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `featureMask` | `uint32_t` | 4 | Bitmask | Bitmask: active features (see `featureMask()`) | + +**Notes:** Feature bits are defined in `feature.h`. + +## `MSP_SET_FEATURE (37 / 0x25)` +**Description:** Sets the enabled features using a bitmask. Clears all previous features first. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `featureMask` | `uint32_t` | 4 | Bitmask | Bitmask: features to enable | + +**Reply Payload:** **None** + +**Notes:** Expects 4 bytes. Updates feature configuration and related settings (e.g., RSSI source). + +## `MSP_BOARD_ALIGNMENT (38 / 0x26)` +**Description:** Returns the sensor board alignment angles relative to the craft frame. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `rollAlign` | `int16_t` | 2 | deci-degrees | Board alignment roll angle (`boardAlignment()->rollDeciDegrees`). Negative values tilt left. | +| `pitchAlign` | `int16_t` | 2 | deci-degrees | Board alignment pitch angle (`boardAlignment()->pitchDeciDegrees`). Negative values nose down. | +| `yawAlign` | `int16_t` | 2 | deci-degrees | Board alignment yaw angle (`boardAlignment()->yawDeciDegrees`). Negative values rotate counter-clockwise. | + +**Notes:** Ranges are typically -1800 to +1800 (i.e. -180.0° to +180.0°). + +## `MSP_SET_BOARD_ALIGNMENT (39 / 0x27)` +**Description:** Sets the sensor board alignment angles. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `rollAlign` | `int16_t` | 2 | deci-degrees | Sets `boardAlignmentMutable()->rollDeciDegrees`. | +| `pitchAlign` | `int16_t` | 2 | deci-degrees | Sets `boardAlignmentMutable()->pitchDeciDegrees`. | +| `yawAlign` | `int16_t` | 2 | deci-degrees | Sets `boardAlignmentMutable()->yawDeciDegrees`. | + +**Reply Payload:** **None** + +**Notes:** Expects 6 bytes encoded as little-endian signed deci-degrees (-1800 to +1800 typical). + +## `MSP_CURRENT_METER_CONFIG (40 / 0x28)` +**Description:** Retrieves the configuration for the current sensor. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `scale` | `int16_t` | 2 | 0.1 mV/A | Current sensor scale factor (`batteryMetersConfig()->current.scale`). Stored in 0.1 mV/A; signed for calibration. | +| `offset` | `int16_t` | 2 | mV | Current sensor offset (`batteryMetersConfig()->current.offset`). Signed millivolt adjustment. | +| `type` | `uint8_t` | 1 | [currentSensor_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-currentsensor_e) | Enum `currentSensor_e` Type of current sensor hardware | +| `capacity` | `uint16_t` | 2 | mAh (legacy) | Battery capacity (constrained 0-65535) (`currentBatteryProfile->capacity.value`). Note: This is legacy, use `MSP2_INAV_BATTERY_CONFIG` for full 32-bit capacity | + +**Notes:** Scale and offset are signed values matching `batteryMetersConfig()->current` fields. + +## `MSP_SET_CURRENT_METER_CONFIG (41 / 0x29)` +**Description:** Sets the configuration for the current sensor. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `scale` | `int16_t` | 2 | 0.1 mV/A | Sets `batteryMetersConfigMutable()->current.scale` (0.1 mV/A, signed). | +| `offset` | `int16_t` | 2 | mV | Sets `batteryMetersConfigMutable()->current.offset` (signed millivolts). | +| `type` | `uint8_t` | 1 | [currentSensor_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-currentsensor_e) | Enum `currentSensor_e` Sets `batteryMetersConfigMutable()->current.type`. | +| `capacity` | `uint16_t` | 2 | mAh (legacy) | Sets `currentBatteryProfileMutable->capacity.value` (truncated to 16 bits) | + +**Reply Payload:** **None** + +**Notes:** Expects 7 bytes. Signed values use little-endian two's complement. + +## `MSP_MIXER (42 / 0x2a)` +**Description:** Retrieves the mixer type (Legacy, INAV always returns QuadX). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `mixerMode` | `uint8_t` | 1 | Always 3 (QuadX) in INAV for compatibility | + +**Notes:** This command is largely obsolete. Mixer configuration is handled differently in INAV (presets, custom mixes). See `MSP2_INAV_MIXER`. + +## `MSP_SET_MIXER (43 / 0x2b)` +**Description:** Sets the mixer type (Legacy, ignored by INAV). + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `mixerMode` | `uint8_t` | 1 | Mixer mode to set (ignored by INAV) | + +**Reply Payload:** **None** + +**Notes:** Expects 1 byte. Calls `mixerUpdateStateFlags()` for potential side effects related to presets. + +## `MSP_RX_CONFIG (44 / 0x2c)` +**Description:** Retrieves receiver configuration settings. Some fields are Betaflight compatibility placeholders. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `serialRxProvider` | `uint8_t` | 1 | [rxSerialReceiverType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-rxserialreceivertype_e) | Enum `rxSerialReceiverType_e`. Serial RX provider (`rxConfig()->serialrx_provider`). | +| `maxCheck` | `uint16_t` | 2 | PWM | Upper channel value threshold for stick commands (`rxConfig()->maxcheck`) | +| `midRc` | `uint16_t` | 2 | PWM | Center channel value (`PWM_RANGE_MIDDLE`, typically 1500) | +| `minCheck` | `uint16_t` | 2 | PWM | Lower channel value threshold for stick commands (`rxConfig()->mincheck`) | +| `spektrumSatBind` | `uint8_t` | 1 | Count/Flag | Spektrum bind pulses (`rxConfig()->spektrum_sat_bind`). 0 if `USE_SPEKTRUM_BIND` disabled. | +| `rxMinUsec` | `uint16_t` | 2 | PWM | Minimum expected pulse width (`rxConfig()->rx_min_usec`) | +| `rxMaxUsec` | `uint16_t` | 2 | PWM | Maximum expected pulse width (`rxConfig()->rx_max_usec`) | +| `bfCompatRcInterpolation` | `uint8_t` | 1 | - | BF compatibility. Always 0 | +| `bfCompatRcInterpolationInt` | `uint8_t` | 1 | - | BF compatibility. Always 0 | +| `bfCompatAirModeThreshold` | `uint16_t` | 2 | - | BF compatibility. Always 0 | +| `reserved1` | `uint8_t` | 1 | - | Reserved/Padding. Always 0 | +| `reserved2` | `uint32_t` | 4 | - | Reserved/Padding. Always 0 | +| `reserved3` | `uint8_t` | 1 | - | Reserved/Padding. Always 0 | +| `bfCompatFpvCamAngle` | `uint8_t` | 1 | - | BF compatibility. Always 0 | +| `receiverType` | `uint8_t` | 1 | [rxReceiverType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-rxreceivertype_e) | Enum `rxReceiverType_e` Receiver type (Parallel PWM, PPM, Serial) ('rxConfig()->receiverType') | + +## `MSP_SET_RX_CONFIG (45 / 0x2d)` +**Description:** Sets receiver configuration settings. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `serialRxProvider` | `uint8_t` | 1 | [rxSerialReceiverType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-rxserialreceivertype_e) | Enum `rxSerialReceiverType_e`. Sets `rxConfigMutable()->serialrx_provider`. | +| `maxCheck` | `uint16_t` | 2 | PWM | Sets `rxConfigMutable()->maxcheck`. | +| `midRc` | `uint16_t` | 2 | PWM | Ignored (`PWM_RANGE_MIDDLE` is used) | +| `minCheck` | `uint16_t` | 2 | PWM | Sets `rxConfigMutable()->mincheck`. | +| `spektrumSatBind` | `uint8_t` | 1 | Count/Flag | Sets `rxConfigMutable()->spektrum_sat_bind` (if `USE_SPEKTRUM_BIND`). | +| `rxMinUsec` | `uint16_t` | 2 | PWM | Sets `rxConfigMutable()->rx_min_usec`. | +| `rxMaxUsec` | `uint16_t` | 2 | PWM | Sets `rxConfigMutable()->rx_max_usec`. | +| `bfCompatRcInterpolation` | `uint8_t` | 1 | - | Ignored | +| `bfCompatRcInterpolationInt` | `uint8_t` | 1 | - | Ignored | +| `bfCompatAirModeThreshold` | `uint16_t` | 2 | - | Ignored | +| `reserved1` | `uint8_t` | 1 | - | Ignored | +| `reserved2` | `uint32_t` | 4 | - | Ignored | +| `reserved3` | `uint8_t` | 1 | - | Ignored | +| `bfCompatFpvCamAngle` | `uint8_t` | 1 | - | Ignored | +| `receiverType` | `uint8_t` | 1 | [rxReceiverType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-rxreceivertype_e) | Enum `rxReceiverType_e` Sets `rxConfigMutable()->receiverType`. | + +**Reply Payload:** **None** + +**Notes:** Expects 24 bytes. + +## `MSP_LED_COLORS (46 / 0x2e)` +**Description:** Retrieves the HSV color definitions for configurable LED colors. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `hue` | `uint16_t` | 2 | Hue value (0-359) | +| `saturation` | `uint8_t` | 1 | Saturation value (0-255) | +| `value` | `uint8_t` | 1 | Value/Brightness (0-255) | + +**Notes:** Only available if `USE_LED_STRIP` is defined. + +## `MSP_SET_LED_COLORS (47 / 0x2f)` +**Description:** Sets the HSV color definitions for configurable LED colors. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `hue` | `uint16_t` | 2 | Hue value (0-359) | +| `saturation` | `uint8_t` | 1 | Saturation value (0-255) | +| `value` | `uint8_t` | 1 | Value/Brightness (0-255) | + +**Reply Payload:** **None** + +**Notes:** Only available if `USE_LED_STRIP` is defined. Expects `LED_CONFIGURABLE_COLOR_COUNT * 4` bytes. + +## `MSP_LED_STRIP_CONFIG (48 / 0x30)` +**Description:** Retrieves the configuration for each LED on the strip (legacy packed format). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `legacyLedConfig` | `uint32_t` | 4 | Packed LED configuration (position, function, overlay, color, direction, params). See C code for bit packing details | + +**Notes:** Only available if `USE_LED_STRIP` is defined. Superseded by `MSP2_INAV_LED_STRIP_CONFIG_EX` which uses a clearer struct. + +## `MSP_SET_LED_STRIP_CONFIG (49 / 0x31)` +**Description:** Sets the configuration for a single LED on the strip using the legacy packed format. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `ledIndex` | `uint8_t` | 1 | Index of the LED to configure (0 to `LED_MAX_STRIP_LENGTH - 1`) | +| `legacyLedConfig` | `uint32_t` | 4 | Packed LED configuration to set | + +**Reply Payload:** **None** + +**Notes:** Only available if `USE_LED_STRIP` is defined. Expects 5 bytes. Calls `reevaluateLedConfig()`. Superseded by `MSP2_INAV_SET_LED_STRIP_CONFIG_EX`. + +## `MSP_RSSI_CONFIG (50 / 0x32)` +**Description:** Retrieves the channel used for analog RSSI input. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `rssiChannel` | `uint8_t` | 1 | AUX channel index (1-based) used for RSSI, or 0 if disabled (`rxConfig()->rssi_channel`) | + +## `MSP_SET_RSSI_CONFIG (51 / 0x33)` +**Description:** Sets the channel used for analog RSSI input. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `rssiChannel` | `uint8_t` | 1 | AUX channel index (1-based) to use for RSSI, or 0 to disable | + +**Reply Payload:** **None** + +**Notes:** Expects 1 byte. Input value is constrained 0 to `MAX_SUPPORTED_RC_CHANNEL_COUNT`. Updates the effective RSSI source. + +## `MSP_ADJUSTMENT_RANGES (52 / 0x34)` +**Description:** Returns all defined RC adjustment ranges (tuning via aux channels). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `adjustmentIndex` | `uint8_t` | 1 | - | Index of the adjustment slot (0 to `MAX_SIMULTANEOUS_ADJUSTMENT_COUNT - 1`) | +| `auxChannelIndex` | `uint8_t` | 1 | - | 0-based index of the AUX channel controlling the adjustment value | +| `rangeStartStep` | `uint8_t` | 1 | step | Start step (0-48). Each step is 25 PWM units; 0 is <=900 and 48 is >=2100. | +| `rangeEndStep` | `uint8_t` | 1 | step | End step (0-48). Uses the same 25-PWM step mapping as rangeStartStep. | +| `adjustmentFunction` | `uint8_t` | 1 | [adjustmentFunction_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-adjustmentfunction_e) | Function/parameter being adjusted (see `adjustmentFunction_e`). | +| `auxSwitchChannelIndex` | `uint8_t` | 1 | - | 0-based index of the AUX channel acting as an enable switch (or 0 if always enabled) | + +**Notes:** See `adjustmentRange_t`. + +## `MSP_SET_ADJUSTMENT_RANGE (53 / 0x35)` +**Description:** Sets a single RC adjustment range configuration by its index. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `rangeIndex` | `uint8_t` | 1 | - | Index of the adjustment range to set (0 to `MAX_ADJUSTMENT_RANGE_COUNT - 1`) | +| `adjustmentIndex` | `uint8_t` | 1 | - | Adjustment slot index (0 to `MAX_SIMULTANEOUS_ADJUSTMENT_COUNT - 1`) | +| `auxChannelIndex` | `uint8_t` | 1 | - | 0-based index of the control AUX channel | +| `rangeStartStep` | `uint8_t` | 1 | step | Start step (0-48). Each step is 25 PWM units; 0 is <=900 and 48 is >=2100. | +| `rangeEndStep` | `uint8_t` | 1 | step | End step (0-48). Uses the same 25-PWM step mapping as rangeStartStep. | +| `adjustmentFunction` | `uint8_t` | 1 | [adjustmentFunction_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-adjustmentfunction_e) | Function/parameter being adjusted. | +| `auxSwitchChannelIndex` | `uint8_t` | 1 | - | 0-based index of the enable switch AUX channel (or 0) | + +**Reply Payload:** **None** + +**Notes:** Expects 7 bytes. Returns error if `rangeIndex` or `adjustmentIndex` is invalid. + +## `MSP_CF_SERIAL_CONFIG (54 / 0x36)` +**Description:** Deprecated command to get serial port configuration. + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Not implemented in INAV `fc_msp.c`. Use `MSP2_COMMON_SERIAL_CONFIG`. + +## `MSP_SET_CF_SERIAL_CONFIG (55 / 0x37)` +**Description:** Deprecated command to set serial port configuration. + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Not implemented in INAV `fc_msp.c`. Use `MSP2_COMMON_SET_SERIAL_CONFIG`. + +## `MSP_VOLTAGE_METER_CONFIG (56 / 0x38)` +**Description:** Retrieves legacy voltage meter configuration (scaled values). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `vbatScale` | `uint8_t` | 1 | Scale / 10 | Voltage sensor scale factor / 10 (`batteryMetersConfig()->voltage.scale / 10`). 0 if `USE_ADC` disabled | +| `vbatMinCell` | `uint8_t` | 1 | 0.1V | Minimum cell voltage / 10 (`currentBatteryProfile->voltage.cellMin / 10`). 0 if `USE_ADC` disabled | +| `vbatMaxCell` | `uint8_t` | 1 | 0.1V | Maximum cell voltage / 10 (`currentBatteryProfile->voltage.cellMax / 10`). 0 if `USE_ADC` disabled | +| `vbatWarningCell` | `uint8_t` | 1 | 0.1V | Warning cell voltage / 10 (`currentBatteryProfile->voltage.cellWarning / 10`). 0 if `USE_ADC` disabled | + +**Notes:** Superseded by `MSP2_INAV_BATTERY_CONFIG`. + +## `MSP_SET_VOLTAGE_METER_CONFIG (57 / 0x39)` +**Description:** Sets legacy voltage meter configuration (scaled values). + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `vbatScale` | `uint8_t` | 1 | Scale / 10 | Sets `batteryMetersConfigMutable()->voltage.scale = value * 10` (if `USE_ADC`) | +| `vbatMinCell` | `uint8_t` | 1 | 0.1V | Sets `currentBatteryProfileMutable->voltage.cellMin = value * 10` (if `USE_ADC`) | +| `vbatMaxCell` | `uint8_t` | 1 | 0.1V | Sets `currentBatteryProfileMutable->voltage.cellMax = value * 10` (if `USE_ADC`) | +| `vbatWarningCell` | `uint8_t` | 1 | 0.1V | Sets `currentBatteryProfileMutable->voltage.cellWarning = value * 10` (if `USE_ADC`) | + +**Reply Payload:** **None** + +**Notes:** Expects 4 bytes. Superseded by `MSP2_INAV_SET_BATTERY_CONFIG`. + +## `MSP_SONAR_ALTITUDE (58 / 0x3a)` +**Description:** Retrieves the altitude measured by the primary rangefinder (sonar or lidar). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `rangefinderAltitude` | `int32_t` | 4 | cm | Latest altitude reading from the rangefinder (`rangefinderGetLatestAltitude()`). 0 if `USE_RANGEFINDER` disabled or no reading. | + +## `MSP_RX_MAP (64 / 0x40)` +**Description:** Retrieves the RC channel mapping array (AETR, etc.). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `rcMap` | `uint8_t[MAX_MAPPABLE_RX_INPUTS]` | 4 (MAX_MAPPABLE_RX_INPUTS) | Array defining the mapping from input channel index to logical function (Roll, Pitch, Yaw, Throttle, Aux1...) | + +**Notes:** `MAX_MAPPABLE_RX_INPUTS` is currently 4 (Roll, Pitch, Yaw, Throttle). + +## `MSP_SET_RX_MAP (65 / 0x41)` +**Description:** Sets the RC channel mapping array. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `rcMap` | `uint8_t[MAX_MAPPABLE_RX_INPUTS]` | 4 (MAX_MAPPABLE_RX_INPUTS) | Array defining the new channel mapping | + +**Reply Payload:** **None** + +**Notes:** Expects `MAX_MAPPABLE_RX_INPUTS` bytes (currently 4). + +## `MSP_REBOOT (68 / 0x44)` +**Description:** Commands the flight controller to reboot. + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** The FC sends an ACK *before* rebooting. The `mspPostProcessFn` is set to `mspRebootFn` to perform the reboot after the reply is sent. Will fail if the craft is armed. + +## `MSP_DATAFLASH_SUMMARY (70 / 0x46)` +**Description:** Retrieves summary information about the onboard dataflash chip (if present and used for Blackbox via FlashFS). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `flashReady` | `uint8_t` | 1 | Boolean: 1 if flash chip is ready, 0 otherwise. (`flashIsReady()`). 0 if `USE_FLASHFS` disabled | +| `sectorCount` | `uint32_t` | 4 | Total number of sectors on the flash chip (`geometry->sectors`). 0 if `USE_FLASHFS` disabled | +| `totalSize` | `uint32_t` | 4 | Total size of the flash chip in bytes (`geometry->totalSize`). 0 if `USE_FLASHFS` disabled | +| `usedSize` | `uint32_t` | 4 | Currently used size in bytes (FlashFS offset) (`flashfsGetOffset()`). 0 if `USE_FLASHFS` disabled | + +**Notes:** Requires `USE_FLASHFS`. + +## `MSP_DATAFLASH_READ (71 / 0x47)` +**Description:** Reads a block of data from the onboard dataflash (FlashFS). + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `address` | `uint32_t` | 4 | Starting address to read from within the FlashFS volume | +| `size` | `uint16_t` | 2 | (Optional) Number of bytes to read. Defaults to 128 if not provided | + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `address` | `uint32_t` | 4 | The starting address from which data was actually read | +| `data` | `uint8_t[]` | array | The data read from flash. Length is MIN(requested size, remaining buffer space, remaining flashfs data) | + +**Notes:** Requires `USE_FLASHFS`. Read length may be truncated by buffer size or end of flashfs volume. + +## `MSP_DATAFLASH_ERASE (72 / 0x48)` +**Description:** Erases the entire onboard dataflash chip (FlashFS volume). + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Requires `USE_FLASHFS`. This is a potentially long operation. Use with caution. + +## `MSP_LOOP_TIME (73 / 0x49)` +**Description:** Retrieves the configured loop time (PID loop frequency denominator). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `looptime` | `uint16_t` | 2 | PWM | Configured loop time (`gyroConfig()->looptime`) | + +**Notes:** This is the *configured* target loop time, not necessarily the *actual* measured cycle time (see `MSP_STATUS`). + +## `MSP_SET_LOOP_TIME (74 / 0x4a)` +**Description:** Sets the configured loop time. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `looptime` | `uint16_t` | 2 | PWM | New loop time to set (`gyroConfigMutable()->looptime`) | + +**Reply Payload:** **None** + +**Notes:** Expects 2 bytes. + +## `MSP_FAILSAFE_CONFIG (75 / 0x4b)` +**Description:** Retrieves the failsafe configuration settings. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `failsafeDelay` | `uint8_t` | 1 | 0.1s | Delay before failsafe stage 1 activates (`failsafeConfig()->failsafe_delay`) | +| `failsafeOffDelay` | `uint8_t` | 1 | 0.1s | Delay after signal recovery before returning control (`failsafeConfig()->failsafe_off_delay`) | +| `failsafeThrottle` | `uint16_t` | 2 | PWM | Throttle level during failsafe stage 2 (`currentBatteryProfile->failsafe_throttle`) | +| `legacyKillSwitch` | `uint8_t` | 1 | - | Legacy flag, always 0 | +| `failsafeThrottleLowDelay` | `uint16_t` | 2 | 0.1s | Delay for throttle-based failsafe detection (`failsafeConfig()->failsafe_throttle_low_delay`). Units of 0.1 seconds. | +| `failsafeProcedure` | `uint8_t` | 1 | [failsafeProcedure_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-failsafeprocedure_e) | Enum `failsafeProcedure_e` Failsafe procedure (Drop, RTH, Land, etc.) ('failsafeConfig()->failsafe_procedure') | +| `failsafeRecoveryDelay` | `uint8_t` | 1 | 0.1s | Delay after RTH finishes before attempting recovery (`failsafeConfig()->failsafe_recovery_delay`) | +| `failsafeFWRollAngle` | `int16_t` | 2 | deci-degrees | Fixed-wing failsafe roll angle (`failsafeConfig()->failsafe_fw_roll_angle`). Signed deci-degrees. | +| `failsafeFWPitchAngle` | `int16_t` | 2 | deci-degrees | Fixed-wing failsafe pitch angle (`failsafeConfig()->failsafe_fw_pitch_angle`). Signed deci-degrees. | +| `failsafeFWYawRate` | `int16_t` | 2 | deg/s | Fixed-wing failsafe yaw rate (`failsafeConfig()->failsafe_fw_yaw_rate`). Signed degrees per second. | +| `failsafeStickThreshold` | `uint16_t` | 2 | PWM units | Stick movement threshold to exit failsafe (`failsafeConfig()->failsafe_stick_motion_threshold`) | +| `failsafeMinDistance` | `uint16_t` | 2 | cm | Minimum distance from home for RTH failsafe (`failsafeConfig()->failsafe_min_distance`). Units of centimeters. | +| `failsafeMinDistanceProc` | `uint8_t` | 1 | [failsafeProcedure_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-failsafeprocedure_e) | Enum `failsafeProcedure_e` Failsafe procedure if below min distance ('failsafeConfig()->failsafe_min_distance_procedure') | + +## `MSP_SET_FAILSAFE_CONFIG (76 / 0x4c)` +**Description:** Sets the failsafe configuration settings. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `failsafeDelay` | `uint8_t` | 1 | 0.1s | Sets `failsafeConfigMutable()->failsafe_delay`. | +| `failsafeOffDelay` | `uint8_t` | 1 | 0.1s | Sets `failsafeConfigMutable()->failsafe_off_delay`. | +| `failsafeThrottle` | `uint16_t` | 2 | PWM | Sets `currentBatteryProfileMutable->failsafe_throttle`. | +| `legacyKillSwitch` | `uint8_t` | 1 | - | Ignored | +| `failsafeThrottleLowDelay` | `uint16_t` | 2 | 0.1s | Sets `failsafeConfigMutable()->failsafe_throttle_low_delay`. Units of 0.1 seconds. | +| `failsafeProcedure` | `uint8_t` | 1 | [failsafeProcedure_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-failsafeprocedure_e) | Enum `failsafeProcedure_e`. Sets `failsafeConfigMutable()->failsafe_procedure`. | +| `failsafeRecoveryDelay` | `uint8_t` | 1 | 0.1s | Sets `failsafeConfigMutable()->failsafe_recovery_delay`. | +| `failsafeFWRollAngle` | `int16_t` | 2 | deci-degrees | Sets `failsafeConfigMutable()->failsafe_fw_roll_angle`. Signed deci-degrees. | +| `failsafeFWPitchAngle` | `int16_t` | 2 | deci-degrees | Sets `failsafeConfigMutable()->failsafe_fw_pitch_angle`. Signed deci-degrees. | +| `failsafeFWYawRate` | `int16_t` | 2 | deg/s | Sets `failsafeConfigMutable()->failsafe_fw_yaw_rate`. Signed degrees per second. | +| `failsafeStickThreshold` | `uint16_t` | 2 | PWM units | Sets `failsafeConfigMutable()->failsafe_stick_motion_threshold`. | +| `failsafeMinDistance` | `uint16_t` | 2 | cm | Sets `failsafeConfigMutable()->failsafe_min_distance`. Units of centimeters. | +| `failsafeMinDistanceProc` | `uint8_t` | 1 | [failsafeProcedure_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-failsafeprocedure_e) | Enum `failsafeProcedure_e`. Sets `failsafeConfigMutable()->failsafe_min_distance_procedure`. | + +**Reply Payload:** **None** + +**Notes:** Expects 20 bytes. + +## `MSP_SDCARD_SUMMARY (79 / 0x4f)` +**Description:** Retrieves summary information about the SD card status and filesystem. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `sdCardSupported` | `uint8_t` | 1 | Bitmask | Bitmask: Bit 0 = 1 if SD card support compiled in (`USE_SDCARD`) | +| `sdCardState` | `uint8_t` | 1 | [mspSDCardState_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-mspsdcardstate_e) | Enum (`mspSDCardState_e`): Current state (Not Present, Fatal, Card Init, FS Init, Ready). 0 if `USE_SDCARD` disabled | +| `fsError` | `uint8_t` | 1 | - | Last filesystem error code (`afatfs_getLastError()`). 0 if `USE_SDCARD` disabled | +| `freeSpaceKB` | `uint32_t` | 4 | - | Free space in KiB (`afatfs_getContiguousFreeSpace() / 1024`). 0 if `USE_SDCARD` disabled | +| `totalSpaceKB` | `uint32_t` | 4 | - | Total space in KiB (`sdcard_getMetadata()->numBlocks / 2`). 0 if `USE_SDCARD` disabled | + +**Notes:** Requires `USE_SDCARD` and `USE_ASYNCFATFS`. + +## `MSP_BLACKBOX_CONFIG (80 / 0x50)` +**Description:** Legacy command to retrieve Blackbox configuration. Superseded by `MSP2_BLACKBOX_CONFIG`. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `blackboxDevice` | `uint8_t` | 1 | Always 0 (API no longer supported) | +| `blackboxRateNum` | `uint8_t` | 1 | Always 0 | +| `blackboxRateDenom` | `uint8_t` | 1 | Always 0 | +| `blackboxPDenom` | `uint8_t` | 1 | Always 0 | + +**Notes:** Returns fixed zero values. Use `MSP2_BLACKBOX_CONFIG`. + +## `MSP_SET_BLACKBOX_CONFIG (81 / 0x51)` +**Description:** Legacy command to set Blackbox configuration. Superseded by `MSP2_SET_BLACKBOX_CONFIG`. + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Not implemented in `fc_msp.c`. Use `MSP2_SET_BLACKBOX_CONFIG`. + +## `MSP_TRANSPONDER_CONFIG (82 / 0x52)` +**Description:** Get VTX Transponder settings (likely specific to RaceFlight/Betaflight, not standard INAV VTX). + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Not implemented in INAV `fc_msp.c`. + +## `MSP_SET_TRANSPONDER_CONFIG (83 / 0x53)` +**Description:** Set VTX Transponder settings. + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Not implemented in INAV `fc_msp.c`. + +## `MSP_OSD_CONFIG (84 / 0x54)` +**Description:** Retrieves OSD configuration settings and layout for screen 0. Coordinates are packed as `(Y << 8) | X`. When `USE_OSD` is not compiled in, only `osdDriverType` = `OSD_DRIVER_NONE` is returned. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `osdDriverType` | `uint8_t` | 1 | [osdDriver_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-osddriver_e) | Enum `osdDriver_e`: `OSD_DRIVER_MAX7456` if `USE_OSD`, else `OSD_DRIVER_NONE`. | +| `videoSystem` | `uint8_t` | 1 | [videoSystem_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-videosystem_e) | Enum `videoSystem_e`: Video system (Auto/PAL/NTSC) (`osdConfig()->video_system`). Sent even if OSD disabled | +| `units` | `uint8_t` | 1 | [osd_unit_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-osd_unit_e) | Enum `osd_unit_e` Measurement units (Metric/Imperial) (`osdConfig()->units`). Sent even if OSD disabled | +| `rssiAlarm` | `uint8_t` | 1 | % | RSSI alarm threshold (`osdConfig()->rssi_alarm`). Sent even if OSD disabled | +| `capAlarm` | `uint16_t` | 2 | mAh/mWh | Capacity alarm threshold (`currentBatteryProfile->capacity.warning`). Truncated to 16 bits. Sent even if OSD disabled. | +| `timerAlarm` | `uint16_t` | 2 | minutes | Timer alarm threshold in minutes (`osdConfig()->time_alarm`). Sent even if OSD disabled. | +| `altAlarm` | `uint16_t` | 2 | meters | Altitude alarm threshold (`osdConfig()->alt_alarm`). Sent even if OSD disabled | +| `distAlarm` | `uint16_t` | 2 | meters | Distance alarm threshold (`osdConfig()->dist_alarm`). Sent even if OSD disabled | +| `negAltAlarm` | `uint16_t` | 2 | meters | Negative altitude alarm threshold (`osdConfig()->neg_alt_alarm`). Sent even if OSD disabled | +| `itemPositions` | `uint16_t[OSD_ITEM_COUNT]` | OSD_ITEM_COUNT | packed | Packed X/Y position for each OSD item on screen 0 (`osdLayoutsConfig()->item_pos[0][i]`). Sent even if OSD disabled | + +**Notes:** 1 byte if `USE_OSD` disabled; full payload (1 + fields + 2*OSD_ITEM_COUNT bytes) otherwise. + +## `MSP_SET_OSD_CONFIG (85 / 0x55)` +**Description:** Sets OSD configuration or a single item's position on screen 0. +#### Variant: `dataSize >= 10` + +**Description:** dataSize >= 10 + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `selector` | `uint8_t` | 1 | - | Must be 0xFF (-1) to indicate a configuration update. | +| `videoSystem` | `uint8_t` | 1 | [videoSystem_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-videosystem_e) | Enum `videoSystem_e`: Video system (Auto/PAL/NTSC) (`osdConfig()->video_system`). | +| `units` | `uint8_t` | 1 | [osd_unit_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-osd_unit_e) | Enum `osd_unit_e` Measurement units (Metric/Imperial) (`osdConfig()->units`). | +| `rssiAlarm` | `uint8_t` | 1 | % | RSSI alarm threshold (`osdConfig()->rssi_alarm`). | +| `capAlarm` | `uint16_t` | 2 | mAh/mWh | Capacity alarm threshold (`currentBatteryProfile->capacity.warning`). Truncated to 16 bits. | +| `timerAlarm` | `uint16_t` | 2 | minutes | Timer alarm threshold in minutes (`osdConfig()->time_alarm`). | +| `altAlarm` | `uint16_t` | 2 | meters | Altitude alarm threshold (`osdConfig()->alt_alarm`). | +| `distAlarm` | `uint16_t` | 2 | meters | Distance alarm threshold (`osdConfig()->dist_alarm`). Optional trailing field. | +| `negAltAlarm` | `uint16_t` | 2 | meters | Negative altitude alarm threshold (`osdConfig()->neg_alt_alarm`). Optional trailing field. | + +**Reply Payload:** **None** + +#### Variant: `dataSize == 3` + +**Description:** Single item position update + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `itemIndex` | `uint8_t` | 1 | Index | Index of the OSD item to update (0 to `OSD_ITEM_COUNT - 1`). | +| `itemPosition` | `uint16_t` | 2 | packed | Packed X/Y position (`(Y << 8) | X`) for the specified item. | + +**Reply Payload:** **None** + + +**Notes:** Requires `USE_OSD`. Distinguishes formats based on the first byte. Format 1 requires at least 10 bytes. Format 2 requires 3 bytes. Triggers an OSD redraw. See `MSP2_INAV_OSD_SET_*` for more advanced control. + +## `MSP_OSD_CHAR_READ (86 / 0x56)` +**Description:** Reads character data from the OSD font memory. + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Not implemented in INAV `fc_msp.c`. Requires direct hardware access, typically done via DisplayPort. + +## `MSP_OSD_CHAR_WRITE (87 / 0x57)` +**Description:** Writes character data to the OSD font memory. +#### Variant: `payloadSize >= OSD_CHAR_BYTES + 2 (>=66 bytes)` + +**Description:** 16-bit character index with full 64-byte payload (visible + metadata). + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `address` | `uint16_t` | 2 | Character slot index (0-1023). | +| `charData` | `uint8_t[OSD_CHAR_BYTES]` | 64 (OSD_CHAR_BYTES) | All 64 bytes, including driver metadata. | + +**Reply Payload:** **None** + +#### Variant: `payloadSize == OSD_CHAR_BYTES + 1 (65 bytes)` + +**Description:** 8-bit character index with full 64-byte payload. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `address` | `uint8_t` | 1 | Character slot index (0-255). | +| `charData` | `uint8_t[OSD_CHAR_BYTES]` | 64 (OSD_CHAR_BYTES) | All 64 bytes, including driver metadata. | + +**Reply Payload:** **None** + +#### Variant: `payloadSize == OSD_CHAR_VISIBLE_BYTES + 2 (56 bytes)` + +**Description:** 16-bit character index with only the 54 visible bytes. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `address` | `uint16_t` | 2 | Character slot index (0-1023). | +| `charData` | `uint8_t[OSD_CHAR_VISIBLE_BYTES]` | 54 (OSD_CHAR_VISIBLE_BYTES) | Visible pixel data only (no metadata). | + +**Reply Payload:** **None** + +#### Variant: `payloadSize == OSD_CHAR_VISIBLE_BYTES + 1 (55 bytes)` + +**Description:** 8-bit character index with only the 54 visible bytes. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `address` | `uint8_t` | 1 | Character slot index (0-255). | +| `charData` | `uint8_t[OSD_CHAR_VISIBLE_BYTES]` | 54 (OSD_CHAR_VISIBLE_BYTES) | Visible pixel data only (no metadata). | + +**Reply Payload:** **None** + + +**Notes:** Requires `USE_OSD`. Minimum payload is `OSD_CHAR_VISIBLE_BYTES + 1` (8-bit address + 54 bytes). Payload size determines the address width and whether the extra metadata bytes are present. Writes characters via `displayWriteFontCharacter()`. + +## `MSP_VTX_CONFIG (88 / 0x58)` +**Description:** Retrieves the current VTX (Video Transmitter) configuration and capabilities. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `vtxDeviceType` | `uint8_t` | 1 | [vtxDevType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-vtxdevtype_e) | Enum (`vtxDevType_e`): Type of VTX device detected/configured. `VTXDEV_UNKNOWN` if none | +| `band` | `uint8_t` | 1 | - | VTX band number (from `vtxSettingsConfig`) | +| `channel` | `uint8_t` | 1 | - | VTX channel number (from `vtxSettingsConfig`) | +| `power` | `uint8_t` | 1 | - | VTX power level index (from `vtxSettingsConfig()`). | +| `pitMode` | `uint8_t` | 1 | - | Boolean: 1 if VTX is currently in pit mode, 0 otherwise. | +| `vtxReady` | `uint8_t` | 1 | - | Boolean: 1 if VTX device reported ready, 0 otherwise | +| `lowPowerDisarm` | `uint8_t` | 1 | [vtxLowerPowerDisarm_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-vtxlowerpowerdisarm_e) | Enum `vtxLowerPowerDisarm_e`: Low-power behaviour while disarmed (`vtxSettingsConfig()->lowPowerDisarm`). | +| `vtxTableAvailable` | `uint8_t` | 1 | - | Boolean: 1 if VTX tables (band/power) are available for query | +| `bandCount` | `uint8_t` | 1 | - | Number of bands supported by the VTX device | +| `channelCount` | `uint8_t` | 1 | - | Number of channels per band supported by the VTX device | +| `powerCount` | `uint8_t` | 1 | - | Number of power levels supported by the VTX device | + +**Notes:** Returns 1 byte (`VTXDEV_UNKNOWN`) when no VTX is detected or `USE_VTX_CONTROL` is disabled; otherwise sends full payload. BF compatibility field `frequency` (uint16) is missing compared to some BF versions. Use `MSP_VTXTABLE_BAND` and `MSP_VTXTABLE_POWERLEVEL` for details. + +## `MSP_SET_VTX_CONFIG (89 / 0x59)` +**Description:** Sets VTX band/channel and related options. Fields are a progressive superset based on payload length. +#### Variant: `payloadSize >= 14` + +**Description:** Full payload (Betaflight 1.42+): includes explicit band/channel/frequency and capability counts. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `bandChanOrFreq` | `uint16_t` | 2 | - | Encoded band/channel if <= `VTXCOMMON_MSP_BANDCHAN_CHKVAL`; otherwise frequency placeholder. | +| `power` | `uint8_t` | 1 | - | | +| `pitMode` | `uint8_t` | 1 | - | | +| `lowPowerDisarm` | `uint8_t` | 1 | [vtxLowerPowerDisarm_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-vtxlowerpowerdisarm_e) | | +| `pitModeFreq` | `uint16_t` | 2 | - | | +| `band` | `uint8_t` | 1 | - | | +| `channel` | `uint8_t` | 1 | - | | +| `frequency` | `uint16_t` | 2 | - | | +| `bandCount` | `uint8_t` | 1 | - | Read and ignored. | +| `channelCount` | `uint8_t` | 1 | - | Read and ignored. | +| `powerCount` | `uint8_t` | 1 | - | If 0 < value < current capability, caps `vtxDevice->capability.powerCount`. | + +**Reply Payload:** **None** + +#### Variant: `payloadSize >= 11` + +**Description:** Extends payload with explicit frequency. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `bandChanOrFreq` | `uint16_t` | 2 | - | | +| `power` | `uint8_t` | 1 | - | | +| `pitMode` | `uint8_t` | 1 | - | | +| `lowPowerDisarm` | `uint8_t` | 1 | [vtxLowerPowerDisarm_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-vtxlowerpowerdisarm_e) | | +| `pitModeFreq` | `uint16_t` | 2 | - | | +| `band` | `uint8_t` | 1 | - | | +| `channel` | `uint8_t` | 1 | - | | +| `frequency` | `uint16_t` | 2 | - | Read and ignored by INAV. | + +**Reply Payload:** **None** + +#### Variant: `payloadSize >= 9` + +**Description:** Adds explicit band/channel overrides (API 1.42 extension). + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `bandChanOrFreq` | `uint16_t` | 2 | - | | +| `power` | `uint8_t` | 1 | - | | +| `pitMode` | `uint8_t` | 1 | - | | +| `lowPowerDisarm` | `uint8_t` | 1 | [vtxLowerPowerDisarm_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-vtxlowerpowerdisarm_e) | | +| `pitModeFreq` | `uint16_t` | 2 | - | | +| `band` | `uint8_t` | 1 | - | 1..N; overrides band when present. | +| `channel` | `uint8_t` | 1 | - | 1..8; overrides channel when present. | + +**Reply Payload:** **None** + +#### Variant: `payloadSize >= 7` + +**Description:** Adds pit-mode frequency placeholder. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `bandChanOrFreq` | `uint16_t` | 2 | - | | +| `power` | `uint8_t` | 1 | - | | +| `pitMode` | `uint8_t` | 1 | - | | +| `lowPowerDisarm` | `uint8_t` | 1 | [vtxLowerPowerDisarm_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-vtxlowerpowerdisarm_e) | | +| `pitModeFreq` | `uint16_t` | 2 | - | Read and skipped. | + +**Reply Payload:** **None** + +#### Variant: `payloadSize >= 5` + +**Description:** Adds low-power disarm behaviour. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `bandChanOrFreq` | `uint16_t` | 2 | - | | +| `power` | `uint8_t` | 1 | - | | +| `pitMode` | `uint8_t` | 1 | - | | +| `lowPowerDisarm` | `uint8_t` | 1 | [vtxLowerPowerDisarm_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-vtxlowerpowerdisarm_e) | 0=Off, 1=Always, 2=Until first arm. | + +**Reply Payload:** **None** + +#### Variant: `payloadSize >= 4` + +**Description:** Adds power index and pit mode flag. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `bandChanOrFreq` | `uint16_t` | 2 | | +| `power` | `uint8_t` | 1 | | +| `pitMode` | `uint8_t` | 1 | | + +**Reply Payload:** **None** + +#### Variant: `payloadSize == 2` + +**Description:** Minimum payload (band/channel encoded in 0..63). + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `bandChanOrFreq` | `uint16_t` | 2 | If <= `VTXCOMMON_MSP_BANDCHAN_CHKVAL`, decoded as band/channel; otherwise treated as a frequency placeholder. | + +**Reply Payload:** **None** + + +**Notes:** Requires dataSize >= 2. If no VTX device or device type is VTXDEV_UNKNOWN, fields are read and discarded. The first uint16 is interpreted as band/channel when value <= VTXCOMMON_MSP_BANDCHAN_CHKVAL, otherwise treated as a frequency value that is not applied by this path. Subsequent fields are applied only if present. If dataSize < 2 the command returns MSP_RESULT_ERROR. + +## `MSP_ADVANCED_CONFIG (90 / 0x5a)` +**Description:** Retrieves advanced hardware-related configuration (PWM protocols, rates). Some fields are BF compatibility placeholders. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `gyroSyncDenom` | `uint8_t` | 1 | - | Always 1 (BF compatibility) | +| `pidProcessDenom` | `uint8_t` | 1 | - | Always 1 (BF compatibility) | +| `useUnsyncedPwm` | `uint8_t` | 1 | - | Always 1 (BF compatibility, INAV uses async PWM based on protocol) | +| `motorPwmProtocol` | `uint8_t` | 1 | [motorPwmProtocolTypes_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-motorpwmprotocoltypes_e) | Motor PWM protocol type (`motorConfig()->motorPwmProtocol`). | +| `motorPwmRate` | `uint16_t` | 2 | Hz | Motor PWM rate (if applicable) (`motorConfig()->motorPwmRate`). | +| `servoPwmRate` | `uint16_t` | 2 | Hz | Servo PWM rate (`servoConfig()->servoPwmRate`). | +| `legacyGyroSync` | `uint8_t` | 1 | - | Always 0 (BF compatibility) | + +## `MSP_SET_ADVANCED_CONFIG (91 / 0x5b)` +**Description:** Sets advanced hardware-related configuration (PWM protocols, rates). + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `gyroSyncDenom` | `uint8_t` | 1 | - | Ignored (legacy Betaflight field). | +| `pidProcessDenom` | `uint8_t` | 1 | - | Ignored (legacy Betaflight field). | +| `useUnsyncedPwm` | `uint8_t` | 1 | - | Ignored (legacy Betaflight field). | +| `motorPwmProtocol` | `uint8_t` | 1 | [motorPwmProtocolTypes_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-motorpwmprotocoltypes_e) | Sets `motorConfigMutable()->motorPwmProtocol`. | +| `motorPwmRate` | `uint16_t` | 2 | Hz | Sets `motorConfigMutable()->motorPwmRate`. | +| `servoPwmRate` | `uint16_t` | 2 | Hz | Sets `servoConfigMutable()->servoPwmRate`. | +| `legacyGyroSync` | `uint8_t` | 1 | - | Ignored (legacy Betaflight field). | + +**Reply Payload:** **None** + +**Notes:** Expects 9 bytes. + +## `MSP_FILTER_CONFIG (92 / 0x5c)` +**Description:** Retrieves filter configuration settings (Gyro, D-term, Yaw, Accel). Some fields are BF compatibility placeholders or legacy. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `gyroMainLpfHz` | `uint8_t` | 1 | Hz | Gyro main low-pass filter cutoff frequency (`gyroConfig()->gyro_main_lpf_hz`) | +| `dtermLpfHz` | `uint16_t` | 2 | Hz | D-term low-pass filter cutoff frequency (`pidProfile()->dterm_lpf_hz`) | +| `yawLpfHz` | `uint16_t` | 2 | Hz | Yaw low-pass filter cutoff frequency (`pidProfile()->yaw_lpf_hz`) | +| `legacyGyroNotchHz` | `uint16_t` | 2 | - | Always 0 (Legacy) | +| `legacyGyroNotchCutoff` | `uint16_t` | 2 | - | Always 1 (Legacy) | +| `bfCompatDtermNotchHz` | `uint16_t` | 2 | - | Always 0 (BF compatibility) | +| `bfCompatDtermNotchCutoff` | `uint16_t` | 2 | - | Always 1 (BF compatibility) | +| `bfCompatGyroNotch2Hz` | `uint16_t` | 2 | - | Always 0 (BF compatibility) | +| `bfCompatGyroNotch2Cutoff` | `uint16_t` | 2 | - | Always 1 (BF compatibility) | +| `accNotchHz` | `uint16_t` | 2 | Hz | Accelerometer notch filter center frequency (`accelerometerConfig()->acc_notch_hz`) | +| `accNotchCutoff` | `uint16_t` | 2 | Hz | Accelerometer notch filter cutoff frequency (`accelerometerConfig()->acc_notch_cutoff`) | +| `legacyGyroStage2LpfHz` | `uint16_t` | 2 | - | Always 0 (Legacy) | + +## `MSP_SET_FILTER_CONFIG (93 / 0x5d)` +**Description:** Sets filter configuration settings. Handles different payload lengths for backward compatibility. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `gyroMainLpfHz` | `uint8_t` | 1 | Hz | Sets `gyroConfigMutable()->gyro_main_lpf_hz`. (Size >= 5) | +| `dtermLpfHz` | `uint16_t` | 2 | Hz | Sets `pidProfileMutable()->dterm_lpf_hz` (constrained 0-500). (Size >= 5) | +| `yawLpfHz` | `uint16_t` | 2 | Hz | Sets `pidProfileMutable()->yaw_lpf_hz` (constrained 0-255). (Size >= 5) | +| `legacyGyroNotchHz` | `uint16_t` | 2 | - | Ignored. (Size >= 9) | +| `legacyGyroNotchCutoff` | `uint16_t` | 2 | - | Ignored. (Size >= 9) | +| `bfCompatDtermNotchHz` | `uint16_t` | 2 | - | Ignored. (Size >= 13) | +| `bfCompatDtermNotchCutoff` | `uint16_t` | 2 | - | Ignored. (Size >= 13) | +| `bfCompatGyroNotch2Hz` | `uint16_t` | 2 | - | Ignored. (Size >= 17) | +| `bfCompatGyroNotch2Cutoff` | `uint16_t` | 2 | - | Ignored. (Size >= 17) | +| `accNotchHz` | `uint16_t` | 2 | Hz | Sets `accelerometerConfigMutable()->acc_notch_hz` (constrained 0-255). (Size >= 21) | +| `accNotchCutoff` | `uint16_t` | 2 | Hz | Sets `accelerometerConfigMutable()->acc_notch_cutoff` (constrained 1-255). (Size >= 21) | +| `legacyGyroStage2LpfHz` | `uint16_t` | 2 | - | Ignored. (Size >= 22) | + +**Reply Payload:** **None** + +**Notes:** Requires at least 22 bytes; intermediate length checks enforce legacy Betaflight frame layout and call `pidInitFilters()` once the D-term notch placeholders are consumed. + +## `MSP_PID_ADVANCED (94 / 0x5e)` +**Description:** Retrieves advanced PID tuning parameters. Many fields are BF compatibility placeholders. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `legacyRollPitchItermIgnore` | `uint16_t` | 2 | - | Always 0 (Legacy) | +| `legacyYawItermIgnore` | `uint16_t` | 2 | - | Always 0 (Legacy) | +| `legacyYawPLimit` | `uint16_t` | 2 | - | Always 0 (Legacy) | +| `bfCompatDeltaMethod` | `uint8_t` | 1 | - | Always 0 (BF compatibility) | +| `bfCompatVbatPidComp` | `uint8_t` | 1 | - | Always 0 (BF compatibility) | +| `bfCompatSetpointRelaxRatio` | `uint8_t` | 1 | - | Always 0 (BF compatibility) | +| `reserved1` | `uint8_t` | 1 | - | Always 0 | +| `legacyPidSumLimit` | `uint16_t` | 2 | - | Always 0 (Legacy) | +| `bfCompatItermThrottleGain` | `uint8_t` | 1 | - | Always 0 (BF compatibility) | +| `accelLimitRollPitch` | `uint16_t` | 2 | dps / 10 | Axis acceleration limit for Roll/Pitch / 10 (`pidProfile()->axisAccelerationLimitRollPitch / 10`) | +| `accelLimitYaw` | `uint16_t` | 2 | dps / 10 | Axis acceleration limit for Yaw / 10 (`pidProfile()->axisAccelerationLimitYaw / 10`) | + +**Notes:** Acceleration limits are scaled by 10 for compatibility. + +## `MSP_SET_PID_ADVANCED (95 / 0x5f)` +**Description:** Sets advanced PID tuning parameters. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `legacyRollPitchItermIgnore` | `uint16_t` | 2 | - | Ignored (legacy compatibility). | +| `legacyYawItermIgnore` | `uint16_t` | 2 | - | Ignored (legacy compatibility). | +| `legacyYawPLimit` | `uint16_t` | 2 | - | Ignored (legacy compatibility). | +| `bfCompatDeltaMethod` | `uint8_t` | 1 | - | Ignored (BF compatibility). | +| `bfCompatVbatPidComp` | `uint8_t` | 1 | - | Ignored (BF compatibility). | +| `bfCompatSetpointRelaxRatio` | `uint8_t` | 1 | - | Ignored (BF compatibility). | +| `reserved1` | `uint8_t` | 1 | - | Ignored (reserved). | +| `legacyPidSumLimit` | `uint16_t` | 2 | - | Ignored (legacy compatibility). | +| `bfCompatItermThrottleGain` | `uint8_t` | 1 | - | Ignored (BF compatibility). | +| `accelLimitRollPitch` | `uint16_t` | 2 | dps / 10 | Sets `pidProfileMutable()->axisAccelerationLimitRollPitch = value * 10`. | +| `accelLimitYaw` | `uint16_t` | 2 | dps / 10 | Sets `pidProfileMutable()->axisAccelerationLimitYaw = value * 10`. | + +**Reply Payload:** **None** + +**Notes:** Expects 17 bytes. + +## `MSP_SENSOR_CONFIG (96 / 0x60)` +**Description:** Retrieves the configured hardware type for various sensors. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `accHardware` | `uint8_t` | 1 | [accelerationSensor_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-accelerationsensor_e) | Enum (`accelerationSensor_e`): Accelerometer hardware type (`accelerometerConfig()->acc_hardware`) | +| `baroHardware` | `uint8_t` | 1 | [baroSensor_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-barosensor_e) | Enum (`baroSensor_e`): Barometer hardware type (`barometerConfig()->baro_hardware`). 0 if `USE_BARO` disabled | +| `magHardware` | `uint8_t` | 1 | [magSensor_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-magsensor_e) | Enum (`magSensor_e`): Magnetometer hardware type (`compassConfig()->mag_hardware`). 0 if `USE_MAG` disabled | +| `pitotHardware` | `uint8_t` | 1 | [pitotSensor_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-pitotsensor_e) | Enum (`pitotSensor_e`): Pitot tube hardware type (`pitotmeterConfig()->pitot_hardware`). 0 if `USE_PITOT` disabled | +| `rangefinderHardware` | `uint8_t` | 1 | [rangefinderType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-rangefindertype_e) | Enum (`rangefinderType_e`): Rangefinder hardware type (`rangefinderConfig()->rangefinder_hardware`). 0 if `USE_RANGEFINDER` disabled | +| `opflowHardware` | `uint8_t` | 1 | [opticalFlowSensor_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-opticalflowsensor_e) | Enum (`opticalFlowSensor_e`): Optical flow hardware type (`opticalFlowConfig()->opflow_hardware`). 0 if `USE_OPFLOW` disabled | + +## `MSP_SET_SENSOR_CONFIG (97 / 0x61)` +**Description:** Sets the configured hardware type for various sensors. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `accHardware` | `uint8_t` | 1 | [accelerationSensor_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-accelerationsensor_e) | Sets `accelerometerConfigMutable()->acc_hardware` | +| `baroHardware` | `uint8_t` | 1 | [baroSensor_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-barosensor_e) | Sets `barometerConfigMutable()->baro_hardware` (if `USE_BARO`) | +| `magHardware` | `uint8_t` | 1 | [magSensor_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-magsensor_e) | Sets `compassConfigMutable()->mag_hardware` (if `USE_MAG`) | +| `pitotHardware` | `uint8_t` | 1 | [pitotSensor_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-pitotsensor_e) | Sets `pitotmeterConfigMutable()->pitot_hardware` (if `USE_PITOT`) | +| `rangefinderHardware` | `uint8_t` | 1 | [rangefinderType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-rangefindertype_e) | Sets `rangefinderConfigMutable()->rangefinder_hardware` (if `USE_RANGEFINDER`) | +| `opflowHardware` | `uint8_t` | 1 | [opticalFlowSensor_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-opticalflowsensor_e) | Sets `opticalFlowConfigMutable()->opflow_hardware` (if `USE_OPFLOW`) | + +**Reply Payload:** **None** + +**Notes:** Expects 6 bytes. + +## `MSP_SPECIAL_PARAMETERS (98 / 0x62)` +**Description:** Betaflight specific + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Not implemented in INAV `fc_msp.c`. + +## `MSP_SET_SPECIAL_PARAMETERS (99 / 0x63)` +**Description:** Betaflight specific + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Not implemented in INAV `fc_msp.c`. + +## `MSP_IDENT (100 / 0x64)` +**Description:** Provides basic flight controller identity information. Not implemented in modern INAV, but used by legacy versions and MultiWii. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `MultiWii version` | `uint8_t` | 1 | n/a | Scaled version major*100+minor | +| `Mixer Mode` | `uint8_t` | 1 | Enum | Mixer type | +| `MSP Version` | `uint8_t` | 1 | n/a | Scaled version major*100+minor | +| `Platform Capability` | `uint32_t` | 4 | Bitmask | Bitmask: MW capabilities | + +**Notes:** Obsolete. Listed for legacy compatibility only. + +## `MSP_STATUS (101 / 0x65)` +**Description:** Provides basic flight controller status including cycle time, errors, sensor status, active modes (first 32), and the current configuration profile. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `cycleTime` | `uint16_t` | 2 | µs | Main loop cycle time (`cycleTime`) | +| `i2cErrors` | `uint16_t` | 2 | Count | Number of I2C errors encountered (`i2cGetErrorCounter()`). 0 if `USE_I2C` not defined | +| `sensorStatus` | `uint16_t` | 2 | Bitmask | Bitmask: available/active sensors (`packSensorStatus()`). See notes | +| `activeModesLow` | `uint32_t` | 4 | Bitmask | Bitmask: First 32 bits of the active flight modes bitmask (`packBoxModeFlags()`) | +| `profile` | `uint8_t` | 1 | Index | Current configuration profile index (0-based) (`getConfigProfile()`) | + +**Notes:** Superseded by `MSP_STATUS_EX` and `MSP2_INAV_STATUS`. `sensorStatus` bitmask: (Bit 0: ACC, 1: BARO, 2: MAG, 3: GPS, 4: RANGEFINDER, 5: OPFLOW, 6: PITOT, 7: TEMP; Bit 15: hardware failure). `activeModesLow` only contains the first 32 modes; use `MSP_ACTIVEBOXES` for the full set. + +## `MSP_RAW_IMU (102 / 0x66)` +**Description:** Provides raw sensor readings from the IMU (Accelerometer, Gyroscope, Magnetometer). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `accX` | `int16_t` | 2 | ~1/512 G | Raw accelerometer X reading, scaled (`acc.accADCf[X] * 512`) | +| `accY` | `int16_t` | 2 | ~1/512 G | Raw accelerometer Y reading, scaled (`acc.accADCf[Y] * 512`) | +| `accZ` | `int16_t` | 2 | ~1/512 G | Raw accelerometer Z reading, scaled (`acc.accADCf[Z] * 512`) | +| `gyroX` | `int16_t` | 2 | deg/s | Gyroscope X-axis rate (`gyroRateDps(X)`) | +| `gyroY` | `int16_t` | 2 | deg/s | Gyroscope Y-axis rate (`gyroRateDps(Y)`) | +| `gyroZ` | `int16_t` | 2 | deg/s | Gyroscope Z-axis rate (`gyroRateDps(Z)`) | +| `magX` | `int16_t` | 2 | Raw units | Raw magnetometer X reading (`mag.magADC[X]`). 0 if `USE_MAG` disabled | +| `magY` | `int16_t` | 2 | Raw units | Raw magnetometer Y reading (`mag.magADC[Y]`). 0 if `USE_MAG` disabled | +| `magZ` | `int16_t` | 2 | Raw units | Raw magnetometer Z reading (`mag.magADC[Z]`). 0 if `USE_MAG` disabled | + +**Notes:** Acc scaling is approximate (512 LSB/G). Mag units depend on the sensor. + +## `MSP_SERVO (103 / 0x67)` +**Description:** Provides the current output values for all supported servos. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `servoOutputs` | `int16_t[MAX_SUPPORTED_SERVOS]` | 36 (MAX_SUPPORTED_SERVOS) | PWM | Array of current servo output values (typically 1000-2000) | + +## `MSP_MOTOR (104 / 0x68)` +**Description:** Provides the current output values for the first 8 motors. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `motorOutputs` | `int16_t[8]` | 16 | PWM | Array of current motor output values (typically 1000-2000). Values beyond `MAX_SUPPORTED_MOTORS` are 0 | + +## `MSP_RC (105 / 0x69)` +**Description:** Provides the current values of the received RC channels. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `rcChannels` | `int16_t[]` | array | PWM | Array of current RC channel values (typically 1000-2000). Length depends on detected channels | + +**Notes:** Array length equals `rxRuntimeConfig.channelCount`. + +## `MSP_RAW_GPS (106 / 0x6a)` +**Description:** Provides raw GPS data (fix status, coordinates, altitude, speed, course). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `fixType` | `uint8_t` | 1 | [gpsFixType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-gpsfixtype_e) | Enum `gpsFixType_e` GPS fix type (`gpsSol.fixType`) | +| `numSat` | `uint8_t` | 1 | Count | Number of satellites used in solution (`gpsSol.numSat`) | +| `latitude` | `int32_t` | 4 | deg * 1e7 | Latitude (`gpsSol.llh.lat`) | +| `longitude` | `int32_t` | 4 | deg * 1e7 | Longitude (`gpsSol.llh.lon`) | +| `altitude` | `int16_t` | 2 | cm | Altitude above MSL (`gpsSol.llh.alt`) sent as centimeters | +| `speed` | `int16_t` | 2 | cm/s | Ground speed (`gpsSol.groundSpeed`) | +| `groundCourse` | `int16_t` | 2 | deci-degrees | Ground course (`gpsSol.groundCourse`) | +| `hdop` | `uint16_t` | 2 | HDOP * 100 | Horizontal Dilution of Precision (`gpsSol.hdop`) | + +**Notes:** Only available if `USE_GPS` is defined. Altitude is truncated to meters. + +## `MSP_COMP_GPS (107 / 0x6b)` +**Description:** Provides computed GPS values: distance and direction to home. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `distanceToHome` | `uint16_t` | 2 | meters | Distance to the home point (`GPS_distanceToHome`) | +| `directionToHome` | `int16_t` | 2 | degrees | Direction to the home point (0-360) (`GPS_directionToHome`) | +| `gpsHeartbeat` | `uint8_t` | 1 | Boolean | Indicates if GPS data is being received (`gpsSol.flags.gpsHeartbeat`) | + +**Notes:** Only available if `USE_GPS` is defined. + +## `MSP_ATTITUDE (108 / 0x6c)` +**Description:** Provides the current attitude estimate (roll, pitch, yaw). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `roll` | `int16_t` | 2 | deci-degrees | Roll angle (`attitude.values.roll`) | +| `pitch` | `int16_t` | 2 | deci-degrees | Pitch angle (`attitude.values.pitch`) | +| `yaw` | `int16_t` | 2 | degrees | Yaw/Heading angle (`DECIDEGREES_TO_DEGREES(attitude.values.yaw)`) | + +**Notes:** Yaw is in degrees. + +## `MSP_ALTITUDE (109 / 0x6d)` +**Description:** Provides estimated altitude, vertical speed (variometer), and raw barometric altitude. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `estimatedAltitude` | `int32_t` | 4 | cm | Estimated altitude above home/sea level (`getEstimatedActualPosition(Z)`) | +| `variometer` | `int16_t` | 2 | cm/s | Estimated vertical speed (`getEstimatedActualVelocity(Z)`) | +| `baroAltitude` | `int32_t` | 4 | cm | Latest raw altitude from barometer (`baroGetLatestAltitude()`). 0 if `USE_BARO` disabled | + +## `MSP_ANALOG (110 / 0x6e)` +**Description:** Provides analog sensor readings: battery voltage, current consumption (mAh), RSSI, and current draw (Amps). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `vbat` | `uint8_t` | 1 | 0.1V | Battery voltage, scaled (`getBatteryVoltage() / 10`), constrained 0-255 | +| `mAhDrawn` | `uint16_t` | 2 | mAh | Consumed battery capacity (`getMAhDrawn()`), constrained 0-65535 | +| `rssi` | `uint16_t` | 2 | 0-1023 or % | Received Signal Strength Indicator (`getRSSI()`). Units depend on source | +| `amperage` | `int16_t` | 2 | 0.01A | Current draw (`getAmperage()`), constrained -32768 to 32767 | + +**Notes:** Superseded by `MSP2_INAV_ANALOG` which provides higher precision and more fields. + +## `MSP_RC_TUNING (111 / 0x6f)` +**Description:** Retrieves RC tuning parameters (rates, expos, TPA) for the current control rate profile. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `legacyRcRate` | `uint8_t` | 1 | Always 100 (Legacy, unused) | +| `rcExpo` | `uint8_t` | 1 | Roll/Pitch RC Expo (`currentControlRateProfile->stabilized.rcExpo8`) | +| `rollRate` | `uint8_t` | 1 | Roll Rate (`currentControlRateProfile->stabilized.rates[FD_ROLL]`) | +| `pitchRate` | `uint8_t` | 1 | Pitch Rate (`currentControlRateProfile->stabilized.rates[FD_PITCH]`) | +| `yawRate` | `uint8_t` | 1 | Yaw Rate (`currentControlRateProfile->stabilized.rates[FD_YAW]`) | +| `dynamicThrottlePID` | `uint8_t` | 1 | Dynamic Throttle PID (TPA) value (`currentControlRateProfile->throttle.dynPID`) | +| `throttleMid` | `uint8_t` | 1 | Throttle Midpoint (`currentControlRateProfile->throttle.rcMid8`) | +| `throttleExpo` | `uint8_t` | 1 | Throttle Expo (`currentControlRateProfile->throttle.rcExpo8`) | +| `tpaBreakpoint` | `uint16_t` | 2 | Throttle PID Attenuation (TPA) breakpoint (`currentControlRateProfile->throttle.pa_breakpoint`) | +| `rcYawExpo` | `uint8_t` | 1 | Yaw RC Expo (`currentControlRateProfile->stabilized.rcYawExpo8`) | + +**Notes:** Superseded by `MSP2_INAV_RATE_PROFILE` which includes manual rates/expos. + +## `MSP_ACTIVEBOXES (113 / 0x71)` +**Description:** Provides the full bitmask of currently active flight modes (boxes). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `activeModes` | `boxBitmask_t` | - | Bitmask | Bitmask: all active modes (`packBoxModeFlags()`). Size depends on `boxBitmask_t` definition | + +**Notes:** Use this instead of `MSP_STATUS` or `MSP_STATUS_EX` if more than 32 modes are possible. + +## `MSP_MISC (114 / 0x72)` +**Description:** Retrieves miscellaneous configuration settings, mostly related to RC, GPS, Mag, and Battery voltage (legacy formats). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `midRc` | `uint16_t` | 2 | PWM | Mid RC value (`PWM_RANGE_MIDDLE`, typically 1500) | +| `legacyMinThrottle` | `uint16_t` | 2 | - | Always 0 (Legacy) | +| `maxThrottle` | `uint16_t` | 2 | PWM | Maximum throttle command (`getMaxThrottle()`) | +| `minCommand` | `uint16_t` | 2 | PWM | Minimum motor command when disarmed (`motorConfig()->mincommand`) | +| `failsafeThrottle` | `uint16_t` | 2 | PWM | Failsafe throttle level (`currentBatteryProfile->failsafe_throttle`) | +| `gpsType` | `uint8_t` | 1 | [gpsProvider_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-gpsprovider_e) | Enum `gpsProvider_e` GPS provider type (`gpsConfig()->provider`). 0 if `USE_GPS` disabled | +| `legacyGpsBaud` | `uint8_t` | 1 | - | Always 0 (Legacy) | +| `gpsSbasMode` | `uint8_t` | 1 | [sbasMode_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-sbasmode_e) | Enum `sbasMode_e` GPS SBAS mode (`gpsConfig()->sbasMode`). 0 if `USE_GPS` disabled | +| `legacyMwCurrentOut` | `uint8_t` | 1 | - | Always 0 (Legacy) | +| `rssiChannel` | `uint8_t` | 1 | Index | RSSI channel index (1-based) (`rxConfig()->rssi_channel`) | +| `reserved1` | `uint8_t` | 1 | - | Always 0 | +| `magDeclination` | `uint16_t` | 2 | 0.1 degrees | Magnetic declination / 10 (`compassConfig()->mag_declination / 10`). 0 if `USE_MAG` disabled | +| `vbatScale` | `uint8_t` | 1 | Scale / 10 | Voltage scale / 10 (`batteryMetersConfig()->voltage.scale / 10`). 0 if `USE_ADC` disabled | +| `vbatMinCell` | `uint8_t` | 1 | 0.1V | Min cell voltage / 10 (`currentBatteryProfile->voltage.cellMin / 10`). 0 if `USE_ADC` disabled | +| `vbatMaxCell` | `uint8_t` | 1 | 0.1V | Max cell voltage / 10 (`currentBatteryProfile->voltage.cellMax / 10`). 0 if `USE_ADC` disabled | +| `vbatWarningCell` | `uint8_t` | 1 | 0.1V | Warning cell voltage / 10 (`currentBatteryProfile->voltage.cellWarning / 10`). 0 if `USE_ADC` disabled | + +**Notes:** Superseded by `MSP2_INAV_MISC` and other specific commands which offer better precision and more fields. + +## `MSP_BOXNAMES (116 / 0x74)` +**Description:** Provides a semicolon-separated string containing the names of all available flight modes (boxes). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `boxNamesString` | `char[]` | array | String containing mode names separated by ';'. Null termination not guaranteed by MSP, relies on payload size. (`serializeBoxNamesReply()`) | + +**Notes:** The exact set of names depends on compiled features and configuration. Due to the size of the payload, it is recommended that [`MSP_BOXIDS`](#msp_boxids-119--0x77) is used instead. + +## `MSP_PIDNAMES (117 / 0x75)` +**Description:** Provides a semicolon-separated string containing the names of the PID controllers. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `pidNamesString` | `char[]` | array | String "ROLL;PITCH;YAW;ALT;Pos;PosR;NavR;LEVEL;MAG;VEL;". Null termination not guaranteed by MSP | + +## `MSP_WP (118 / 0x76)` +**Description:** Get/Set a single waypoint from the mission plan. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `waypointIndex` | `uint8_t` | 1 | Index of the waypoint to retrieve (0 to `NAV_MAX_WAYPOINTS - 1`) | + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `waypointIndex` | `uint8_t` | 1 | Index | Index of the returned waypoint | +| `action` | `uint8_t` | 1 | [navWaypointActions_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-navwaypointactions_e) | Enum `navWaypointActions_e` Waypoint action type | +| `latitude` | `int32_t` | 4 | deg * 1e7 | Latitude coordinate | +| `longitude` | `int32_t` | 4 | deg * 1e7 | Longitude coordinate | +| `altitude` | `int32_t` | 4 | cm | Altitude coordinate (relative to home or sea level, see flag) | +| `param1` | `int16_t` | 2 | Varies | Parameter 1 (meaning depends on action) | +| `param2` | `int16_t` | 2 | Varies | Parameter 2 (meaning depends on action) | +| `param3` | `int16_t` | 2 | Varies | Parameter 3 (meaning depends on action) | +| `flag` | `uint8_t` | 1 | Bitmask | Bitmask: Waypoint flags (`NAV_WP_FLAG_*`) | + +**Notes:** See `navWaypoint_t` and `navWaypointActions_e`. + +## `MSP_BOXIDS (119 / 0x77)` +**Description:** Provides a list of permanent IDs associated with the available flight modes (boxes). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `boxIds` | `uint8_t[]` | array | Array of permanent IDs for each configured box (`serializeBoxReply()`). Length depends on number of boxes | + +**Notes:** Useful for mapping mode range configurations (`MSP_MODE_RANGES`) back to user-understandable modes via `MSP_BOXNAMES`. + +## `MSP_SERVO_CONFIGURATIONS (120 / 0x78)` +**Description:** Retrieves the configuration parameters for all supported servos (min, max, middle, rate). Legacy format with unused fields. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `min` | `int16_t` | 2 | PWM | Minimum servo endpoint (`servoParams(i)->min`) | +| `max` | `int16_t` | 2 | PWM | Maximum servo endpoint (`servoParams(i)->max`) | +| `middle` | `int16_t` | 2 | PWM | Middle/Neutral servo position (`servoParams(i)->middle`) | +| `rate` | `int8_t` | 1 | % (-100 to 100) | Servo rate/scaling (`servoParams(i)->rate`, -125..125). Encoded as two's complement | +| `reserved1` | `uint8_t` | 1 | - | Always 0 | +| `reserved2` | `uint8_t` | 1 | - | Always 0 | +| `legacyForwardChan` | `uint8_t` | 1 | - | Always 255 (Legacy) | +| `legacyReversedSources` | `uint32_t` | 4 | - | Always 0 (Legacy) | + +**Notes:** Superseded by `MSP2_INAV_SERVO_CONFIG` which has a cleaner structure. + +## `MSP_NAV_STATUS (121 / 0x79)` +**Description:** Retrieves the current status of the navigation system. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `navMode` | `uint8_t` | 1 | [navSystemStatus_Mode_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-navsystemstatus_mode_e) | Enum (`navSystemStatus_Mode_e`): Current navigation mode (None, RTH, NAV, Hold, etc.) (`NAV_Status.mode`) | +| `navState` | `uint8_t` | 1 | [navSystemStatus_State_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-navsystemstatus_state_e) | Enum (`navSystemStatus_State_e`): Current navigation state (`NAV_Status.state`) | +| `activeWpAction` | `uint8_t` | 1 | [navWaypointActions_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-navwaypointactions_e) | Enum (`navWaypointActions_e`): Action of the currently executing waypoint (`NAV_Status.activeWpAction`) | +| `activeWpNumber` | `uint8_t` | 1 | - | Index: Index of the currently executing waypoint (`NAV_Status.activeWpNumber`) | +| `navError` | `uint8_t` | 1 | [navSystemStatus_Error_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-navsystemstatus_error_e) | Enum (`navSystemStatus_Error_e`): Current navigation error code (`NAV_Status.error`) | +| `targetHeading` | `int16_t` | 2 | degrees | Target heading for heading controller (`getHeadingHoldTarget()`) | + +**Notes:** Requires `USE_GPS`. + +## `MSP_NAV_CONFIG (122 / 0x7a)` + +**Request Payload:** **None** + +**Reply Payload:** **None** + +## `MSP_3D (124 / 0x7c)` +**Description:** Retrieves settings related to 3D/reversible motor operation. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `deadbandLow` | `uint16_t` | 2 | PWM | Lower deadband limit for 3D mode (`reversibleMotorsConfig()->deadband_low`) | +| `deadbandHigh` | `uint16_t` | 2 | PWM | Upper deadband limit for 3D mode (`reversibleMotorsConfig()->deadband_high`) | +| `neutral` | `uint16_t` | 2 | PWM | Neutral throttle point for 3D mode (`reversibleMotorsConfig()->neutral`) | + +**Notes:** Requires reversible motor support. + +## `MSP_RC_DEADBAND (125 / 0x7d)` +**Description:** Retrieves RC input deadband settings. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `deadband` | `uint8_t` | 1 | PWM | General RC deadband for Roll/Pitch (`rcControlsConfig()->deadband`) | +| `yawDeadband` | `uint8_t` | 1 | PWM | Specific deadband for Yaw (`rcControlsConfig()->yaw_deadband`) | +| `altHoldDeadband` | `uint8_t` | 1 | PWM | Deadband for altitude hold adjustments (`rcControlsConfig()->alt_hold_deadband`) | +| `throttleDeadband` | `uint16_t` | 2 | PWM | Deadband around throttle mid-stick (`rcControlsConfig()->mid_throttle_deadband`) | + +## `MSP_SENSOR_ALIGNMENT (126 / 0x7e)` +**Description:** Retrieves sensor alignment settings (legacy format). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `gyroAlign` | `uint8_t` | 1 | Always 0 (Legacy alignment enum) | +| `accAlign` | `uint8_t` | 1 | Always 0 (Legacy alignment enum) | +| `magAlign` | `uint8_t` | 1 | Magnetometer alignment (`compassConfig()->mag_align`). 0 if `USE_MAG` disabled | +| `opflowAlign` | `uint8_t` | 1 | Optical flow alignment (`opticalFlowConfig()->opflow_align`). 0 if `USE_OPFLOW` disabled | + +**Notes:** Board alignment is now typically handled by `MSP_BOARD_ALIGNMENT`. This returns legacy enum values where applicable. + +## `MSP_LED_STRIP_MODECOLOR (127 / 0x7f)` +**Description:** Retrieves the color index assigned to each LED mode and function/direction combination, including special colors. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `modeIndex` | `uint8_t` | 1 | [ledModeIndex_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-ledmodeindex_e) | Index of the LED mode Enum (`ledModeIndex_e`). `LED_MODE_COUNT` for special colors | +| `directionOrSpecialIndex` | `uint8_t` | 1 | - | Index of the direction (`ledDirectionId_e`) or special color (`ledSpecialColorIds_e`) | +| `colorIndex` | `uint8_t` | 1 | - | Index of the color assigned from `ledStripConfig()->colors` | + +**Notes:** Only available if `USE_LED_STRIP` is defined. Entries where `modeIndex == LED_MODE_COUNT` describe special colors. + +## `MSP_BATTERY_STATE (130 / 0x82)` +**Description:** Provides battery state information, formatted primarily for DJI FPV Goggles compatibility. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `cellCount` | `uint8_t` | 1 | Count | Number of battery cells (`getBatteryCellCount()`) | +| `capacity` | `uint16_t` | 2 | mAh | Battery capacity (`currentBatteryProfile->capacity.value`) | +| `vbatScaled` | `uint8_t` | 1 | 0.1V | Battery voltage / 10 (`getBatteryVoltage() / 10`) | +| `mAhDrawn` | `uint16_t` | 2 | mAh | Consumed capacity (`getMAhDrawn()`) | +| `amperage` | `int16_t` | 2 | 0.01A | Current draw (`getAmperage()`) | +| `batteryState` | `uint8_t` | 1 | [batteryState_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-batterystate_e) | Enum `batteryState_e` Current battery state (`getBatteryState()`, see `BATTERY_STATE_*`) | +| `vbatActual` | `uint16_t` | 2 | 0.01V | Actual battery voltage (`getBatteryVoltage()`) | + +**Notes:** Only available if `USE_DJI_HD_OSD` or `USE_MSP_DISPLAYPORT` is defined. Some values are duplicated from `MSP_ANALOG` / `MSP2_INAV_ANALOG` but potentially with different scaling/types. + +## `MSP_VTXTABLE_BAND (137 / 0x89)` +**Description:** Retrieves information about a specific VTX band from the VTX table. (Implementation missing in provided `fc_msp.c`) + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** The ID is defined, but no handler exists in the provided C code. Likely intended to query band names and frequencies. + +## `MSP_VTXTABLE_POWERLEVEL (138 / 0x8a)` +**Description:** Retrieves information about a specific VTX power level from the VTX table. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `powerLevelIndex` | `uint8_t` | 1 | 1-based index of the power level to query | + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `powerLevelIndex` | `uint8_t` | 1 | 1-based index of the returned power level | +| `powerValue` | `uint16_t` | 2 | Always 0 (Actual power value in mW is not stored/returned via MSP) | +| `labelLength` | `uint8_t` | 1 | Length of the power level label string that follows | +| `label` | `char[]` | array | Power level label string (e.g., "25", "200"). Length given by previous field | + +**Notes:** Requires `USE_VTX_CONTROL`. Returns error if index is out of bounds. The `powerValue` field is unused. + +## `MSP_STATUS_EX (150 / 0x96)` +**Description:** Provides extended flight controller status, including CPU load, arming flags, and calibration status, in addition to `MSP_STATUS` fields. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `cycleTime` | `uint16_t` | 2 | µs | Main loop cycle time | +| `i2cErrors` | `uint16_t` | 2 | Count | I2C errors | +| `sensorStatus` | `uint16_t` | 2 | Bitmask | Bitmask: Sensor status | +| `activeModesLow` | `uint32_t` | 4 | Bitmask | Bitmask: First 32 active modes | +| `profile` | `uint8_t` | 1 | Index | Current config profile index | +| `cpuLoad` | `uint16_t` | 2 | % | Average system load percentage (`averageSystemLoadPercent`) | +| `armingFlags` | `uint16_t` | 2 | Bitmask | Bitmask: Flight controller arming flags (`armingFlags`). Note: Truncated to 16 bits | +| `accCalibAxisFlags` | `uint8_t` | 1 | Bitmask | Bitmask: Accelerometer calibrated axes flags (`accGetCalibrationAxisFlags()`) | + +**Notes:** Superseded by `MSP2_INAV_STATUS` which provides the full 32-bit `armingFlags` and other enhancements. + +## `MSP_SENSOR_STATUS (151 / 0x97)` +**Description:** Provides the hardware status for each individual sensor system. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `overallHealth` | `uint8_t` | 1 | Boolean | 1 if all essential hardware is healthy, 0 otherwise (`isHardwareHealthy()`) | +| `gyroStatus` | `uint8_t` | 1 | [hardwareSensorStatus_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-hardwaresensorstatus_e) | Enum `hardwareSensorStatus_e` Gyro hardware status (`getHwGyroStatus()`) | +| `accStatus` | `uint8_t` | 1 | [hardwareSensorStatus_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-hardwaresensorstatus_e) | Enum `hardwareSensorStatus_e` Accelerometer hardware status (`getHwAccelerometerStatus()`) | +| `magStatus` | `uint8_t` | 1 | [hardwareSensorStatus_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-hardwaresensorstatus_e) | Enum `hardwareSensorStatus_e` Compass hardware status (`getHwCompassStatus()`) | +| `baroStatus` | `uint8_t` | 1 | [hardwareSensorStatus_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-hardwaresensorstatus_e) | Enum `hardwareSensorStatus_e` Barometer hardware status (`getHwBarometerStatus()`) | +| `gpsStatus` | `uint8_t` | 1 | [hardwareSensorStatus_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-hardwaresensorstatus_e) | Enum `hardwareSensorStatus_e` GPS hardware status (`getHwGPSStatus()`) | +| `rangefinderStatus` | `uint8_t` | 1 | [hardwareSensorStatus_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-hardwaresensorstatus_e) | Enum `hardwareSensorStatus_e` Rangefinder hardware status (`getHwRangefinderStatus()`) | +| `pitotStatus` | `uint8_t` | 1 | [hardwareSensorStatus_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-hardwaresensorstatus_e) | Enum `hardwareSensorStatus_e` Pitot hardware status (`getHwPitotmeterStatus()`) | +| `opflowStatus` | `uint8_t` | 1 | [hardwareSensorStatus_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-hardwaresensorstatus_e) | Enum `hardwareSensorStatus_e` Optical Flow hardware status (`getHwOpticalFlowStatus()`) | + +**Notes:** Status values map to the `hardwareSensorStatus_e` enum: `HW_SENSOR_NONE`, `HW_SENSOR_OK`, `HW_SENSOR_UNAVAILABLE`, `HW_SENSOR_UNHEALTHY`. + +## `MSP_UID (160 / 0xa0)` +**Description:** Provides the unique identifier of the microcontroller. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `uid0` | `uint32_t` | 4 | First 32 bits of the unique ID (`U_ID_0`) | +| `uid1` | `uint32_t` | 4 | Middle 32 bits of the unique ID (`U_ID_1`) | +| `uid2` | `uint32_t` | 4 | Last 32 bits of the unique ID (`U_ID_2`) | + +**Notes:** Total 12 bytes, representing a 96-bit unique ID. + +## `MSP_GPSSVINFO (164 / 0xa4)` +**Description:** Provides satellite signal strength information (legacy U-Blox compatibility stub). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `protocolVersion` | `uint8_t` | 1 | Always 1 (Stub version) | +| `numChannels` | `uint8_t` | 1 | Always 0 (Number of SV info channels reported) | +| `hdopHundredsDigit` | `uint8_t` | 1 | Hundreds digit of HDOP (stub always writes 0) | +| `hdopTensDigit` | `uint8_t` | 1 | Tens digit of HDOP (`gpsSol.hdop / 100`, truncated) | +| `hdopUnitsDigit` | `uint8_t` | 1 | Units digit of HDOP (`gpsSol.hdop / 100`, duplicated by stub) | + +**Notes:** Requires `USE_GPS`. This is just a stub in INAV and does not provide actual per-satellite signal info. HDOP digits are not formatted correctly: tens and units both contain `gpsSol.hdop / 100`. + +## `MSP_GPSSTATISTICS (166 / 0xa6)` +**Description:** Provides debugging statistics for the GPS communication link. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `lastMessageDt` | `uint16_t` | 2 | ms | Time since last valid GPS message (`gpsStats.lastMessageDt`) | +| `errors` | `uint32_t` | 4 | Count | Number of GPS communication errors (`gpsStats.errors`) | +| `timeouts` | `uint32_t` | 4 | Count | Number of GPS communication timeouts (`gpsStats.timeouts`) | +| `packetCount` | `uint32_t` | 4 | Count | Number of valid GPS packets received (`gpsStats.packetCount`) | +| `hdop` | `uint16_t` | 2 | HDOP * 100 | Horizontal Dilution of Precision (`gpsSol.hdop`) | +| `eph` | `uint16_t` | 2 | cm | Estimated Horizontal Position Accuracy (`gpsSol.eph`) | +| `epv` | `uint16_t` | 2 | cm | Estimated Vertical Position Accuracy (`gpsSol.epv`) | + +**Notes:** Requires `USE_GPS`. + +## `MSP_OSD_VIDEO_CONFIG (180 / 0xb4)` + +**Request Payload:** **None** + +**Reply Payload:** **None** + +## `MSP_SET_OSD_VIDEO_CONFIG (181 / 0xb5)` + +**Request Payload:** **None** + +**Reply Payload:** **None** + +## `MSP_DISPLAYPORT (182 / 0xb6)` + +**Request Payload:** **None** + +**Reply Payload:** **None** + +## `MSP_SET_TX_INFO (186 / 0xba)` +**Description:** Allows a transmitter LUA script (or similar) to send runtime information (currently only RSSI) to the firmware. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `rssi` | `uint8_t` | 1 | Raw | RSSI value (0-255) provided by the external source; firmware scales it to 10-bit (`value << 2`) | + +**Reply Payload:** **None** + +**Notes:** Calls `setRSSIFromMSP()`. Expects 1 byte. + +## `MSP_TX_INFO (187 / 0xbb)` +**Description:** Provides information potentially useful for transmitter LUA scripts. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `rssiSource` | `uint8_t` | 1 | [rssiSource_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-rssisource_e) | Enum: Source of the RSSI value (`getRSSISource()`, see `rssiSource_e`) | +| `rtcDateTimeIsSet` | `uint8_t` | 1 | - | Boolean: 1 if the RTC has been set, 0 otherwise | + +**Notes:** See `rssiSource_e`. + +## `MSP_SET_RAW_RC (200 / 0xc8)` +**Description:** Provides raw RC channel data to the flight controller, typically used when the receiver is connected via MSP (e.g., MSP RX feature). + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `rcChannels` | `uint16_t[]` | array | PWM | Array of RC channel values (typically 1000-2000). Number of channels determined by payload size | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_RX_MSP`. Maximum channels `MAX_SUPPORTED_RC_CHANNEL_COUNT`. Calls `rxMspFrameReceive()`. + +## `MSP_SET_RAW_GPS (201 / 0xc9)` +**Description:** Provides raw GPS data to the flight controller, typically for simulation or external GPS injection. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `fixType` | `uint8_t` | 1 | [gpsFixType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-gpsfixtype_e) | Enum `gpsFixType_e` GPS fix type | +| `numSat` | `uint8_t` | 1 | Count | Number of satellites | +| `latitude` | `int32_t` | 4 | deg * 1e7 | Latitude | +| `longitude` | `int32_t` | 4 | deg * 1e7 | Longitude | +| `altitude` | `uint16_t` | 2 | m | Altitude in meters (converted to centimeters internally; limited to 0-65535 m) | +| `speed` | `uint16_t` | 2 | cm/s | Ground speed (`gpsSol.groundSpeed`) | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_GPS`. Expects 14 bytes. Updates `gpsSol` structure and calls `onNewGPSData()`. Note the altitude unit mismatch (meters in MSP, cm internal). Does not provide velocity components. + +## `MSP_SET_BOX (203 / 0xcb)` +**Description:** Sets the state of flight modes (boxes). (Likely unused/obsolete in INAV). + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Not implemented in INAV `fc_msp.c`. Mode changes are typically handled via RC channels (`MSP_MODE_RANGES`). + +## `MSP_SET_RC_TUNING (204 / 0xcc)` +**Description:** Sets RC tuning parameters (rates, expos, TPA) for the current control rate profile. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `legacyRcRate` | `uint8_t` | 1 | Ignored | +| `rcExpo` | `uint8_t` | 1 | Sets `currentControlRateProfile->stabilized.rcExpo8` | +| `rollRate` | `uint8_t` | 1 | Sets `currentControlRateProfile->stabilized.rates[FD_ROLL]` (constrained) | +| `pitchRate` | `uint8_t` | 1 | Sets `currentControlRateProfile->stabilized.rates[FD_PITCH]` (constrained) | +| `yawRate` | `uint8_t` | 1 | Sets `currentControlRateProfile->stabilized.rates[FD_YAW]` (constrained) | +| `dynamicThrottlePID` | `uint8_t` | 1 | Sets `currentControlRateProfile->throttle.dynPID` (constrained) | +| `throttleMid` | `uint8_t` | 1 | Sets `currentControlRateProfile->throttle.rcMid8` | +| `throttleExpo` | `uint8_t` | 1 | Sets `currentControlRateProfile->throttle.rcExpo8` | +| `tpaBreakpoint` | `uint16_t` | 2 | Sets `currentControlRateProfile->throttle.pa_breakpoint` | +| `rcYawExpo` | `uint8_t` | 1 | (Optional) Sets `currentControlRateProfile->stabilized.rcYawExpo8` | + +**Reply Payload:** **None** + +**Notes:** Expects 10 or 11 bytes. Calls `schedulePidGainsUpdate()`. Superseded by `MSP2_INAV_SET_RATE_PROFILE`. + +## `MSP_ACC_CALIBRATION (205 / 0xcd)` +**Description:** Starts the accelerometer calibration procedure. + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Will fail if armed. Calls `accStartCalibration()`. + +## `MSP_MAG_CALIBRATION (206 / 0xce)` +**Description:** Starts the magnetometer calibration procedure. + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Will fail if armed. Enables the `CALIBRATE_MAG` state flag. + +## `MSP_SET_MISC (207 / 0xcf)` +**Description:** Sets miscellaneous configuration settings (legacy formats/scaling). + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `midRc` | `uint16_t` | 2 | PWM | Ignored | +| `legacyMinThrottle` | `uint16_t` | 2 | - | Ignored | +| `legacyMaxThrottle` | `uint16_t` | 2 | - | Ignored | +| `minCommand` | `uint16_t` | 2 | PWM | Sets `motorConfigMutable()->mincommand` (constrained 0-PWM_RANGE_MAX) | +| `failsafeThrottle` | `uint16_t` | 2 | PWM | Sets `currentBatteryProfileMutable->failsafe_throttle` (constrained PWM_RANGE_MIN/MAX) | +| `gpsType` | `uint8_t` | 1 | [gpsProvider_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-gpsprovider_e) | Enum `gpsProvider_e` (Sets `gpsConfigMutable()->provider`) | +| `legacyGpsBaud` | `uint8_t` | 1 | - | Ignored | +| `gpsSbasMode` | `uint8_t` | 1 | [sbasMode_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-sbasmode_e) | Enum `sbasMode_e` (Sets `gpsConfigMutable()->sbasMode`) | +| `legacyMwCurrentOut` | `uint8_t` | 1 | - | Ignored | +| `rssiChannel` | `uint8_t` | 1 | Index | Sets `rxConfigMutable()->rssi_channel` (constrained 0-MAX_SUPPORTED_RC_CHANNEL_COUNT). Updates source | +| `reserved1` | `uint8_t` | 1 | - | Ignored | +| `magDeclination` | `uint16_t` | 2 | 0.1 degrees | Sets `compassConfigMutable()->mag_declination = value * 10` (if `USE_MAG`) | +| `vbatScale` | `uint8_t` | 1 | Scale / 10 | Sets `batteryMetersConfigMutable()->voltage.scale = value * 10` (if `USE_ADC`) | +| `vbatMinCell` | `uint8_t` | 1 | 0.1V | Sets `currentBatteryProfileMutable->voltage.cellMin = value * 10` (if `USE_ADC`) | +| `vbatMaxCell` | `uint8_t` | 1 | 0.1V | Sets `currentBatteryProfileMutable->voltage.cellMax = value * 10` (if `USE_ADC`) | +| `vbatWarningCell` | `uint8_t` | 1 | 0.1V | Sets `currentBatteryProfileMutable->voltage.cellWarning = value * 10` (if `USE_ADC`) | + +**Reply Payload:** **None** + +**Notes:** Expects 22 bytes. Superseded by `MSP2_INAV_SET_MISC`. + +## `MSP_RESET_CONF (208 / 0xd0)` +**Description:** Resets all configuration settings to their default values and saves to EEPROM. + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Will fail if armed. Suspends RX, calls `resetEEPROM()`, `writeEEPROM()`, `readEEPROM()`, resumes RX. Use with caution! + +## `MSP_SET_WP (209 / 0xd1)` +**Description:** Sets a single waypoint in the mission plan. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `waypointIndex` | `uint8_t` | 1 | Index | Index of the waypoint to set (0 to `NAV_MAX_WAYPOINTS - 1`) | +| `action` | `uint8_t` | 1 | [navWaypointActions_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-navwaypointactions_e) | Enum `navWaypointActions_e` Waypoint action type | +| `latitude` | `int32_t` | 4 | deg * 1e7 | Latitude coordinate | +| `longitude` | `int32_t` | 4 | deg * 1e7 | Longitude coordinate | +| `altitude` | `int32_t` | 4 | cm | Altitude coordinate | +| `param1` | `uint16_t` | 2 | Varies | Parameter 1 | +| `param2` | `uint16_t` | 2 | Varies | Parameter 2 | +| `param3` | `uint16_t` | 2 | Varies | Parameter 3 | +| `flag` | `uint8_t` | 1 | [navWaypointFlags_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-navwaypointflags_e) | Bitmask: Waypoint flags (`navWaypointFlags_e`) | + +**Reply Payload:** **None** + +**Notes:** Expects 21 bytes. Calls `setWaypoint()`. If `USE_FW_AUTOLAND` is enabled, this also interacts with autoland approach settings based on waypoint index and flags. + +## `MSP_SELECT_SETTING (210 / 0xd2)` +**Description:** Selects the active configuration profile and saves it. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `profileIndex` | `uint8_t` | 1 | Index of the profile to activate (0-based) | + +**Reply Payload:** **None** + +**Notes:** Will fail if armed. Calls `setConfigProfileAndWriteEEPROM()`. + +## `MSP_SET_HEAD (211 / 0xd3)` +**Description:** Sets the target heading for the heading hold controller (e.g., during MAG mode). + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `heading` | `uint16_t` | 2 | degrees | Target heading (0-359) | + +**Reply Payload:** **None** + +**Notes:** Expects 2 bytes. Calls `updateHeadingHoldTarget()`. + +## `MSP_SET_SERVO_CONFIGURATION (212 / 0xd4)` +**Description:** Sets the configuration for a single servo (legacy format). + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `servoIndex` | `uint8_t` | 1 | Index | Index of the servo to configure (0 to `MAX_SUPPORTED_SERVOS - 1`) | +| `min` | `uint16_t` | 2 | PWM | Minimum servo endpoint | +| `max` | `uint16_t` | 2 | PWM | Maximum servo endpoint | +| `middle` | `uint16_t` | 2 | PWM | Middle/Neutral servo position | +| `rate` | `uint8_t` | 1 | % | Servo rate/scaling | +| `reserved1` | `uint8_t` | 1 | - | Ignored | +| `reserved2` | `uint8_t` | 1 | - | Ignored | +| `legacyForwardChan` | `uint8_t` | 1 | - | Ignored | +| `legacyReversedSources` | `uint32_t` | 4 | - | Ignored | + +**Reply Payload:** **None** + +**Notes:** Expects 15 bytes. Returns error if index is invalid. Calls `servoComputeScalingFactors()`. Superseded by `MSP2_INAV_SET_SERVO_CONFIG`. + +## `MSP_SET_MOTOR (214 / 0xd6)` +**Description:** Sets the disarmed motor values, typically used for motor testing or propeller balancing functions in a configurator. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `motorValues` | `uint16_t[8]` | 16 | PWM | Array of motor values to set when disarmed. Only affects first `MAX_SUPPORTED_MOTORS` entries | + +**Reply Payload:** **None** + +**Notes:** Expects 16 bytes. Modifies the `motor_disarmed` array. These values are *not* saved persistently. + +## `MSP_SET_NAV_CONFIG (215 / 0xd7)` + +**Request Payload:** **None** + +**Reply Payload:** **None** + +## `MSP_SET_3D (217 / 0xd9)` +**Description:** Sets parameters related to 3D/reversible motor operation. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `deadbandLow` | `uint16_t` | 2 | PWM | Sets `reversibleMotorsConfigMutable()->deadband_low` | +| `deadbandHigh` | `uint16_t` | 2 | PWM | Sets `reversibleMotorsConfigMutable()->deadband_high` | +| `neutral` | `uint16_t` | 2 | PWM | Sets `reversibleMotorsConfigMutable()->neutral` | + +**Reply Payload:** **None** + +**Notes:** Expects 6 bytes. Requires reversible motor support. + +## `MSP_SET_RC_DEADBAND (218 / 0xda)` +**Description:** Sets RC input deadband values. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `deadband` | `uint8_t` | 1 | PWM | Sets `rcControlsConfigMutable()->deadband` | +| `yawDeadband` | `uint8_t` | 1 | PWM | Sets `rcControlsConfigMutable()->yaw_deadband` | +| `altHoldDeadband` | `uint8_t` | 1 | PWM | Sets `rcControlsConfigMutable()->alt_hold_deadband` | +| `throttleDeadband` | `uint16_t` | 2 | PWM | Sets `rcControlsConfigMutable()->mid_throttle_deadband` | + +**Reply Payload:** **None** + +**Notes:** Expects 5 bytes. + +## `MSP_SET_RESET_CURR_PID (219 / 0xdb)` +**Description:** Resets the PIDs of the *current* profile to their default values. Does not save. + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Calls `PG_RESET_CURRENT(pidProfile)`. To save, follow with `MSP_EEPROM_WRITE`. + +## `MSP_SET_SENSOR_ALIGNMENT (220 / 0xdc)` +**Description:** Sets sensor alignment (legacy format). + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `gyroAlign` | `uint8_t` | 1 | Ignored | +| `accAlign` | `uint8_t` | 1 | Ignored | +| `magAlign` | `uint8_t` | 1 | Sets `compassConfigMutable()->mag_align` (if `USE_MAG`) | +| `opflowAlign` | `uint8_t` | 1 | Sets `opticalFlowConfigMutable()->opflow_align` (if `USE_OPFLOW`) | + +**Reply Payload:** **None** + +**Notes:** Expects 4 bytes. Use `MSP_SET_BOARD_ALIGNMENT` for primary board orientation. + +## `MSP_SET_LED_STRIP_MODECOLOR (221 / 0xdd)` +**Description:** Sets the color index for a specific LED mode/function combination. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `modeIndex` | `uint8_t` | 1 | Index of the LED mode (`ledModeIndex_e` or `LED_MODE_COUNT` for special) | +| `directionOrSpecialIndex` | `uint8_t` | 1 | Index of the direction (`ledDirectionId_e`) or special color (`ledSpecialColorIds_e`) | +| `colorIndex` | `uint8_t` | 1 | Index of the color to assign from `ledStripConfig()->colors` | + +**Reply Payload:** **None** + +**Notes:** Only available if `USE_LED_STRIP` is defined. Expects 3 bytes. Returns error if setting fails (invalid index). + +## `MSP_SET_ACC_TRIM (239 / 0xef)` +**Description:** Sets the accelerometer trim values (leveling calibration). + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Not implemented in INAV `fc_msp.c`. Use `MSP_ACC_CALIBRATION`. + +## `MSP_ACC_TRIM (240 / 0xf0)` +**Description:** Gets the accelerometer trim values. + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Not implemented in INAV `fc_msp.c`. Calibration data via `MSP_CALIBRATION_DATA`. + +## `MSP_SERVO_MIX_RULES (241 / 0xf1)` +**Description:** Retrieves the custom servo mixer rules (legacy format). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `targetChannel` | `uint8_t` | 1 | Index | Servo output channel index (0-based) | +| `inputSource` | `uint8_t` | 1 | [inputSource_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-inputsource_e) | Enum `inputSource_e` Input source for the mix (RC chan, Roll, Pitch...) | +| `rate` | `int16_t` | 2 | % | Mixing rate/weight (`-1000` to `+1000`, percent with sign) | +| `speed` | `uint8_t` | 1 | 0-255 | Speed/Slew rate limit (`0`=instant, higher slows response) | +| `reserved1` | `uint8_t` | 1 | - | Always 0 | +| `legacyMax` | `uint8_t` | 1 | - | Always 100 (Legacy) | +| `legacyBox` | `uint8_t` | 1 | - | Always 0 (Legacy) | + +**Notes:** Superseded by `MSP2_INAV_SERVO_MIXER`. + +## `MSP_SET_SERVO_MIX_RULE (242 / 0xf2)` +**Description:** Sets a single custom servo mixer rule (legacy format). + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `ruleIndex` | `uint8_t` | 1 | Index | Index of the rule to set (0 to `MAX_SERVO_RULES - 1`) | +| `targetChannel` | `uint8_t` | 1 | Index | Servo output channel index | +| `inputSource` | `uint8_t` | 1 | [inputSource_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-inputsource_e) | Enum `inputSource_e` Input source for the mix | +| `rate` | `int16_t` | 2 | % | Mixing rate/weight (`-1000` to `+1000`, percent with sign) | +| `speed` | `uint8_t` | 1 | 0-255 | Speed/Slew rate limit (`0`=instant, higher slows response) | +| `legacyMinMax` | `uint16_t` | 2 | - | Ignored | +| `legacyBox` | `uint8_t` | 1 | - | Ignored | + +**Reply Payload:** **None** + +**Notes:** Expects 9 bytes. Returns error if index invalid. Calls `loadCustomServoMixer()`. Superseded by `MSP2_INAV_SET_SERVO_MIXER`. + +## `MSP_SET_PASSTHROUGH (245 / 0xf5)` +**Description:** Enables serial passthrough mode to peripherals like ESCs (BLHeli 4-way) or other serial devices. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `status` | `uint8_t` | 1 | 1 if passthrough started successfully, 0 on error (e.g., port not found). For 4way, returns number of ESCs found | + +**Notes:** Accepts 0 bytes (defaults to ESC 4-way) or up to 2 bytes for mode/argument. If successful, sets `mspPostProcessFn` to the appropriate handler (`mspSerialPassthroughFn` or `esc4wayProcess`). This handler takes over the serial port after the reply is sent. Requires `USE_SERIAL_4WAY_BLHELI_INTERFACE` for ESC passthrough. + +## `MSP_RTC (246 / 0xf6)` +**Description:** Retrieves the current Real-Time Clock time. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `seconds` | `int32_t` | 4 | Seconds | Seconds since epoch (or relative time if not set). 0 if RTC time unknown | +| `millis` | `uint16_t` | 2 | Milliseconds | Millisecond part of the time. 0 if RTC time unknown | + +**Notes:** Requires RTC hardware/support. Returns (0, 0) if time is not available/set. + +## `MSP_SET_RTC (247 / 0xf7)` +**Description:** Sets the Real-Time Clock time. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `seconds` | `int32_t` | 4 | Seconds | Seconds component of time to set | +| `millis` | `uint16_t` | 2 | Milliseconds | Millisecond component of time to set | + +**Reply Payload:** **None** + +**Notes:** Requires RTC hardware/support. Expects 6 bytes. Uses `rtcSet()`. + +## `MSP_EEPROM_WRITE (250 / 0xfa)` +**Description:** Saves the current configuration from RAM to non-volatile memory (EEPROM/Flash). + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Will fail if armed. Suspends RX, calls `writeEEPROM()`, `readEEPROM()`, resumes RX. + +## `MSP_RESERVE_1 (251 / 0xfb)` + +**Request Payload:** **None** + +**Reply Payload:** **None** + +## `MSP_RESERVE_2 (252 / 0xfc)` + +**Request Payload:** **None** + +**Reply Payload:** **None** + +## `MSP_DEBUGMSG (253 / 0xfd)` +**Description:** Retrieves debug ("serial printf") messages from the firmware. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `Message Text` | `char[]` | array | Debug message text (not NUL-terminated). See [serial printf debugging](https://github.com/iNavFlight/inav/blob/master/docs/development/serial_printf_debugging.md) | + +**Notes:** Published via the LOG UART or shared MSP/LOG port using `mspSerialPushPort()`. + +## `MSP_DEBUG (254 / 0xfe)` +**Description:** Retrieves values from the firmware's `debug[]` array (legacy 16-bit version). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `debugValues` | `uint16_t[4]` | 8 | First 4 values from the `debug` array | + +**Notes:** Useful for developers. Values are truncated to the lower 16 bits of each `debug[]` entry. See `MSP2_INAV_DEBUG` for full 32-bit values. + +## `MSP_V2_FRAME (255 / 0xff)` +**Description:** This ID is used as a *payload indicator* within an MSPv1 message structure (`$M>`) to signify that the following payload conforms to the MSPv2 format. It's not a command itself. + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** See MSPv2 documentation for the actual frame structure that follows this indicator. + +## `MSP2_COMMON_TZ (4097 / 0x1001)` +**Description:** Gets the time zone offset configuration. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `tzOffsetMinutes` | `int16_t` | 2 | Minutes | Time zone offset from UTC (`timeConfig()->tz_offset`) | +| `tzAutoDst` | `uint8_t` | 1 | Boolean | Automatic daylight saving time enabled (`timeConfig()->tz_automatic_dst`) | + +## `MSP2_COMMON_SET_TZ (4098 / 0x1002)` +**Description:** Sets the time zone offset configuration. +#### Variant: `dataSize == 2` + +**Description:** dataSize == 2 + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `tz_offset` | `int16_t` | 2 | minutes | Timezone offset from UTC. | + +**Reply Payload:** **None** + +#### Variant: `dataSize == 3` + +**Description:** dataSize == 3 + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `tz_offset` | `int16_t` | 2 | minutes | Timezone offset from UTC. | +| `tz_automatic_dst` | `uint8_t` | 1 | bool | Automatic DST enable (0/1). | + +**Reply Payload:** **None** + + +**Notes:** Accepts 2 or 3 bytes. + +## `MSP2_COMMON_SETTING (4099 / 0x1003)` +**Description:** Gets the value of a specific configuration setting, identified by name or index. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `settingIdentifier` | `Varies` | - | Setting name (null-terminated string) OR index selector (`0x00` followed by `uint16_t` index) | + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `settingValue` | `uint8_t[]` | array | Raw byte value of the setting. Size depends on the setting's type (`settingGetValueSize()`) | + +**Notes:** Returns error if setting not found. Use `MSP2_COMMON_SETTING_INFO` to discover settings, types, and sizes. + +## `MSP2_COMMON_SET_SETTING (4100 / 0x1004)` +**Description:** Sets the value of a specific configuration setting, identified by name or index. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `settingIdentifier` | `Varies` | - | Setting name (null-terminated string) OR Index (0x00 followed by `uint16_t` index) | +| `settingValue` | `uint8_t[]` | array | Raw byte value to set for the setting. Size must match the setting's type | + +**Reply Payload:** **None** + +**Notes:** Performs type checking and range validation (min/max). Returns error if setting not found, value size mismatch, or value out of range. Handles different data types (`uint8`, `int16`, `float`, `string`, etc.) internally. + +## `MSP2_COMMON_MOTOR_MIXER (4101 / 0x1005)` +**Description:** Retrieves the current motor mixer configuration (throttle, roll, pitch, yaw weights) for each motor. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `motorMix` | `uint16_t[4]` | 8 | Scaled (0-4000) | Weights for a single motor `[throttle, roll, pitch, yaw]`, each encoded as `(mix + 2.0) * 1000` (range 0-4000) | + +**Notes:** Scaling is `(float_weight + 2.0) * 1000`. `primaryMotorMixer()` provides the data. If multiple mixer profiles are enabled (`MAX_MIXER_PROFILE_COUNT > 1`), an additional block of mixes for the next profile follows immediately. + +## `MSP2_COMMON_SET_MOTOR_MIXER (4102 / 0x1006)` +**Description:** Sets the motor mixer weights for a single motor in the primary mixer profile. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `motorIndex` | `uint8_t` | 1 | Index | Index of the motor to configure (0 to `MAX_SUPPORTED_MOTORS - 1`) | +| `throttleWeight` | `uint16_t` | 2 | Scaled (0-4000) | Sets throttle weight from `(value / 1000.0) - 2.0 | +| `rollWeight` | `uint16_t` | 2 | Scaled (0-4000) | Sets roll weight from `(value / 1000.0) - 2.0 | +| `pitchWeight` | `uint16_t` | 2 | Scaled (0-4000) | Sets pitch weight from `(value / 1000.0) - 2.0 | +| `yawWeight` | `uint16_t` | 2 | Scaled (0-4000) | Sets yaw weight from `(value / 1000.0) - 2.0 | + +**Reply Payload:** **None** + +**Notes:** Expects 9 bytes. Modifies `primaryMotorMixerMutable()`. Returns error if index is invalid. + +## `MSP2_COMMON_SETTING_INFO (4103 / 0x1007)` +**Description:** Gets detailed information about a specific configuration setting (name, type, range, flags, current value, etc.). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `settingName` | `char[]` | array | Null-terminated setting name | +| `pgn` | `uint16_t` | 2 | Parameter Group Number (PGN) ID | +| `type` | `uint8_t` | 1 | Variable type (`VAR_UINT8`, `VAR_FLOAT`, etc.) | +| `section` | `uint8_t` | 1 | Setting section (`MASTER_VALUE`, `PROFILE_VALUE`, etc.) | +| `mode` | `uint8_t` | 1 | Setting mode (`MODE_NORMAL`, `MODE_LOOKUP`, etc.) | +| `minValue` | `int32_t` | 4 | Minimum allowed value (as signed 32-bit) | +| `maxValue` | `uint32_t` | 4 | Maximum allowed value (as unsigned 32-bit) | +| `settingIndex` | `uint16_t` | 2 | Absolute index of the setting | +| `profileIndex` | `uint8_t` | 1 | Current profile index (if applicable, else 0) | +| `profileCount` | `uint8_t` | 1 | Total number of profiles (if applicable, else 0) | +| `lookupNames` | `char[]` | array | (If `mode == MODE_LOOKUP`) Series of null-terminated strings for each possible value from min to max | +| `settingValue` | `uint8_t[]` | array | Current raw byte value of the setting | + +## `MSP2_COMMON_PG_LIST (4104 / 0x1008)` +**Description:** Gets a list of Parameter Group Numbers (PGNs) used by settings, along with the start and end setting indexes for each group. Can request info for a single PGN. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `pgn` | `uint16_t` | 2 | (Optional) PGN ID to query. If omitted, returns all used PGNs | + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `pgn` | `uint16_t` | 2 | Parameter Group Number (PGN) ID | +| `startIndex` | `uint16_t` | 2 | Absolute index of the first setting in this group | +| `endIndex` | `uint16_t` | 2 | Absolute index of the last setting in this group | + +**Notes:** Allows efficient fetching of related settings by group. + +## `MSP2_COMMON_SERIAL_CONFIG (4105 / 0x1009)` +**Description:** Retrieves the configuration for all available serial ports. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `identifier` | `uint8_t` | 1 | [serialPortIdentifier_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-serialportidentifier_e) | Port identifier Enum (`serialPortIdentifier_e`) | +| `functionMask` | `uint32_t` | 4 | Bitmask | Bitmask: enabled functions (`FUNCTION_*`) | +| `mspBaudIndex` | `uint8_t` | 1 | - | Baud rate index for MSP function | +| `gpsBaudIndex` | `uint8_t` | 1 | - | Baud rate index for GPS function | +| `telemetryBaudIndex` | `uint8_t` | 1 | - | Baud rate index for Telemetry function | +| `peripheralBaudIndex` | `uint8_t` | 1 | - | Baud rate index for other peripheral functions | + +**Notes:** Baud rate indexes map to actual baud rates (e.g., 9600, 115200). See `baudRates` array. + +## `MSP2_COMMON_SET_SERIAL_CONFIG (4106 / 0x100a)` +**Description:** Sets the configuration for one or more serial ports. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `identifier` | `uint8_t` | 1 | [serialPortIdentifier_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-serialportidentifier_e) | Port identifier Enum (`serialPortIdentifier_e`) | +| `functionMask` | `uint32_t` | 4 | Bitmask | Bitmask: functions to enable | +| `mspBaudIndex` | `uint8_t` | 1 | - | Baud rate index for MSP | +| `gpsBaudIndex` | `uint8_t` | 1 | - | Baud rate index for GPS | +| `telemetryBaudIndex` | `uint8_t` | 1 | - | Baud rate index for Telemetry | +| `peripheralBaudIndex` | `uint8_t` | 1 | - | Baud rate index for peripherals | + +**Reply Payload:** **None** + +**Notes:** Payload size must be a multiple of the size of one port config entry (1 + 4 + 4 = 9 bytes). Returns error if identifier is invalid or size is incorrect. Baud rate indexes are constrained `BAUD_MIN` to `BAUD_MAX`. + +## `MSP2_COMMON_SET_RADAR_POS (4107 / 0x100b)` +**Description:** Sets the position and status information for a "radar" Point of Interest (POI). Used for displaying other craft/objects on the OSD map. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `poiIndex` | `uint8_t` | 1 | Index | Index of the POI slot (0 to `RADAR_MAX_POIS - 1`) | +| `state` | `uint8_t` | 1 | - | Status of the POI (0=undefined, 1=armed, 2=lost) | +| `latitude` | `int32_t` | 4 | deg * 1e7 | Latitude of the POI | +| `longitude` | `int32_t` | 4 | deg * 1e7 | Longitude of the POI | +| `altitude` | `int32_t` | 4 | cm | Altitude of the POI | +| `heading` | `uint16_t` | 2 | degrees | Heading of the POI | +| `speed` | `uint16_t` | 2 | cm/s | Speed of the POI | +| `linkQuality` | `uint8_t` | 1 | 0-4 | Link quality indicator | + +**Reply Payload:** **None** + +**Notes:** Expects 19 bytes. POI index is clamped to `RADAR_MAX_POIS - 1`. Updates the `radar_pois` array. + +## `MSP2_COMMON_SET_RADAR_ITD (4108 / 0x100c)` +**Description:** Sets radar information to display (likely internal/unused). + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Not implemented in INAV `fc_msp.c`. + +## `MSP2_COMMON_SET_MSP_RC_LINK_STATS (4109 / 0x100d)` +**Description:** Provides RC link statistics (RSSI, LQ) to the FC, typically from an MSP-based RC link (like ExpressLRS). Sent periodically by the RC link. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `sublinkID` | `uint8_t` | 1 | - | Sublink identifier (usually 0) | +| `validLink` | `uint8_t` | 1 | Boolean | Indicates if the link is currently valid (not in failsafe) | +| `rssiPercent` | `uint8_t` | 1 | % | Uplink RSSI percentage (0-100) | +| `uplinkRSSI_dBm` | `uint8_t` | 1 | -dBm | Uplink RSSI in dBm (sent as positive, e.g., 70 means -70dBm) | +| `downlinkLQ` | `uint8_t` | 1 | % | Downlink Link Quality (0-100) | +| `uplinkLQ` | `uint8_t` | 1 | % | Uplink Link Quality (0-100) | +| `uplinkSNR` | `int8_t` | 1 | dB | Uplink Signal-to-Noise Ratio | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_RX_MSP`. Expects at least 7 bytes. Updates `rxLinkStatistics` and sets RSSI via `setRSSIFromMSP_RC()` only if `sublinkID` is 0. This message expects **no reply** (`MSP_RESULT_NO_REPLY`). + +## `MSP2_COMMON_SET_MSP_RC_INFO (4110 / 0x100e)` +**Description:** Provides additional RC link information (power levels, band, mode) to the FC from an MSP-based RC link. Sent less frequently than link stats. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `sublinkID` | `uint8_t` | 1 | - | Sublink identifier (usually 0) | +| `uplinkTxPower` | `uint16_t` | 2 | mW | Uplink transmitter power level | +| `downlinkTxPower` | `uint16_t` | 2 | mW | Downlink transmitter power level | +| `band` | `char[4]` | 4 | - | Operating band string (e.g., "2G4", "900"), null-terminated/padded | +| `mode` | `char[6]` | 6 | - | Operating mode/rate string (e.g., "100HZ", "F1000"), null-terminated/padded | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_RX_MSP`. Expects at least 15 bytes. Updates `rxLinkStatistics` only if `sublinkID` is 0. Converts band/mode strings to uppercase. This message expects **no reply** (`MSP_RESULT_NO_REPLY`). + +## `MSP2_COMMON_GET_RADAR_GPS (4111 / 0x100f)` +**Description:** Provides the GPS positions (latitude, longitude, altitude) for each radar point of interest. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `poiLatitude` | `int32_t` | 4 | deg * 1e7 | Latitude of a radar POI | +| `poiLongitude` | `int32_t` | 4 | deg * 1e7 | Longitude of a radar POI | +| `poiAltitude` | `int32_t` | 4 | cm | Altitude of a radar POI | + +**Notes:** Returns the stored GPS coordinates for all radar POIs (`radar_pois[i].gps`). + +## `MSP2_SENSOR_RANGEFINDER (7937 / 0x1f01)` +**Description:** Provides rangefinder data (distance, quality) from an external MSP-based sensor. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `quality` | `uint8_t` | 1 | 0-255 | Quality of the measurement | +| `distanceMm` | `int32_t` | 4 | mm | Measured distance. Negative value indicates out of range | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_RANGEFINDER_MSP`. Calls `mspRangefinderReceiveNewData()`. + +## `MSP2_SENSOR_OPTIC_FLOW (7938 / 0x1f02)` +**Description:** Provides optical flow data (motion, quality) from an external MSP-based sensor. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `quality` | `uint8_t` | 1 | Quality of the measurement (0-255) | +| `motionX` | `int32_t` | 4 | Raw integrated flow value X | +| `motionY` | `int32_t` | 4 | Raw integrated flow value Y | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_OPFLOW_MSP`. Calls `mspOpflowReceiveNewData()`. + +## `MSP2_SENSOR_GPS (7939 / 0x1f03)` +**Description:** Provides detailed GPS data from an external MSP-based GPS module. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `instance` | `uint8_t` | 1 | - | Sensor instance number (for multi-GPS) | +| `gpsWeek` | `uint16_t` | 2 | - | GPS week number (0xFFFF if unavailable) | +| `msTOW` | `uint32_t` | 4 | ms | Milliseconds Time of Week | +| `fixType` | `uint8_t` | 1 | [gpsFixType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-gpsfixtype_e) | Enum `gpsFixType_e` Type of GPS fix | +| `satellitesInView` | `uint8_t` | 1 | Count | Number of satellites used in solution | +| `hPosAccuracy` | `uint16_t` | 2 | mm | Horizontal position accuracy estimate in milimeters | +| `vPosAccuracy` | `uint16_t` | 2 | mm | Vertical position accuracy estimate in milimeters | +| `hVelAccuracy` | `uint16_t` | 2 | cm/s | Horizontal velocity accuracy estimate | +| `hdop` | `uint16_t` | 2 | HDOP * 100 | Horizontal Dilution of Precision | +| `longitude` | `int32_t` | 4 | deg * 1e7 | Longitude | +| `latitude` | `int32_t` | 4 | deg * 1e7 | Latitude | +| `mslAltitude` | `int32_t` | 4 | cm | Altitude above Mean Sea Level | +| `nedVelNorth` | `int32_t` | 4 | cm/s | North velocity (NED frame) | +| `nedVelEast` | `int32_t` | 4 | cm/s | East velocity (NED frame) | +| `nedVelDown` | `int32_t` | 4 | cm/s | Down velocity (NED frame) | +| `groundCourse` | `uint16_t` | 2 | deg * 100 | Ground course (0-36000) | +| `trueYaw` | `uint16_t` | 2 | deg * 100 | True heading/yaw (0-36000, 65535 if unavailable) | +| `year` | `uint16_t` | 2 | - | Year (e.g., 2023) | +| `month` | `uint8_t` | 1 | - | Month (1-12) | +| `day` | `uint8_t` | 1 | - | Day of month (1-31) | +| `hour` | `uint8_t` | 1 | - | Hour (0-23) | +| `min` | `uint8_t` | 1 | - | Minute (0-59) | +| `sec` | `uint8_t` | 1 | - | Second (0-59) | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_GPS_PROTO_MSP`. Calls `mspGPSReceiveNewData()`. + +## `MSP2_SENSOR_COMPASS (7940 / 0x1f04)` +**Description:** Provides magnetometer data from an external MSP-based compass module. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `instance` | `uint8_t` | 1 | - | Sensor instance number | +| `timeMs` | `uint32_t` | 4 | ms | Timestamp from the sensor | +| `magX` | `int16_t` | 2 | mGauss | Front component reading | +| `magY` | `int16_t` | 2 | mGauss | Right component reading | +| `magZ` | `int16_t` | 2 | mGauss | Down component reading | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_MAG_MSP`. Calls `mspMagReceiveNewData()`. + +## `MSP2_SENSOR_BAROMETER (7941 / 0x1f05)` +**Description:** Provides barometer data from an external MSP-based barometer module. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `instance` | `uint8_t` | 1 | - | Sensor instance number | +| `timeMs` | `uint32_t` | 4 | ms | Timestamp from the sensor | +| `pressurePa` | `float` | 4 | Pa | Absolute pressure | +| `temp` | `int16_t` | 2 | 0.01 deg C | Temperature | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_BARO_MSP`. Calls `mspBaroReceiveNewData()`. + +## `MSP2_SENSOR_AIRSPEED (7942 / 0x1f06)` +**Description:** Provides airspeed data from an external MSP-based pitot sensor module. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `instance` | `uint8_t` | 1 | - | Sensor instance number | +| `timeMs` | `uint32_t` | 4 | ms | Timestamp from the sensor | +| `diffPressurePa` | `float` | 4 | Pa | Differential pressure | +| `temp` | `int16_t` | 2 | 0.01 deg C | Temperature | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_PITOT_MSP`. Calls `mspPitotmeterReceiveNewData()`. + +## `MSP2_SENSOR_HEADTRACKER (7943 / 0x1f07)` +**Description:** Provides head tracker orientation data. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `...` | `Varies` | - | Head tracker angles (e.g., int16 Roll, Pitch, Yaw in deci-degrees) | | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_HEADTRACKER` and `USE_HEADTRACKER_MSP`. Calls `mspHeadTrackerReceiverNewData()`. Payload structure needs verification from `mspHeadTrackerReceiverNewData` implementation. + +## `MSP2_INAV_STATUS (8192 / 0x2000)` +**Description:** Provides comprehensive flight controller status, extending `MSP_STATUS_EX` with full arming flags, battery profile, and mixer profile. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `cycleTime` | `uint16_t` | 2 | µs | Main loop cycle time | +| `i2cErrors` | `uint16_t` | 2 | Count | I2C errors | +| `sensorStatus` | `uint16_t` | 2 | Bitmask | Bitmask: Sensor status | +| `cpuLoad` | `uint16_t` | 2 | % | Average system load percentage | +| `profileAndBattProfile` | `uint8_t` | 1 | Packed | Bits 0-3: Config profile index (`getConfigProfile()`), Bits 4-7: Battery profile index (`getConfigBatteryProfile()`) | +| `armingFlags` | `uint32_t` | 4 | Bitmask | Bitmask: Full 32-bit flight controller arming flags (`armingFlags`) | +| `activeModes` | `boxBitmask_t` | - | Bitmask | Bitmask words for active flight modes (`packBoxModeFlags()`) | +| `mixerProfile` | `uint8_t` | 1 | Index | Current mixer profile index (`getConfigMixerProfile()`) | + +**Notes:** `sensorStatus` bits follow `packSensorStatus()` (bit 15 indicates hardware failure). `profileAndBattProfile` packs the current config profile in the low nibble and the battery profile in the high nibble. `activeModes` is emitted as a little-endian array of 32-bit words sized to `CHECKBOX_ITEM_COUNT`. + +## `MSP2_INAV_OPTICAL_FLOW (8193 / 0x2001)` +**Description:** Provides data from the optical flow sensor. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `quality` | `uint8_t` | 1 | 0-255 | Raw quality indicator from the sensor (`opflow.rawQuality`). 0 if `USE_OPFLOW` disabled | +| `flowRateX` | `int16_t` | 2 | degrees/s | Optical flow rate X (roll axis) (`RADIANS_TO_DEGREES(opflow.flowRate[X])`). 0 if `USE_OPFLOW` disabled | +| `flowRateY` | `int16_t` | 2 | degrees/s | Optical flow rate Y (pitch axis) (`RADIANS_TO_DEGREES(opflow.flowRate[Y])`). 0 if `USE_OPFLOW` disabled | +| `bodyRateX` | `int16_t` | 2 | degrees/s | Compensated body rate X (roll axis) (`RADIANS_TO_DEGREES(opflow.bodyRate[X])`). 0 if `USE_OPFLOW` disabled | +| `bodyRateY` | `int16_t` | 2 | degrees/s | Compensated body rate Y (pitch axis) (`RADIANS_TO_DEGREES(opflow.bodyRate[Y])`). 0 if `USE_OPFLOW` disabled | + +**Notes:** Requires `USE_OPFLOW`. + +## `MSP2_INAV_ANALOG (8194 / 0x2002)` +**Description:** Provides detailed analog sensor readings, superseding `MSP_ANALOG` with higher precision and additional fields. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `batteryFlags` | `uint8_t` | 1 | Bitmask | Bitmask: Bit0=Full on plug-in, Bit1=Use capacity thresholds, Bits2-3=`batteryState_e` (`getBatteryState()`), Bits4-7=Cell count (`getBatteryCellCount()`) | +| `vbat` | `uint16_t` | 2 | 0.01V | Battery voltage (`getBatteryVoltage()`) | +| `amperage` | `int16_t` | 2 | 0.01A | Current draw (`getAmperage()`) | +| `powerDraw` | `uint32_t` | 4 | 0.01W | Power draw (`getPower()`) | +| `mAhDrawn` | `uint32_t` | 4 | mAh | Consumed capacity (`getMAhDrawn()`) | +| `mWhDrawn` | `uint32_t` | 4 | mWh | Consumed energy (`getMWhDrawn()`) | +| `remainingCapacity` | `uint32_t` | 4 | Capacity unit (`batteryMetersConfig()->capacity_unit`) | Estimated remaining capacity (`getBatteryRemainingCapacity()`) | +| `percentageRemaining` | `uint8_t` | 1 | % | Estimated remaining capacity percentage (`calculateBatteryPercentage()`) | +| `rssi` | `uint16_t` | 2 | Raw (0-1023) | RSSI value (`getRSSI()`) | + +**Notes:** Requires `USE_CURRENT_METER`/`USE_ADC` for current-related fields; values fall back to zero when unavailable. Capacity fields are reported in the units configured by `batteryMetersConfig()->capacity_unit` (mAh or mWh). + +## `MSP2_INAV_MISC (8195 / 0x2003)` +**Description:** Retrieves miscellaneous configuration settings, superseding `MSP_MISC` with higher precision and capacity fields. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `midRc` | `uint16_t` | 2 | PWM | Mid RC value (`PWM_RANGE_MIDDLE`) | +| `legacyMinThrottle` | `uint16_t` | 2 | - | Always 0 (Legacy) | +| `maxThrottle` | `uint16_t` | 2 | PWM | Maximum throttle command (`getMaxThrottle()`) | +| `minCommand` | `uint16_t` | 2 | PWM | Minimum motor command (`motorConfig()->mincommand`) | +| `failsafeThrottle` | `uint16_t` | 2 | PWM | Failsafe throttle level (`currentBatteryProfile->failsafe_throttle`) | +| `gpsType` | `uint8_t` | 1 | [gpsProvider_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-gpsprovider_e) | Enum `gpsProvider_e` GPS provider type (`gpsConfig()->provider`). 0 if `USE_GPS` disabled | +| `legacyGpsBaud` | `uint8_t` | 1 | - | Always 0 (Legacy) | +| `gpsSbasMode` | `uint8_t` | 1 | [sbasMode_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-sbasmode_e) | Enum `sbasMode_e` GPS SBAS mode (`gpsConfig()->sbasMode`). 0 if `USE_GPS` disabled | +| `rssiChannel` | `uint8_t` | 1 | Index | RSSI channel index (1-based, 0 disables) (`rxConfig()->rssi_channel`) | +| `magDeclination` | `int16_t` | 2 | 0.1 degrees | Magnetic declination / 10 (`compassConfig()->mag_declination / 10`). 0 if `USE_MAG` disabled | +| `vbatScale` | `uint16_t` | 2 | Scale | Voltage scale (`batteryMetersConfig()->voltage.scale`). 0 if `USE_ADC` disabled | +| `vbatSource` | `uint8_t` | 1 | [batVoltageSource_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-batvoltagesource_e) | Enum `batVoltageSource_e` Voltage source (`batteryMetersConfig()->voltageSource`). 0 if `USE_ADC` disabled | +| `cellCount` | `uint8_t` | 1 | Count | Configured cell count (`currentBatteryProfile->cells`). 0 if `USE_ADC` disabled | +| `vbatCellDetect` | `uint16_t` | 2 | 0.01V | Cell detection voltage (`currentBatteryProfile->voltage.cellDetect`). 0 if `USE_ADC` disabled | +| `vbatMinCell` | `uint16_t` | 2 | 0.01V | Min cell voltage (`currentBatteryProfile->voltage.cellMin`). 0 if `USE_ADC` disabled | +| `vbatMaxCell` | `uint16_t` | 2 | 0.01V | Max cell voltage (`currentBatteryProfile->voltage.cellMax`). 0 if `USE_ADC` disabled | +| `vbatWarningCell` | `uint16_t` | 2 | 0.01V | Warning cell voltage (`currentBatteryProfile->voltage.cellWarning`). 0 if `USE_ADC` disabled | +| `capacityValue` | `uint32_t` | 4 | mAh/mWh | Battery capacity (`currentBatteryProfile->capacity.value`) | +| `capacityWarning` | `uint32_t` | 4 | mAh/mWh | Capacity warning threshold (`currentBatteryProfile->capacity.warning`) | +| `capacityCritical` | `uint32_t` | 4 | mAh/mWh | Capacity critical threshold (`currentBatteryProfile->capacity.critical`) | +| `capacityUnit` | `uint8_t` | 1 | [batCapacityUnit_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-batcapacityunit_e) | Enum `batCapacityUnit_e` Capacity unit (`batteryMetersConfig()->capacity_unit`) | + +## `MSP2_INAV_SET_MISC (8196 / 0x2004)` +**Description:** Sets miscellaneous configuration settings, superseding `MSP_SET_MISC`. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `midRc` | `uint16_t` | 2 | PWM | Ignored | +| `legacyMinThrottle` | `uint16_t` | 2 | - | Ignored | +| `legacyMaxThrottle` | `uint16_t` | 2 | - | Ignored | +| `minCommand` | `uint16_t` | 2 | PWM | Sets `motorConfigMutable()->mincommand` (constrained) | +| `failsafeThrottle` | `uint16_t` | 2 | PWM | Sets `currentBatteryProfileMutable->failsafe_throttle` (constrained) | +| `gpsType` | `uint8_t` | 1 | [gpsProvider_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-gpsprovider_e) | Enum `gpsProvider_e` Sets `gpsConfigMutable()->provider` (if `USE_GPS`) | +| `legacyGpsBaud` | `uint8_t` | 1 | - | Ignored | +| `gpsSbasMode` | `uint8_t` | 1 | [sbasMode_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-sbasmode_e) | Enum `sbasMode_e` Sets `gpsConfigMutable()->sbasMode` (if `USE_GPS`) | +| `rssiChannel` | `uint8_t` | 1 | Index | Sets `rxConfigMutable()->rssi_channel` (1-based, 0 disables) when <= `MAX_SUPPORTED_RC_CHANNEL_COUNT` | +| `magDeclination` | `int16_t` | 2 | 0.1 degrees | Sets `compassConfigMutable()->mag_declination = value * 10` (if `USE_MAG`) | +| `vbatScale` | `uint16_t` | 2 | Scale | Sets `batteryMetersConfigMutable()->voltage.scale` (if `USE_ADC`) | +| `vbatSource` | `uint8_t` | 1 | [batVoltageSource_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-batvoltagesource_e) | Enum `batVoltageSource_e` Sets `batteryMetersConfigMutable()->voltageSource` (if `USE_ADC`, validated) | +| `cellCount` | `uint8_t` | 1 | Count | Sets `currentBatteryProfileMutable->cells` (if `USE_ADC`) | +| `vbatCellDetect` | `uint16_t` | 2 | 0.01V | Sets `currentBatteryProfileMutable->voltage.cellDetect` (if `USE_ADC`) | +| `vbatMinCell` | `uint16_t` | 2 | 0.01V | Sets `currentBatteryProfileMutable->voltage.cellMin` (if `USE_ADC`) | +| `vbatMaxCell` | `uint16_t` | 2 | 0.01V | Sets `currentBatteryProfileMutable->voltage.cellMax` (if `USE_ADC`) | +| `vbatWarningCell` | `uint16_t` | 2 | 0.01V | Sets `currentBatteryProfileMutable->voltage.cellWarning` (if `USE_ADC`) | +| `capacityValue` | `uint32_t` | 4 | mAh/mWh | Sets `currentBatteryProfileMutable->capacity.value` | +| `capacityWarning` | `uint32_t` | 4 | mAh/mWh | Sets `currentBatteryProfileMutable->capacity.warning` | +| `capacityCritical` | `uint32_t` | 4 | mAh/mWh | Sets `currentBatteryProfileMutable->capacity.critical` | +| `capacityUnit` | `uint8_t` | 1 | [batCapacityUnit_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-batcapacityunit_e) | Enum `batCapacityUnit_e` Sets `batteryMetersConfigMutable()->capacity_unit` (validated, updates OSD energy unit if changed) | + +**Reply Payload:** **None** + +**Notes:** Expects 41 bytes. Performs validation on `vbatSource` and `capacityUnit`. + +## `MSP2_INAV_BATTERY_CONFIG (8197 / 0x2005)` +**Description:** Retrieves the configuration specific to the battery voltage and current sensors and capacity settings for the current battery profile. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `vbatScale` | `uint16_t` | 2 | Scale | Voltage scale (`batteryMetersConfig()->voltage.scale`) | +| `vbatSource` | `uint8_t` | 1 | [batVoltageSource_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-batvoltagesource_e) | Enum `batVoltageSource_e` Voltage source (`batteryMetersConfig()->voltageSource`) | +| `cellCount` | `uint8_t` | 1 | Count | Configured cell count (`currentBatteryProfile->cells`) | +| `vbatCellDetect` | `uint16_t` | 2 | 0.01V | Cell detection voltage (`currentBatteryProfile->voltage.cellDetect`) | +| `vbatMinCell` | `uint16_t` | 2 | 0.01V | Min cell voltage (`currentBatteryProfile->voltage.cellMin`) | +| `vbatMaxCell` | `uint16_t` | 2 | 0.01V | Max cell voltage (`currentBatteryProfile->voltage.cellMax`) | +| `vbatWarningCell` | `uint16_t` | 2 | 0.01V | Warning cell voltage (`currentBatteryProfile->voltage.cellWarning`) | +| `currentOffset` | `int16_t` | 2 | mV | Current sensor offset (`batteryMetersConfig()->current.offset`) | +| `currentScale` | `int16_t` | 2 | 0.1 mV/A | Current sensor scale (`batteryMetersConfig()->current.scale`) | +| `capacityValue` | `uint32_t` | 4 | mAh/mWh | Battery capacity (`currentBatteryProfile->capacity.value`) | +| `capacityWarning` | `uint32_t` | 4 | mAh/mWh | Capacity warning threshold (`currentBatteryProfile->capacity.warning`) | +| `capacityCritical` | `uint32_t` | 4 | mAh/mWh | Capacity critical threshold (`currentBatteryProfile->capacity.critical`) | +| `capacityUnit` | `uint8_t` | 1 | [batCapacityUnit_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-batcapacityunit_e) | Enum `batCapacityUnit_e` Capacity unit (`batteryMetersConfig()->capacity_unit`) | + +**Notes:** Fields are 0 if `USE_ADC` is not defined. + +## `MSP2_INAV_SET_BATTERY_CONFIG (8198 / 0x2006)` +**Description:** Sets the battery voltage/current sensor configuration and capacity settings for the current battery profile. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `vbatScale` | `uint16_t` | 2 | Scale | Sets `batteryMetersConfigMutable()->voltage.scale` (if `USE_ADC`) | +| `vbatSource` | `uint8_t` | 1 | [batVoltageSource_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-batvoltagesource_e) | Enum `batVoltageSource_e` Sets `batteryMetersConfigMutable()->voltageSource` (if `USE_ADC`, validated) | +| `cellCount` | `uint8_t` | 1 | Count | Sets `currentBatteryProfileMutable->cells` (if `USE_ADC`) | +| `vbatCellDetect` | `uint16_t` | 2 | 0.01V | Sets `currentBatteryProfileMutable->voltage.cellDetect` (if `USE_ADC`) | +| `vbatMinCell` | `uint16_t` | 2 | 0.01V | Sets `currentBatteryProfileMutable->voltage.cellMin` (if `USE_ADC`) | +| `vbatMaxCell` | `uint16_t` | 2 | 0.01V | Sets `currentBatteryProfileMutable->voltage.cellMax` (if `USE_ADC`) | +| `vbatWarningCell` | `uint16_t` | 2 | 0.01V | Sets `currentBatteryProfileMutable->voltage.cellWarning` (if `USE_ADC`) | +| `currentOffset` | `int16_t` | 2 | mV | Sets `batteryMetersConfigMutable()->current.offset` | +| `currentScale` | `int16_t` | 2 | 0.1 mV/A | Sets `batteryMetersConfigMutable()->current.scale` | +| `capacityValue` | `uint32_t` | 4 | mAh/mWh | Sets `currentBatteryProfileMutable->capacity.value` | +| `capacityWarning` | `uint32_t` | 4 | mAh/mWh | Sets `currentBatteryProfileMutable->capacity.warning` | +| `capacityCritical` | `uint32_t` | 4 | mAh/mWh | Sets `currentBatteryProfileMutable->capacity.critical` | +| `capacityUnit` | `uint8_t` | 1 | [batCapacityUnit_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-batcapacityunit_e) | Enum `batCapacityUnit_e` Sets `batteryMetersConfigMutable()->capacity_unit` (validated, updates OSD energy unit if changed) | + +**Reply Payload:** **None** + +**Notes:** Expects 29 bytes. Performs validation on `vbatSource` and `capacityUnit`. + +## `MSP2_INAV_RATE_PROFILE (8199 / 0x2007)` +**Description:** Retrieves the rates and expos for the current control rate profile, including both stabilized and manual flight modes. Supersedes `MSP_RC_TUNING`. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `throttleMid` | `uint8_t` | 1 | Throttle Midpoint (`currentControlRateProfile->throttle.rcMid8`) | +| `throttleExpo` | `uint8_t` | 1 | Throttle Expo (`currentControlRateProfile->throttle.rcExpo8`) | +| `dynamicThrottlePID` | `uint8_t` | 1 | TPA value (`currentControlRateProfile->throttle.dynPID`) | +| `tpaBreakpoint` | `uint16_t` | 2 | TPA breakpoint (`currentControlRateProfile->throttle.pa_breakpoint`) | +| `stabRcExpo` | `uint8_t` | 1 | Stabilized Roll/Pitch Expo (`currentControlRateProfile->stabilized.rcExpo8`) | +| `stabRcYawExpo` | `uint8_t` | 1 | Stabilized Yaw Expo (`currentControlRateProfile->stabilized.rcYawExpo8`) | +| `stabRollRate` | `uint8_t` | 1 | Stabilized Roll Rate (`currentControlRateProfile->stabilized.rates[FD_ROLL]`) | +| `stabPitchRate` | `uint8_t` | 1 | Stabilized Pitch Rate (`currentControlRateProfile->stabilized.rates[FD_PITCH]`) | +| `stabYawRate` | `uint8_t` | 1 | Stabilized Yaw Rate (`currentControlRateProfile->stabilized.rates[FD_YAW]`) | +| `manualRcExpo` | `uint8_t` | 1 | Manual Roll/Pitch Expo (`currentControlRateProfile->manual.rcExpo8`) | +| `manualRcYawExpo` | `uint8_t` | 1 | Manual Yaw Expo (`currentControlRateProfile->manual.rcYawExpo8`) | +| `manualRollRate` | `uint8_t` | 1 | Manual Roll Rate (`currentControlRateProfile->manual.rates[FD_ROLL]`) | +| `manualPitchRate` | `uint8_t` | 1 | Manual Pitch Rate (`currentControlRateProfile->manual.rates[FD_PITCH]`) | +| `manualYawRate` | `uint8_t` | 1 | Manual Yaw Rate (`currentControlRateProfile->manual.rates[FD_YAW]`) | + +## `MSP2_INAV_SET_RATE_PROFILE (8200 / 0x2008)` +**Description:** Sets the rates and expos for the current control rate profile (stabilized and manual). Supersedes `MSP_SET_RC_TUNING`. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `throttleMid` | `uint8_t` | 1 | Sets `currentControlRateProfile->throttle.rcMid8` | +| `throttleExpo` | `uint8_t` | 1 | Sets `currentControlRateProfile->throttle.rcExpo8` | +| `dynamicThrottlePID` | `uint8_t` | 1 | Sets `currentControlRateProfile->throttle.dynPID` | +| `tpaBreakpoint` | `uint16_t` | 2 | Sets `currentControlRateProfile->throttle.pa_breakpoint` | +| `stabRcExpo` | `uint8_t` | 1 | Sets `currentControlRateProfile->stabilized.rcExpo8` | +| `stabRcYawExpo` | `uint8_t` | 1 | Sets `currentControlRateProfile->stabilized.rcYawExpo8` | +| `stabRollRate` | `uint8_t` | 1 | Sets `currentControlRateProfile->stabilized.rates[FD_ROLL]` (constrained) | +| `stabPitchRate` | `uint8_t` | 1 | Sets `currentControlRateProfile->stabilized.rates[FD_PITCH]` (constrained) | +| `stabYawRate` | `uint8_t` | 1 | Sets `currentControlRateProfile->stabilized.rates[FD_YAW]` (constrained) | +| `manualRcExpo` | `uint8_t` | 1 | Sets `currentControlRateProfile->manual.rcExpo8` | +| `manualRcYawExpo` | `uint8_t` | 1 | Sets `currentControlRateProfile->manual.rcYawExpo8` | +| `manualRollRate` | `uint8_t` | 1 | Sets `currentControlRateProfile->manual.rates[FD_ROLL]` (constrained) | +| `manualPitchRate` | `uint8_t` | 1 | Sets `currentControlRateProfile->manual.rates[FD_PITCH]` (constrained) | +| `manualYawRate` | `uint8_t` | 1 | Sets `currentControlRateProfile->manual.rates[FD_YAW]` (constrained) | + +**Reply Payload:** **None** + +**Notes:** Expects 15 bytes. Constraints applied to rates based on axis. + +## `MSP2_INAV_AIR_SPEED (8201 / 0x2009)` +**Description:** Retrieves the estimated or measured airspeed. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `airspeed` | `uint32_t` | 4 | cm/s | Estimated/measured airspeed (`getAirspeedEstimate()`, cm/s). 0 if unavailable | + +**Notes:** Requires `USE_PITOT`; returns 0 when pitot functionality is not enabled or calibrated. + +## `MSP2_INAV_OUTPUT_MAPPING (8202 / 0x200a)` +**Description:** Retrieves the output mapping configuration (identifies which timer outputs are used for Motors/Servos). Legacy version sending only 8-bit usage flags. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `usageFlags` | `uint8_t` | 1 | Timer usage flags (lower 8 bits of `timerHardware[i].usageFlags`, e.g. `TIM_USE_MOTOR`, `TIM_USE_SERVO`) | + +**Notes:** Superseded by `MSP2_INAV_OUTPUT_MAPPING_EXT2`. Only includes timers *not* used for PPM/PWM input. + +## `MSP2_INAV_MC_BRAKING (8203 / 0x200b)` +**Description:** Retrieves configuration parameters for the multirotor braking mode feature. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `brakingSpeedThreshold` | `uint16_t` | 2 | cm/s | Speed above which braking engages (`navConfig()->mc.braking_speed_threshold`) | +| `brakingDisengageSpeed` | `uint16_t` | 2 | cm/s | Speed below which braking disengages (`navConfig()->mc.braking_disengage_speed`) | +| `brakingTimeout` | `uint16_t` | 2 | ms | Timeout before braking force reduces (`navConfig()->mc.braking_timeout`) | +| `brakingBoostFactor` | `uint8_t` | 1 | % | Boost factor applied during braking (`navConfig()->mc.braking_boost_factor`) | +| `brakingBoostTimeout` | `uint16_t` | 2 | ms | Timeout for the boost factor (`navConfig()->mc.braking_boost_timeout`) | +| `brakingBoostSpeedThreshold` | `uint16_t` | 2 | cm/s | Speed threshold for boost engagement (`navConfig()->mc.braking_boost_speed_threshold`) | +| `brakingBoostDisengageSpeed` | `uint16_t` | 2 | cm/s | Speed threshold for boost disengagement (`navConfig()->mc.braking_boost_disengage_speed`) | +| `brakingBankAngle` | `uint8_t` | 1 | degrees | Maximum bank angle allowed during braking (`navConfig()->mc.braking_bank_angle`) | + +**Notes:** Payload is empty if `USE_MR_BRAKING_MODE` is not defined. + +## `MSP2_INAV_SET_MC_BRAKING (8204 / 0x200c)` +**Description:** Sets configuration parameters for the multirotor braking mode feature. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `brakingSpeedThreshold` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->mc.braking_speed_threshold` | +| `brakingDisengageSpeed` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->mc.braking_disengage_speed` | +| `brakingTimeout` | `uint16_t` | 2 | ms | Sets `navConfigMutable()->mc.braking_timeout` | +| `brakingBoostFactor` | `uint8_t` | 1 | % | Sets `navConfigMutable()->mc.braking_boost_factor` | +| `brakingBoostTimeout` | `uint16_t` | 2 | ms | Sets `navConfigMutable()->mc.braking_boost_timeout` | +| `brakingBoostSpeedThreshold` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->mc.braking_boost_speed_threshold` | +| `brakingBoostDisengageSpeed` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->mc.braking_boost_disengage_speed` | +| `brakingBankAngle` | `uint8_t` | 1 | degrees | Sets `navConfigMutable()->mc.braking_bank_angle` | + +**Reply Payload:** **None** + +**Notes:** Expects 14 bytes. Returns error if `USE_MR_BRAKING_MODE` is not defined. + +## `MSP2_INAV_OUTPUT_MAPPING_EXT (8205 / 0x200d)` +**Description:** Retrieves extended output mapping configuration (timer ID and usage flags). Obsolete, use `MSP2_INAV_OUTPUT_MAPPING_EXT2`. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `timerId` | `uint8_t` | 1 | Hardware timer identifier (e.g., `TIM1`, `TIM2`). Value depends on target | +| `usageFlags` | `uint8_t` | 1 | Timer usage flags (lower 8 bits of `timerHardware[i].usageFlags`, e.g. `TIM_USE_MOTOR`, `TIM_USE_SERVO`) | + +**Notes:** Usage flags are truncated to 8 bits. `timerId` mapping is target-specific. + +## `MSP2_INAV_TIMER_OUTPUT_MODE (8206 / 0x200e)` +**Description:** Reads timer output mode overrides. +#### Variant: `dataSize == 0` + +**Description:** List all timers + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `timerIndex` | `uint8_t` | 1 | - | Timer index | +| `outputMode` | `uint8_t` | 1 | [outputMode_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-outputmode_e) | OUTPUT_MODE_* | + +#### Variant: `dataSize == 1` + +**Description:** Query one timer + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `timerIndex` | `uint8_t` | 1 | 0..HARDWARE_TIMER_DEFINITION_COUNT-1 | + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `timerIndex` | `uint8_t` | 1 | - | Echoed timer index | +| `outputMode` | `uint8_t` | 1 | [outputMode_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-outputmode_e) | OUTPUT_MODE_* | + + +**Notes:** Non-SITL only. HARDWARE_TIMER_DEFINITION_COUNT is target specific. Returns MSP_RESULT_ACK on success, MSP_RESULT_ERROR on invalid timer index. + +## `MSP2_INAV_SET_TIMER_OUTPUT_MODE (8207 / 0x200f)` +**Description:** Set the output mode override for a specific hardware timer. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `timerIndex` | `uint8_t` | 1 | - | Index of the hardware timer definition | +| `outputMode` | `uint8_t` | 1 | [outputMode_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-outputmode_e) | Output mode override (`outputMode_e` enum) to set | + +**Reply Payload:** **None** + +**Notes:** Only available on non-SITL builds. Expects 2 bytes. Returns error if `timerIndex` is invalid. + +## `MSP2_INAV_MIXER (8208 / 0x2010)` +**Description:** Retrieves INAV-specific mixer configuration details. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `motorDirectionInverted` | `uint8_t` | 1 | - | Boolean: 1 if motor direction is reversed globally (`mixerConfig()->motorDirectionInverted`) | +| `reserved1` | `uint8_t` | 1 | - | Always 0 (Was yaw jump prevention limit) | +| `motorStopOnLow` | `uint8_t` | 1 | - | Boolean: 1 if motors stop at minimum throttle (`mixerConfig()->motorstopOnLow`) | +| `platformType` | `uint8_t` | 1 | [flyingPlatformType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-flyingplatformtype_e) | Enum (`mixerConfig()->platformType`) | +| `hasFlaps` | `uint8_t` | 1 | - | Boolean: 1 if the current mixer configuration includes flaps (`mixerConfig()->hasFlaps`) | +| `appliedMixerPreset` | `int16_t` | 2 | [mixerPreset_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-mixerpreset_e) | Enum (`mixerPreset_e`): Mixer preset currently applied (`mixerConfig()->appliedMixerPreset`) WARNING: cannot figure where this is | +| `maxMotors` | `uint8_t` | 1 | - | Constant: Maximum motors supported (`MAX_SUPPORTED_MOTORS`) | +| `maxServos` | `uint8_t` | 1 | - | Constant: Maximum servos supported (`MAX_SUPPORTED_SERVOS`) | + +## `MSP2_INAV_SET_MIXER (8209 / 0x2011)` +**Description:** Sets INAV-specific mixer configuration details. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `motorDirectionInverted` | `uint8_t` | 1 | - | Sets `mixerConfigMutable()->motorDirectionInverted` | +| `reserved1` | `uint8_t` | 1 | - | Ignored | +| `motorStopOnLow` | `uint8_t` | 1 | - | Sets `mixerConfigMutable()->motorstopOnLow` | +| `platformType` | `uint8_t` | 1 | [flyingPlatformType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-flyingplatformtype_e) | Sets `mixerConfigMutable()->platformType` | +| `hasFlaps` | `uint8_t` | 1 | - | Sets `mixerConfigMutable()->hasFlaps` | +| `appliedMixerPreset` | `int16_t` | 2 | - | Sets `mixerConfigMutable()->appliedMixerPreset` | +| `maxMotors` | `uint8_t` | 1 | - | Ignored | +| `maxServos` | `uint8_t` | 1 | - | Ignored | + +**Reply Payload:** **None** + +**Notes:** Expects 9 bytes. Calls `mixerUpdateStateFlags()`. + +## `MSP2_INAV_OSD_LAYOUTS (8210 / 0x2012)` +**Description:** Retrieves OSD layout metadata or item positions for specific layouts/items. +#### Variant: `dataSize == 0` + +**Description:** Query layout/item counts + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `layoutCount` | `uint8_t` | 1 | Number of OSD layouts (`OSD_LAYOUT_COUNT`) | +| `itemCount` | `uint8_t` | 1 | Number of OSD items per layout (`OSD_ITEM_COUNT`) | + +#### Variant: `dataSize == 1` + +**Description:** Fetch all item positions for a layout + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `layoutIndex` | `uint8_t` | 1 | Layout index (0 to `OSD_LAYOUT_COUNT - 1`) | + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `itemPosition` | `uint16_t` | 2 | packed coords | Packed X/Y position (`osdLayoutsConfig()->item_pos[layoutIndex][item]`) | + +#### Variant: `dataSize == 3` + +**Description:** Fetch a single item position + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `layoutIndex` | `uint8_t` | 1 | Layout index (0 to `OSD_LAYOUT_COUNT - 1`) | +| `itemIndex` | `uint16_t` | 2 | OSD item index (0 to `OSD_ITEM_COUNT - 1`) | + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `itemPosition` | `uint16_t` | 2 | packed coords | Packed X/Y position (`osdLayoutsConfig()->item_pos[layoutIndex][itemIndex]`) | + + +**Notes:** Requires `USE_OSD`. Returns `MSP_RESULT_ACK` on success, `MSP_RESULT_ERROR` if indexes are out of range. + +## `MSP2_INAV_OSD_SET_LAYOUT_ITEM (8211 / 0x2013)` +**Description:** Sets the position of a single OSD item within a specific layout. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `layoutIndex` | `uint8_t` | 1 | Index | Index of the OSD layout (0 to `OSD_LAYOUT_COUNT - 1`) | +| `itemIndex` | `uint8_t` | 1 | Index | Index of the OSD item | +| `itemPosition` | `uint16_t` | 2 | Coordinates | Packed X/Y position using `OSD_POS(x, y)` with `OSD_VISIBLE_FLAG` bit | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_OSD`. Expects 4 bytes. Returns error if indexes are invalid. If the modified layout is not the currently active one, it temporarily overrides the active layout for 10 seconds to show the change. Otherwise, triggers a full OSD redraw. + +## `MSP2_INAV_OSD_ALARMS (8212 / 0x2014)` +**Description:** Retrieves OSD alarm threshold settings. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `rssiAlarm` | `uint8_t` | 1 | % | RSSI alarm threshold (`osdConfig()->rssi_alarm`) | +| `timerAlarm` | `uint16_t` | 2 | seconds | Timer alarm threshold (`osdConfig()->time_alarm`) | +| `altAlarm` | `uint16_t` | 2 | meters | Altitude alarm threshold (`osdConfig()->alt_alarm`) | +| `distAlarm` | `uint16_t` | 2 | meters | Distance alarm threshold (`osdConfig()->dist_alarm`) | +| `negAltAlarm` | `uint16_t` | 2 | meters | Negative altitude alarm threshold (`osdConfig()->neg_alt_alarm`) | +| `gForceAlarm` | `uint16_t` | 2 | G * 1000 | G-force alarm threshold (`osdConfig()->gforce_alarm * 1000`) | +| `gForceAxisMinAlarm` | `int16_t` | 2 | G * 1000 | Min G-force per-axis alarm (`osdConfig()->gforce_axis_alarm_min * 1000`) | +| `gForceAxisMaxAlarm` | `int16_t` | 2 | G * 1000 | Max G-force per-axis alarm (`osdConfig()->gforce_axis_alarm_max * 1000`) | +| `currentAlarm` | `uint8_t` | 1 | A | Current draw alarm threshold (`osdConfig()->current_alarm`) | +| `imuTempMinAlarm` | `int16_t` | 2 | degrees C | Min IMU temperature alarm (`osdConfig()->imu_temp_alarm_min`) | +| `imuTempMaxAlarm` | `int16_t` | 2 | degrees C | Max IMU temperature alarm (`osdConfig()->imu_temp_alarm_max`) | +| `baroTempMinAlarm` | `int16_t` | 2 | degrees C | Min Baro temperature alarm (`osdConfig()->baro_temp_alarm_min`). 0 if `USE_BARO` disabled | +| `baroTempMaxAlarm` | `int16_t` | 2 | degrees C | Max Baro temperature alarm (`osdConfig()->baro_temp_alarm_max`). 0 if `USE_BARO` disabled | +| `adsbWarnDistance` | `uint16_t` | 2 | meters | ADSB warning distance (`osdConfig()->adsb_distance_warning`). 0 if `USE_ADSB` disabled | +| `adsbAlertDistance` | `uint16_t` | 2 | meters | ADSB alert distance (`osdConfig()->adsb_distance_alert`). 0 if `USE_ADSB` disabled | + +**Notes:** Requires `USE_OSD`. + +## `MSP2_INAV_OSD_SET_ALARMS (8213 / 0x2015)` +**Description:** Sets OSD alarm threshold settings. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `rssiAlarm` | `uint8_t` | 1 | % | Sets `osdConfigMutable()->rssi_alarm | +| `timerAlarm` | `uint16_t` | 2 | seconds | Sets `osdConfigMutable()->time_alarm | +| `altAlarm` | `uint16_t` | 2 | meters | Sets `osdConfigMutable()->alt_alarm | +| `distAlarm` | `uint16_t` | 2 | meters | Sets `osdConfigMutable()->dist_alarm | +| `negAltAlarm` | `uint16_t` | 2 | meters | Sets `osdConfigMutable()->neg_alt_alarm` | +| `gForceAlarm` | `uint16_t` | 2 | G * 1000 | Sets `osdConfigMutable()->gforce_alarm = value / 1000.0f` | +| `gForceAxisMinAlarm` | `int16_t` | 2 | G * 1000 | Sets `osdConfigMutable()->gforce_axis_alarm_min = value / 1000.0f` | +| `gForceAxisMaxAlarm` | `int16_t` | 2 | G * 1000 | Sets `osdConfigMutable()->gforce_axis_alarm_max = value / 1000.0f` | +| `currentAlarm` | `uint8_t` | 1 | A | Sets `osdConfigMutable()->current_alarm` | +| `imuTempMinAlarm` | `int16_t` | 2 | degrees C | Sets `osdConfigMutable()->imu_temp_alarm_min` | +| `imuTempMaxAlarm` | `int16_t` | 2 | degrees C | Sets `osdConfigMutable()->imu_temp_alarm_max` | +| `baroTempMinAlarm` | `int16_t` | 2 | degrees C | Sets `osdConfigMutable()->baro_temp_alarm_min` (if `USE_BARO`) | +| `baroTempMaxAlarm` | `int16_t` | 2 | degrees C | Sets `osdConfigMutable()->baro_temp_alarm_max` (if `USE_BARO`) | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_OSD`. Expects 24 bytes. ADSB alarms are not settable via this message. + +## `MSP2_INAV_OSD_PREFERENCES (8214 / 0x2016)` +**Description:** Retrieves OSD display preferences (video system, units, styles, etc.). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `videoSystem` | `uint8_t` | 1 | [videoSystem_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-videosystem_e) | Enum `videoSystem_e`: Video system (Auto/PAL/NTSC) (`osdConfig()->video_system`) | +| `mainVoltageDecimals` | `uint8_t` | 1 | - | Count: Decimal places for main voltage display (`osdConfig()->main_voltage_decimals`) | +| `ahiReverseRoll` | `uint8_t` | 1 | - | Boolean: Reverse roll direction on Artificial Horizon (`osdConfig()->ahi_reverse_roll`) | +| `crosshairsStyle` | `uint8_t` | 1 | [osd_crosshairs_style_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-osd_crosshairs_style_e) | Enum `osd_crosshairs_style_e`: Style of the center crosshairs (`osdConfig()->crosshairs_style`) | +| `leftSidebarScroll` | `uint8_t` | 1 | [osd_sidebar_scroll_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-osd_sidebar_scroll_e) | Enum `osd_sidebar_scroll_e`: Left sidebar scroll behavior (`osdConfig()->left_sidebar_scroll`) | +| `rightSidebarScroll` | `uint8_t` | 1 | [osd_sidebar_scroll_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-osd_sidebar_scroll_e) | Enum `osd_sidebar_scroll_e`: Right sidebar scroll behavior (`osdConfig()->right_sidebar_scroll`) | +| `sidebarScrollArrows` | `uint8_t` | 1 | - | Boolean: Show arrows for scrollable sidebars (`osdConfig()->sidebar_scroll_arrows`) | +| `units` | `uint8_t` | 1 | [osd_unit_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-osd_unit_e) | Enum: `osd_unit_e` Measurement units (Metric/Imperial) (`osdConfig()->units`) | +| `statsEnergyUnit` | `uint8_t` | 1 | [osd_stats_energy_unit_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-osd_stats_energy_unit_e) | Enum `osd_stats_energy_unit_e`: Unit for energy display in post-flight stats (`osdConfig()->stats_energy_unit`) | + +**Notes:** Requires `USE_OSD`. + +## `MSP2_INAV_OSD_SET_PREFERENCES (8215 / 0x2017)` +**Description:** Sets OSD display preferences. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `videoSystem` | `uint8_t` | 1 | [videoSystem_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-videosystem_e) | Sets `osdConfigMutable()->video_system` | +| `mainVoltageDecimals` | `uint8_t` | 1 | - | Sets `osdConfigMutable()->main_voltage_decimals` | +| `ahiReverseRoll` | `uint8_t` | 1 | - | Sets `osdConfigMutable()->ahi_reverse_roll` | +| `crosshairsStyle` | `uint8_t` | 1 | [osd_crosshairs_style_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-osd_crosshairs_style_e) | Sets `osdConfigMutable()->crosshairs_style` | +| `leftSidebarScroll` | `uint8_t` | 1 | [osd_sidebar_scroll_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-osd_sidebar_scroll_e) | Sets `osdConfigMutable()->left_sidebar_scroll` | +| `rightSidebarScroll` | `uint8_t` | 1 | [osd_sidebar_scroll_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-osd_sidebar_scroll_e) | Sets `osdConfigMutable()->right_sidebar_scroll` | +| `sidebarScrollArrows` | `uint8_t` | 1 | - | Sets `osdConfigMutable()->sidebar_scroll_arrows` | +| `units` | `uint8_t` | 1 | [osd_unit_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-osd_unit_e) | Sets `osdConfigMutable()->units` (enum `osd_unit_e`) | +| `statsEnergyUnit` | `uint8_t` | 1 | [osd_stats_energy_unit_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-osd_stats_energy_unit_e) | Sets `osdConfigMutable()->stats_energy_unit` | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_OSD`. Expects 9 bytes. Triggers a full OSD redraw. + +## `MSP2_INAV_SELECT_BATTERY_PROFILE (8216 / 0x2018)` +**Description:** Selects the active battery profile and saves configuration. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `batteryProfileIndex` | `uint8_t` | 1 | Index of the battery profile to activate (0-based) | + +**Reply Payload:** **None** + +**Notes:** Expects 1 byte. Will fail if armed. Calls `setConfigBatteryProfileAndWriteEEPROM()`. + +## `MSP2_INAV_DEBUG (8217 / 0x2019)` +**Description:** Retrieves values from the firmware's 32-bit `debug[]` array. Supersedes `MSP_DEBUG`. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `debugValues` | `int32_t[DEBUG32_VALUE_COUNT]` | 32 (DEBUG32_VALUE_COUNT) | Values from the `debug` array (signed, typically 8 entries) | + +**Notes:** `DEBUG32_VALUE_COUNT` is usually 8. + +## `MSP2_BLACKBOX_CONFIG (8218 / 0x201a)` +**Description:** Retrieves the Blackbox configuration. Supersedes `MSP_BLACKBOX_CONFIG`. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `blackboxSupported` | `uint8_t` | 1 | - | Boolean: 1 if Blackbox is supported (`USE_BLACKBOX`), 0 otherwise | +| `blackboxDevice` | `uint8_t` | 1 | [BlackboxDevice](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-blackboxdevice) | Enum `BlackboxDevice`: Target device for logging (`blackboxConfig()->device`). 0 if not supported | +| `blackboxRateNum` | `uint16_t` | 2 | - | Numerator for logging rate divider (`blackboxConfig()->rate_num`). 0 if not supported | +| `blackboxRateDenom` | `uint16_t` | 2 | - | Denominator for logging rate divider (`blackboxConfig()->rate_denom`). 0 if not supported | +| `blackboxIncludeFlags` | `uint32_t` | 4 | - | Bitmask: Flags for fields included/excluded from logging (`blackboxConfig()->includeFlags`) | + +**Notes:** If `USE_BLACKBOX` is disabled, only the first four fields are returned (all zero). + +## `MSP2_SET_BLACKBOX_CONFIG (8219 / 0x201b)` +**Description:** Sets the Blackbox configuration. Supersedes `MSP_SET_BLACKBOX_CONFIG`. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `blackboxDevice` | `uint8_t` | 1 | [BlackboxDevice](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-blackboxdevice) | Sets `blackboxConfigMutable()->device` | +| `blackboxRateNum` | `uint16_t` | 2 | - | Sets `blackboxConfigMutable()->rate_num` | +| `blackboxRateDenom` | `uint16_t` | 2 | - | Sets `blackboxConfigMutable()->rate_denom` | +| `blackboxIncludeFlags` | `uint32_t` | 4 | - | Sets `blackboxConfigMutable()->includeFlags` | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_BLACKBOX`. Expects 9 bytes. Returns error if Blackbox is currently logging (`!blackboxMayEditConfig()`). + +## `MSP2_INAV_TEMP_SENSOR_CONFIG (8220 / 0x201c)` +**Description:** Retrieves the configuration for all onboard temperature sensors. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `type` | `uint8_t` | 1 | [tempSensorType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-tempsensortype_e) | Enum (`tempSensorType_e`): Type of the temperature sensor | +| `address` | `uint64_t` | 8 | - | Sensor address/ID (e.g., for 1-Wire sensors) | +| `alarmMin` | `int16_t` | 2 | 0.1°C | Min temperature alarm threshold (`sensorConfig->alarm_min`) | +| `alarmMax` | `int16_t` | 2 | 0.1°C | Max temperature alarm threshold (`sensorConfig->alarm_max`) | +| `osdSymbol` | `uint8_t` | 1 | - | Index: OSD symbol to use for this sensor (0 to `TEMP_SENSOR_SYM_COUNT`) | +| `label` | `char[TEMPERATURE_LABEL_LEN]` | 4 (TEMPERATURE_LABEL_LEN) | - | User-defined label for the sensor | + +**Notes:** Requires `USE_TEMPERATURE_SENSOR`. + +## `MSP2_INAV_SET_TEMP_SENSOR_CONFIG (8221 / 0x201d)` +**Description:** Sets the configuration for all onboard temperature sensors. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `type` | `uint8_t` | 1 | [tempSensorType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-tempsensortype_e) | Sets sensor type (`tempSensorType_e`) | +| `address` | `uint64_t` | 8 | - | Sets sensor address/ID | +| `alarmMin` | `int16_t` | 2 | 0.1°C | Sets min alarm threshold (`tempSensorConfigMutable(index)->alarm_min`) | +| `alarmMax` | `int16_t` | 2 | 0.1°C | Sets max alarm threshold (`tempSensorConfigMutable(index)->alarm_max`) | +| `osdSymbol` | `uint8_t` | 1 | - | Sets OSD symbol index (validated) | +| `label` | `char[TEMPERATURE_LABEL_LEN]` | 4 (TEMPERATURE_LABEL_LEN) | - | Sets sensor label (converted to uppercase) | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_TEMPERATURE_SENSOR`. Payload must include `MAX_TEMP_SENSORS` consecutive `tempSensorConfig_t` structures (labels are uppercased). + +## `MSP2_INAV_TEMPERATURES (8222 / 0x201e)` +**Description:** Retrieves the current readings from all configured temperature sensors. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `temperature` | `int16_t` | 2 | 0.1°C | Current temperature reading. -1000 if sensor is invalid or reading failed | + +**Notes:** Requires `USE_TEMPERATURE_SENSOR`. + +## `MSP_SIMULATOR (8223 / 0x201f)` +**Description:** Handles Hardware-in-the-Loop (HITL) simulation data exchange. Receives simulated sensor data and options, sends back control outputs and debug info. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `simulatorVersion` | `uint8_t` | 1 | - | Version of the simulator protocol (`SIMULATOR_MSP_VERSION`) | +| `simulatorFlags_t` | `uint8_t` | 1 | Bitmask | Bitmask: Options for HITL (`HITL_*` flags) | +| `gpsFixType` | `uint8_t` | 1 | [gpsFixType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-gpsfixtype_e) | Enum `gpsFixType_e` Type of GPS fix (If `HITL_HAS_NEW_GPS_DATA`) | +| `gpsNumSat` | `uint8_t` | 1 | - | (If `HITL_HAS_NEW_GPS_DATA`) Simulated satellite count | +| `gpsLat` | `int32_t` | 4 | - | (If `HITL_HAS_NEW_GPS_DATA`) Simulated latitude (1e7 deg) | +| `gpsLon` | `int32_t` | 4 | - | (If `HITL_HAS_NEW_GPS_DATA`) Simulated longitude (1e7 deg) | +| `gpsAlt` | `int32_t` | 4 | - | (If `HITL_HAS_NEW_GPS_DATA`) Simulated altitude (cm) | +| `gpsSpeed` | `uint16_t` | 2 | - | (If `HITL_HAS_NEW_GPS_DATA`) Simulated ground speed (cm/s) | +| `gpsCourse` | `uint16_t` | 2 | - | (If `HITL_HAS_NEW_GPS_DATA`) Simulated ground course (deci-deg) | +| `gpsVelN` | `int16_t` | 2 | - | (If `HITL_HAS_NEW_GPS_DATA`) Simulated North velocity (cm/s) | +| `gpsVelE` | `int16_t` | 2 | - | (If `HITL_HAS_NEW_GPS_DATA`) Simulated East velocity (cm/s) | +| `gpsVelD` | `int16_t` | 2 | - | (If `HITL_HAS_NEW_GPS_DATA`) Simulated Down velocity (cm/s) | +| `imuRoll` | `int16_t` | 2 | - | (If NOT `HITL_USE_IMU`) Simulated Roll (deci-deg) | +| `imuPitch` | `int16_t` | 2 | - | (If NOT `HITL_USE_IMU`) Simulated Pitch (deci-deg) | +| `imuYaw` | `int16_t` | 2 | - | (If NOT `HITL_USE_IMU`) Simulated Yaw (deci-deg) | +| `accX` | `int16_t` | 2 | - | mG (G * 1000) | +| `accY` | `int16_t` | 2 | - | mG (G * 1000) | +| `accZ` | `int16_t` | 2 | - | mG (G * 1000) | +| `gyroX` | `int16_t` | 2 | - | dps * 16 | +| `gyroY` | `int16_t` | 2 | - | dps * 16 | +| `gyroZ` | `int16_t` | 2 | - | dps * 16 | +| `baroPressure` | `uint32_t` | 4 | - | Pa | +| `magX` | `int16_t` | 2 | - | Scaled | +| `magY` | `int16_t` | 2 | - | Scaled | +| `magZ` | `int16_t` | 2 | - | Scaled | +| `vbat` | `uint8_t` | 1 | - | (If `HITL_EXT_BATTERY_VOLTAGE`) Simulated battery voltage (0.1V units) | +| `airspeed` | `uint16_t` | 2 | - | (If `HITL_AIRSPEED`) Simulated airspeed (cm/s) | +| `extFlags` | `uint8_t` | 1 | - | (If `HITL_EXTENDED_FLAGS`) Additional flags (upper 8 bits) | + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `stabilizedRoll` | `uint16_t` | 2 | Stabilized Roll command output (-500 to 500) | +| `stabilizedPitch` | `uint16_t` | 2 | Stabilized Pitch command output (-500 to 500) | +| `stabilizedYaw` | `uint16_t` | 2 | Stabilized Yaw command output (-500 to 500) | +| `stabilizedThrottle` | `uint16_t` | 2 | Stabilized Throttle command output (-500 to 500 if armed, else -500) | +| `debugFlags` | `uint8_t` | 1 | Packed flags: Debug index (0-7), Platform type, Armed state, OSD feature status | +| `debugValue` | `uint32_t` | 4 | Current debug value (`debug[simulatorData.debugIndex]`) | +| `attitudeRoll` | `int16_t` | 2 | Current estimated Roll (deci-deg) | +| `attitudePitch` | `int16_t` | 2 | Current estimated Pitch (deci-deg) | +| `attitudeYaw` | `int16_t` | 2 | Current estimated Yaw (deci-deg) | +| `osdHeader` | `uint8_t` | 1 | OSD RLE Header (255) | +| `osdRows` | `uint8_t` | 1 | (If OSD supported) Number of OSD rows | +| `osdCols` | `uint8_t` | 1 | (If OSD supported) Number of OSD columns | +| `osdStartY` | `uint8_t` | 1 | (If OSD supported) Starting row for RLE data | +| `osdStartX` | `uint8_t` | 1 | (If OSD supported) Starting column for RLE data | +| `osdRleData` | `uint8_t[]` | array | (If OSD supported) Run-length encoded OSD character data. Terminated by `[0, 0]` | + +**Notes:** Requires `USE_SIMULATOR`. Complex message handling state changes for enabling/disabling HITL. Sensor data is injected directly. OSD data is sent using a custom RLE scheme. See `simulatorData` struct and associated code for details. + +## `MSP2_INAV_SERVO_MIXER (8224 / 0x2020)` +**Description:** Retrieves the custom servo mixer rules, including programming framework condition IDs, for primary and secondary mixer profiles. Supersedes `MSP_SERVO_MIX_RULES`. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `targetChannel` | `uint8_t` | 1 | - | Servo output channel index (0-based) | +| `inputSource` | `uint8_t` | 1 | [inputSource_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-inputsource_e) | Enum `inputSource_e` Input source | +| `rate` | `int16_t` | 2 | - | Mixing rate/weight | +| `speed` | `uint8_t` | 1 | - | Speed/Slew rate limit (0-100) | +| `conditionId` | `int8_t` | 1 | - | Logic Condition ID (0 to `MAX_LOGIC_CONDITIONS - 1`, or 255/-1 if none/disabled) | +| `p2TargetChannel` | `uint8_t` | 1 | - | (Optional) Profile 2 Target channel | +| `p2InputSource` | `uint8_t` | 1 | [inputSource_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-inputsource_e) | (Optional) Profile 2 Enum `inputSource_e` Input source | +| `p2Rate` | `int16_t` | 2 | - | (Optional) Profile 2 Rate | +| `p2Speed` | `uint8_t` | 1 | - | (Optional) Profile 2 Speed | +| `p2ConditionId` | `int8_t` | 1 | - | (Optional) Profile 2 Logic Condition ID | + +**Notes:** `conditionId` requires `USE_PROGRAMMING_FRAMEWORK`. + +## `MSP2_INAV_SET_SERVO_MIXER (8225 / 0x2021)` +**Description:** Sets a single custom servo mixer rule, including programming framework condition ID. Supersedes `MSP_SET_SERVO_MIX_RULE`. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `ruleIndex` | `uint8_t` | 1 | - | Index of the rule to set (0 to `MAX_SERVO_RULES - 1`) | +| `targetChannel` | `uint8_t` | 1 | - | Servo output channel index | +| `inputSource` | `uint8_t` | 1 | [inputSource_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-inputsource_e) | Enum `inputSource_e` Input source | +| `rate` | `int16_t` | 2 | - | Mixing rate/weight | +| `speed` | `uint8_t` | 1 | - | Speed/Slew rate limit (0-100) | +| `conditionId` | `int8_t` | 1 | - | Logic Condition ID (255/-1 if none). Ignored if `USE_PROGRAMMING_FRAMEWORK` is disabled | + +**Reply Payload:** **None** + +**Notes:** Expects 7 bytes. Returns error if index invalid. Calls `loadCustomServoMixer()`. + +## `MSP2_INAV_LOGIC_CONDITIONS (8226 / 0x2022)` +**Description:** Retrieves the configuration of all defined Logic Conditions. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `enabled` | `uint8_t` | 1 | - | Boolean: 1 if the condition is enabled | +| `activatorId` | `int8_t` | 1 | - | Activator condition ID (-1/255 if none) | +| `operation` | `uint8_t` | 1 | [logicOperation_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-logicoperation_e) | Enum `logicOperation_e` Logical operation (AND, OR, XOR, etc.) | +| `operandAType` | `uint8_t` | 1 | [logicOperandType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-logicoperandtype_e) | Enum `logicOperandType_e` Type of the first operand (Flight Mode, GVAR, etc.) | +| `operandAValue` | `int32_t` | 4 | - | Value/ID of the first operand | +| `operandBType` | `uint8_t` | 1 | [logicOperandType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-logicoperandtype_e) | Enum `logicOperandType_e`: Type of the second operand | +| `operandBValue` | `int32_t` | 4 | - | Value/ID of the second operand | +| `flags` | `uint8_t` | 1 | Bitmask | Bitmask: Condition flags (`logicConditionFlags_e`) | + +**Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. See `logicCondition_t` structure. + +## `MSP2_INAV_SET_LOGIC_CONDITIONS (8227 / 0x2023)` +**Description:** Sets the configuration for a single Logic Condition by its index. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `conditionIndex` | `uint8_t` | 1 | - | Index of the condition to set (0 to `MAX_LOGIC_CONDITIONS - 1`) | +| `enabled` | `uint8_t` | 1 | - | Boolean: 1 to enable the condition | +| `activatorId` | `int8_t` | 1 | - | Activator condition ID (-1/255 if none) | +| `operation` | `uint8_t` | 1 | [logicOperation_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-logicoperation_e) | Enum `logicOperation_e` Logical operation | +| `operandAType` | `uint8_t` | 1 | [logicOperandType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-logicoperandtype_e) | Enum `logicOperandType_e` Type of operand A | +| `operandAValue` | `int32_t` | 4 | - | Value/ID of operand A | +| `operandBType` | `uint8_t` | 1 | [logicOperandType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-logicoperandtype_e) | Enum `logicOperandType_e` Type of operand B | +| `operandBValue` | `int32_t` | 4 | - | Value/ID of operand B | +| `flags` | `uint8_t` | 1 | Bitmask | Bitmask: Condition flags (`logicConditionFlags_e`) | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. Expects 15 bytes. Returns error if index is invalid. + +## `MSP2_INAV_GLOBAL_FUNCTIONS (8228 / 0x2024)` + +**Request Payload:** **None** + +**Reply Payload:** **None** + +## `MSP2_INAV_SET_GLOBAL_FUNCTIONS (8229 / 0x2025)` + +**Request Payload:** **None** + +**Reply Payload:** **None** + +## `MSP2_INAV_LOGIC_CONDITIONS_STATUS (8230 / 0x2026)` +**Description:** Retrieves the current evaluated status (true/false or numerical value) of all logic conditions. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `conditionValues` | `int32_t[MAX_LOGIC_CONDITIONS]` | 256 (MAX_LOGIC_CONDITIONS) | Array of current values for each logic condition (`logicConditionGetValue(i)`). 1 for true, 0 for false, or numerical value depending on operation | + +**Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. + +## `MSP2_INAV_GVAR_STATUS (8231 / 0x2027)` +**Description:** Retrieves the current values of all Global Variables (GVARS). + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `gvarValues` | `int32_t[MAX_GLOBAL_VARIABLES]` | 32 (MAX_GLOBAL_VARIABLES) | Array of current values for each global variable (`gvGet(i)`) | + +**Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. + +## `MSP2_INAV_PROGRAMMING_PID (8232 / 0x2028)` +**Description:** Retrieves the configuration of all Programming PIDs. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `enabled` | `uint8_t` | 1 | - | Boolean: 1 if the PID is enabled | +| `setpointType` | `uint8_t` | 1 | [logicOperandType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-logicoperandtype_e) | Enum (`logicOperandType_e`) Type of the setpoint source | +| `setpointValue` | `int32_t` | 4 | - | Value/ID of the setpoint source | +| `measurementType` | `uint8_t` | 1 | [logicOperandType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-logicoperandtype_e) | Enum (`logicOperandType_e`) Type of the measurement source | +| `measurementValue` | `int32_t` | 4 | - | Value/ID of the measurement source | +| `gainP` | `uint16_t` | 2 | - | Proportional gain | +| `gainI` | `uint16_t` | 2 | - | Integral gain | +| `gainD` | `uint16_t` | 2 | - | Derivative gain | +| `gainFF` | `uint16_t` | 2 | - | Feed-forward gain | + +**Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. See `programmingPid_t` structure. + +## `MSP2_INAV_SET_PROGRAMMING_PID (8233 / 0x2029)` +**Description:** Sets the configuration for a single Programming PID by its index. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `pidIndex` | `uint8_t` | 1 | - | Index of the Programming PID to set (0 to `MAX_PROGRAMMING_PID_COUNT - 1`) | +| `enabled` | `uint8_t` | 1 | - | Boolean: 1 to enable the PID | +| `setpointType` | `uint8_t` | 1 | [logicOperandType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-logicoperandtype_e) | Enum (`logicOperandType_e`) Type of the setpoint source | +| `setpointValue` | `int32_t` | 4 | - | Value/ID of the setpoint source | +| `measurementType` | `uint8_t` | 1 | [logicOperandType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-logicoperandtype_e) | Enum (`logicOperandType_e`) Type of the measurement source | +| `measurementValue` | `int32_t` | 4 | - | Value/ID of the measurement source | +| `gainP` | `uint16_t` | 2 | - | Proportional gain | +| `gainI` | `uint16_t` | 2 | - | Integral gain | +| `gainD` | `uint16_t` | 2 | - | Derivative gain | +| `gainFF` | `uint16_t` | 2 | - | Feed-forward gain | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. Expects 20 bytes. Returns error if index is invalid. + +## `MSP2_INAV_PROGRAMMING_PID_STATUS (8234 / 0x202a)` +**Description:** Retrieves the current output value of all Programming PIDs. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `pidOutputs` | `int32_t[MAX_PROGRAMMING_PID_COUNT]` | 16 (MAX_PROGRAMMING_PID_COUNT) | Array of current output values for each Programming PID (`programmingPidGetOutput(i)`, signed) | + +**Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. + +## `MSP2_PID (8240 / 0x2030)` +**Description:** Retrieves the standard PID controller gains (P, I, D, FF) for the current PID profile. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `P` | `uint8_t` | 1 | Proportional gain (`pidBank()->pid[i].P`), constrained 0-255 | +| `I` | `uint8_t` | 1 | Integral gain (`pidBank()->pid[i].I`), constrained 0-255 | +| `D` | `uint8_t` | 1 | Derivative gain (`pidBank()->pid[i].D`), constrained 0-255 | +| `FF` | `uint8_t` | 1 | Feed-forward gain (`pidBank()->pid[i].FF`), constrained 0-255 | + +**Notes:** `PID_ITEM_COUNT` defines the number of standard PID controllers (Roll, Pitch, Yaw, Alt, Vel, etc.). Updates from EZ-Tune if enabled. + +## `MSP2_SET_PID (8241 / 0x2031)` +**Description:** Sets the standard PID controller gains (P, I, D, FF) for the current PID profile. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `P` | `uint8_t` | 1 | Sets Proportional gain (`pidBankMutable()->pid[i].P`) | +| `I` | `uint8_t` | 1 | Sets Integral gain (`pidBankMutable()->pid[i].I`) | +| `D` | `uint8_t` | 1 | Sets Derivative gain (`pidBankMutable()->pid[i].D`) | +| `FF` | `uint8_t` | 1 | Sets Feed-forward gain (`pidBankMutable()->pid[i].FF`) | + +**Reply Payload:** **None** + +**Notes:** Expects `PID_ITEM_COUNT * 4` bytes. Calls `schedulePidGainsUpdate()` and `navigationUsePIDs()`. + +## `MSP2_INAV_OPFLOW_CALIBRATION (8242 / 0x2032)` +**Description:** Starts the optical flow sensor calibration procedure. + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Requires `USE_OPFLOW`. Will fail if armed. Calls `opflowStartCalibration()`. + +## `MSP2_INAV_FWUPDT_PREPARE (8243 / 0x2033)` +**Description:** Prepares the flight controller to receive a firmware update via MSP. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `firmwareSize` | `uint32_t` | 4 | Total size of the incoming firmware file in bytes | + +**Reply Payload:** **None** + +**Notes:** Requires `MSP_FIRMWARE_UPDATE`. Expects 4 bytes. Returns error if preparation fails (e.g., no storage, invalid size). Calls `firmwareUpdatePrepare()`. + +## `MSP2_INAV_FWUPDT_STORE (8244 / 0x2034)` +**Description:** Stores a chunk of firmware data received via MSP. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `firmwareChunk` | `uint8_t[]` | array | Chunk of firmware data | + +**Reply Payload:** **None** + +**Notes:** Requires `MSP_FIRMWARE_UPDATE`. Returns error if storage fails (e.g., out of space, checksum error). Called repeatedly until the entire firmware is transferred. Calls `firmwareUpdateStore()`. + +## `MSP2_INAV_FWUPDT_EXEC (8245 / 0x2035)` +**Description:** Executes the firmware update process (flashes the stored firmware and reboots). + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `updateType` | `uint8_t` | 1 | Type of update (e.g., full flash, specific section - currently ignored/unused) | + +**Reply Payload:** **None** + +**Notes:** Requires `MSP_FIRMWARE_UPDATE`. Expects 1 byte. Returns error if update cannot start (e.g., not fully received). Calls `firmwareUpdateExec()`. If successful, the device will reboot into the new firmware. + +## `MSP2_INAV_FWUPDT_ROLLBACK_PREPARE (8246 / 0x2036)` +**Description:** Prepares the flight controller to perform a firmware rollback to the previously stored version. + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Requires `MSP_FIRMWARE_UPDATE`. Returns error if rollback preparation fails (e.g., no rollback image available). Calls `firmwareUpdateRollbackPrepare()`. + +## `MSP2_INAV_FWUPDT_ROLLBACK_EXEC (8247 / 0x2037)` +**Description:** Executes the firmware rollback process (flashes the stored backup firmware and reboots). + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Requires `MSP_FIRMWARE_UPDATE`. Returns error if rollback cannot start. Calls `firmwareUpdateRollbackExec()`. If successful, the device will reboot into the backup firmware. + +## `MSP2_INAV_SAFEHOME (8248 / 0x2038)` +**Description:** Get or Set configuration for a specific Safe Home location. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `safehomeIndex` | `uint8_t` | 1 | Index of the safe home location (0 to `MAX_SAFE_HOMES - 1`) | + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `safehomeIndex` | `uint8_t` | 1 | Index requested | +| `enabled` | `uint8_t` | 1 | Boolean: 1 if this safe home is enabled | +| `latitude` | `int32_t` | 4 | Latitude (1e7 deg) | +| `longitude` | `int32_t` | 4 | Longitude (1e7 deg) | + +**Notes:** Requires `USE_SAFE_HOME`. Used by `mspFcSafeHomeOutCommand`. See `MSP2_INAV_SET_SAFEHOME` for setting. + +## `MSP2_INAV_SET_SAFEHOME (8249 / 0x2039)` +**Description:** Sets the configuration for a specific Safe Home location. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `safehomeIndex` | `uint8_t` | 1 | Index of the safe home location (0 to `MAX_SAFE_HOMES - 1`) | +| `enabled` | `uint8_t` | 1 | Boolean: 1 to enable this safe home | +| `latitude` | `int32_t` | 4 | Latitude (1e7 deg) | +| `longitude` | `int32_t` | 4 | Longitude (1e7 deg) | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_SAFE_HOME`. Expects 10 bytes. Returns error if index invalid. Resets corresponding FW autoland approach if `USE_FW_AUTOLAND` is enabled. + +## `MSP2_INAV_MISC2 (8250 / 0x203a)` +**Description:** Retrieves miscellaneous runtime information including timers and throttle status. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `uptimeSeconds` | `uint32_t` | 4 | Seconds | Time since boot (`micros() / 1000000`) | +| `flightTimeSeconds` | `uint32_t` | 4 | Seconds | Accumulated flight time (`getFlightTime()`) | +| `throttlePercent` | `uint8_t` | 1 | % | Current throttle output percentage (`getThrottlePercent(true)`) | +| `autoThrottleFlag` | `uint8_t` | 1 | Boolean | 1 if navigation is controlling throttle, 0 otherwise (`navigationIsControllingThrottle()`) | + +## `MSP2_INAV_LOGIC_CONDITIONS_SINGLE (8251 / 0x203b)` +**Description:** Gets the configuration for a single Logic Condition by its index. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `conditionIndex` | `uint8_t` | 1 | Index of the condition to retrieve (0 to `MAX_LOGIC_CONDITIONS - 1`) | + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `enabled` | `uint8_t` | 1 | - | Boolean: 1 if enabled | +| `activatorId` | `int8_t` | 1 | - | Activator ID (-1/255 if none) | +| `operation` | `uint8_t` | 1 | [logicOperation_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-logicoperation_e) | Enum `logicOperation_e` Logical operation | +| `operandAType` | `uint8_t` | 1 | [logicOperandType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-logicoperandtype_e) | Enum `logicOperandType_e` Type of operand A | +| `operandAValue` | `int32_t` | 4 | - | Value/ID of operand A | +| `operandBType` | `uint8_t` | 1 | [logicOperandType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-logicoperandtype_e) | Enum `logicOperandType_e` Type of operand B | +| `operandBValue` | `int32_t` | 4 | - | Value/ID of operand B | +| `flags` | `uint8_t` | 1 | Bitmask | Bitmask: Condition flags (`logicConditionFlags_e`) | + +**Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. Used by `mspFcLogicConditionCommand`. + +## `MSP2_INAV_ESC_RPM (8256 / 0x2040)` +**Description:** Retrieves the RPM reported by each ESC via telemetry. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `escRpm` | `uint32_t` | 4 | RPM | RPM reported by the ESC | + +**Notes:** Requires `USE_ESC_SENSOR`. Payload size depends on the number of detected motors with telemetry. + +## `MSP2_INAV_ESC_TELEM (8257 / 0x2041)` +**Description:** Retrieves the full telemetry data structure reported by each ESC. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `motorCount` | `uint8_t` | 1 | Number of motors reporting telemetry (`getMotorCount()`) | +| `escData` | `escSensorData_t[]` | array | Array of `escSensorData_t` structures containing voltage, current, temp, RPM, errors etc. for each ESC | + +**Notes:** Requires `USE_ESC_SENSOR`. See `escSensorData_t` in `sensors/esc_sensor.h` for the exact structure fields. + +## `MSP2_INAV_LED_STRIP_CONFIG_EX (8264 / 0x2048)` +**Description:** Retrieves the full configuration for each LED on the strip using the `ledConfig_t` structure. Supersedes `MSP_LED_STRIP_CONFIG`. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `ledConfig` | `ledConfig_t` | - | Raw `ledConfig_t` structure (6 bytes) holding position, function, overlay, color, direction, and params bitfields (`io/ledstrip.h`). | + +**Notes:** Requires `USE_LED_STRIP`. See `ledConfig_t` in `io/ledstrip.h` for structure fields (position, function, overlay, color, direction, params). + +## `MSP2_INAV_SET_LED_STRIP_CONFIG_EX (8265 / 0x2049)` +**Description:** Sets the configuration for a single LED on the strip using the `ledConfig_t` structure. Supersedes `MSP_SET_LED_STRIP_CONFIG`. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `ledIndex` | `uint8_t` | 1 | Index of the LED to configure (0 to `LED_MAX_STRIP_LENGTH - 1`) | +| `ledConfig` | `ledConfig_t` | - | Raw `ledConfig_t` structure (6 bytes) mirroring the firmware layout. | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_LED_STRIP`. Expects `1 + sizeof(ledConfig_t)` bytes. Returns error if index invalid. Calls `reevaluateLedConfig()`. + +## `MSP2_INAV_FW_APPROACH (8266 / 0x204a)` +**Description:** Get or Set configuration for a specific Fixed Wing Autoland approach. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `approachIndex` | `uint8_t` | 1 | Index of the approach setting (0 to `MAX_FW_LAND_APPOACH_SETTINGS - 1`) | + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `approachIndex` | `uint8_t` | 1 | Index | Index requested | +| `approachAlt` | `int32_t` | 4 | cm | Signed altitude for the approach phase (`navFwAutolandApproach_t.approachAlt`) | +| `landAlt` | `int32_t` | 4 | cm | Signed altitude for the final landing phase (`navFwAutolandApproach_t.landAlt`) | +| `approachDirection` | `uint8_t` | 1 | [fwAutolandApproachDirection_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-fwautolandapproachdirection_e) | Enum `fwAutolandApproachDirection_e`: Direction of approach (From WP, Specific Heading) | +| `landHeading1` | `int16_t` | 2 | degrees | Primary landing heading (if approachDirection requires it) | +| `landHeading2` | `int16_t` | 2 | degrees | Secondary landing heading (if approachDirection requires it) | +| `isSeaLevelRef` | `uint8_t` | 1 | Boolean | 1 if altitudes are relative to sea level, 0 if relative to home | + +**Notes:** Requires `USE_FW_AUTOLAND`. Used by `mspFwApproachOutCommand`. See `MSP2_INAV_SET_FW_APPROACH` for setting. + +## `MSP2_INAV_SET_FW_APPROACH (8267 / 0x204b)` +**Description:** Sets the configuration for a specific Fixed Wing Autoland approach. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `approachIndex` | `uint8_t` | 1 | Index | Index of the approach setting (0 to `MAX_FW_LAND_APPOACH_SETTINGS - 1`) | +| `approachAlt` | `int32_t` | 4 | cm | Signed approach altitude (`navFwAutolandApproach_t.approachAlt`) | +| `landAlt` | `int32_t` | 4 | cm | Signed landing altitude (`navFwAutolandApproach_t.landAlt`) | +| `approachDirection` | `uint8_t` | 1 | [fwAutolandApproachDirection_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-fwautolandapproachdirection_e) | Enum `fwAutolandApproachDirection_e` Sets approach direction | +| `landHeading1` | `int16_t` | 2 | degrees | Sets primary landing heading | +| `landHeading2` | `int16_t` | 2 | degrees | Sets secondary landing heading | +| `isSeaLevelRef` | `uint8_t` | 1 | Boolean | Sets altitude reference | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_FW_AUTOLAND`. Expects 15 bytes. Returns error if index invalid. + +## `MSP2_INAV_GPS_UBLOX_COMMAND (8272 / 0x2050)` +**Description:** Sends a raw command directly to a U-Blox GPS module connected to the FC. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `ubxCommand` | `uint8_t[]` | array | Raw U-Blox UBX protocol command frame (including header, class, ID, length, payload, checksum) | + +**Reply Payload:** **None** + +**Notes:** Requires GPS feature enabled (`FEATURE_GPS`) and the GPS driver to be U-Blox (`isGpsUblox()`). Payload must be at least 8 bytes (minimum UBX frame size). Use with extreme caution, incorrect commands can misconfigure the GPS module. Calls `gpsUbloxSendCommand()`. + +## `MSP2_INAV_RATE_DYNAMICS (8288 / 0x2060)` +**Description:** Retrieves Rate Dynamics configuration parameters for the current control rate profile. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `sensitivityCenter` | `uint8_t` | 1 | % | Sensitivity at stick center (`currentControlRateProfile->rateDynamics.sensitivityCenter`) | +| `sensitivityEnd` | `uint8_t` | 1 | % | Sensitivity at stick ends (`currentControlRateProfile->rateDynamics.sensitivityEnd`) | +| `correctionCenter` | `uint8_t` | 1 | % | Correction strength at stick center (`currentControlRateProfile->rateDynamics.correctionCenter`) | +| `correctionEnd` | `uint8_t` | 1 | % | Correction strength at stick ends (`currentControlRateProfile->rateDynamics.correctionEnd`) | +| `weightCenter` | `uint8_t` | 1 | % | Transition weight at stick center (`currentControlRateProfile->rateDynamics.weightCenter`) | +| `weightEnd` | `uint8_t` | 1 | % | Transition weight at stick ends (`currentControlRateProfile->rateDynamics.weightEnd`) | + +**Notes:** Requires `USE_RATE_DYNAMICS`. + +## `MSP2_INAV_SET_RATE_DYNAMICS (8289 / 0x2061)` +**Description:** Sets Rate Dynamics configuration parameters for the current control rate profile. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `sensitivityCenter` | `uint8_t` | 1 | % | Sets sensitivity at center | +| `sensitivityEnd` | `uint8_t` | 1 | % | Sets sensitivity at ends | +| `correctionCenter` | `uint8_t` | 1 | % | Sets correction at center | +| `correctionEnd` | `uint8_t` | 1 | % | Sets correction at ends | +| `weightCenter` | `uint8_t` | 1 | % | Sets weight at center | +| `weightEnd` | `uint8_t` | 1 | % | Sets weight at ends | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_RATE_DYNAMICS`. Expects 6 bytes. + +## `MSP2_INAV_EZ_TUNE (8304 / 0x2070)` +**Description:** Retrieves the current EZ-Tune parameters. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `enabled` | `uint8_t` | 1 | Boolean: 1 if EZ-Tune is enabled (`ezTune()->enabled`) | +| `filterHz` | `uint16_t` | 2 | Filter frequency used during tuning (`ezTune()->filterHz`) | +| `axisRatio` | `uint8_t` | 1 | Roll vs Pitch axis tuning ratio (`ezTune()->axisRatio`) | +| `response` | `uint8_t` | 1 | Desired response characteristic (`ezTune()->response`) | +| `damping` | `uint8_t` | 1 | Desired damping characteristic (`ezTune()->damping`) | +| `stability` | `uint8_t` | 1 | Stability preference (`ezTune()->stability`) | +| `aggressiveness` | `uint8_t` | 1 | Aggressiveness preference (`ezTune()->aggressiveness`) | +| `rate` | `uint8_t` | 1 | Resulting rate setting (`ezTune()->rate`) | +| `expo` | `uint8_t` | 1 | Resulting expo setting (`ezTune()->expo`) | +| `snappiness` | `uint8_t` | 1 | Snappiness preference (`ezTune()->snappiness`) | + +**Notes:** Requires `USE_EZ_TUNE`. Calls `ezTuneUpdate()` before sending. + +## `MSP2_INAV_EZ_TUNE_SET (8305 / 0x2071)` +**Description:** Sets the EZ-Tune parameters and triggers an update. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `enabled` | `uint8_t` | 1 | Sets enabled state | +| `filterHz` | `uint16_t` | 2 | Sets filter frequency | +| `axisRatio` | `uint8_t` | 1 | Sets axis ratio | +| `response` | `uint8_t` | 1 | Sets response characteristic | +| `damping` | `uint8_t` | 1 | Sets damping characteristic | +| `stability` | `uint8_t` | 1 | Sets stability preference | +| `aggressiveness` | `uint8_t` | 1 | Sets aggressiveness preference | +| `rate` | `uint8_t` | 1 | Sets rate setting | +| `expo` | `uint8_t` | 1 | Sets expo setting | +| `snappiness` | `uint8_t` | 1 | (Optional) Sets snappiness preference | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_EZ_TUNE`. Expects 10 or 11 bytes. Calls `ezTuneUpdate()` after setting parameters. + +## `MSP2_INAV_SELECT_MIXER_PROFILE (8320 / 0x2080)` +**Description:** Selects the active mixer profile and saves configuration. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `mixerProfileIndex` | `uint8_t` | 1 | Index of the mixer profile to activate (0-based) | + +**Reply Payload:** **None** + +**Notes:** Expects 1 byte. Will fail if armed. Calls `setConfigMixerProfileAndWriteEEPROM()`. Only applicable if `MAX_MIXER_PROFILE_COUNT` > 1. + +## `MSP2_ADSB_VEHICLE_LIST (8336 / 0x2090)` +**Description:** Retrieves the list of currently tracked ADSB (Automatic Dependent Surveillance–Broadcast) vehicles. See `adsbVehicle_t` and `adsbVehicleValues_t` in `io/adsb.h` for the exact structure fields. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Repeats|Size (Bytes)|Units|Description| +|---|---|---|---|---|---| +| `maxVehicles` | `uint8_t` | - | 1 | - | Maximum number of vehicles tracked (`MAX_ADSB_VEHICLES`). 0 if `USE_ADSB` disabled | +| `callsignLength` | `uint8_t` | - | 1 | - | Maximum length of callsign string (`ADSB_CALL_SIGN_MAX_LENGTH`). 0 if `USE_ADSB` disabled | +| `totalVehicleMsgs` | `uint32_t` | - | 4 | - | Total vehicle messages received (`getAdsbStatus()->vehiclesMessagesTotal`). 0 if `USE_ADSB` disabled | +| `totalHeartbeatMsgs` | `uint32_t` | - | 4 | - | Total heartbeat messages received (`getAdsbStatus()->heartbeatMessagesTotal`). 0 if `USE_ADSB` disabled | +| `callsign` | `char[ADSB_CALL_SIGN_MAX_LENGTH]` | maxVehicles | 9 (ADSB_CALL_SIGN_MAX_LENGTH) | - | Fixed-length callsign from `adsbVehicle->vehicleValues.callsign` (padded with NULs if shorter). | +| `icao` | `uint32_t` | maxVehicles | 4 | - | ICAO address (`adsbVehicle->vehicleValues.icao`). | +| `lat` | `int32_t` | maxVehicles | 4 | 1e-7 deg | Latitude in degrees * 1e7 (`adsbVehicle->vehicleValues.lat`). | +| `lon` | `int32_t` | maxVehicles | 4 | 1e-7 deg | Longitude in degrees * 1e7 (`adsbVehicle->vehicleValues.lon`). | +| `alt` | `int32_t` | maxVehicles | 4 | cm | Altitude above sea level (`adsbVehicle->vehicleValues.alt`). | +| `headingDeg` | `uint16_t` | maxVehicles | 2 | deg | Course over ground in whole degrees (`CENTIDEGREES_TO_DEGREES(vehicleValues.heading)`). | +| `tslc` | `uint8_t` | maxVehicles | 1 | s | Time since last communication (`adsbVehicle->vehicleValues.tslc`). | +| `emitterType` | `uint8_t` | maxVehicles | 1 | - | Emitter category (`adsbVehicle->vehicleValues.emitterType`) (refers to enum 'ADSB_EMITTER_TYPE', but none found) | +| `ttl` | `uint8_t` | maxVehicles | 1 | - | TTL counter used for list maintenance (`adsbVehicle->ttl`). | + +**Notes:** Requires `USE_ADSB`. Only a subset of `adsbVehicle_t` is transmitted (callsign, core values, heading in whole degrees, TSLC, emitter type, TTL). + +## `MSP2_INAV_CUSTOM_OSD_ELEMENTS (8448 / 0x2100)` +**Description:** Retrieves counts related to custom OSD elements defined by the programming framework. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `maxElements` | `uint8_t` | 1 | Maximum number of custom elements (`MAX_CUSTOM_ELEMENTS`) | +| `maxTextLength` | `uint8_t` | 1 | Maximum length of the text part (`OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1`) | +| `maxParts` | `uint8_t` | 1 | Maximum number of parts per element (`CUSTOM_ELEMENTS_PARTS`) | + +**Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. + +## `MSP2_INAV_CUSTOM_OSD_ELEMENT (8449 / 0x2101)` +**Description:** Gets the configuration of a single custom OSD element defined by the programming framework. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `elementIndex` | `uint8_t` | 1 | Index of the custom element (0 to `MAX_CUSTOM_ELEMENTS - 1`) | + +**Reply Payload:** +|Field|C Type|Repeats|Size (Bytes)|Units|Description| +|---|---|---|---|---|---| +| `partType` | `uint8_t` | CUSTOM_ELEMENTS_PARTS | 1 | [osdCustomElementType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-osdcustomelementtype_e) | Type of this part | +| `partValue` | `uint16_t` | CUSTOM_ELEMENTS_PARTS | 2 | - | Value/ID associated with this part | +| `visibilityType` | `uint8_t` | - | 1 | [osdCustomElementTypeVisibility_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-osdcustomelementtypevisibility_e) | Visibility condition source | +| `visibilityValue` | `uint16_t` | - | 2 | - | Value/ID of the visibility condition source | +| `elementText` | `char[OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1]` | - | OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1 | - | Static text bytes | + +**Notes:** Reply emitted only if idx < MAX_CUSTOM_ELEMENTS; otherwise no body is written. + +## `MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS (8450 / 0x2102)` +**Description:** Sets the configuration of one custom OSD element. + +**Request Payload:** +|Field|C Type|Repeats|Size (Bytes)|Units|Description| +|---|---|---|---|---|---| +| `elementIndex` | `uint8_t` | - | 1 | - | Index of the custom element (0 to `MAX_CUSTOM_ELEMENTS - 1`) | +| `partType` | `uint8_t` | CUSTOM_ELEMENTS_PARTS | 1 | [osdCustomElementType_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-osdcustomelementtype_e) | Type of this part | +| `partValue` | `uint16_t` | CUSTOM_ELEMENTS_PARTS | 2 | - | Value/ID associated with this part | +| `visibilityType` | `uint8_t` | - | 1 | [osdCustomElementTypeVisibility_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-osdcustomelementtypevisibility_e) | Visibility condition source | +| `visibilityValue` | `uint16_t` | - | 2 | - | Value/ID of the visibility condition source | +| `elementText` | `char[OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1]` | - | OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1 | - | Raw bytes | + +**Reply Payload:** **None** + +**Notes:** Payload length must be (OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1) + (CUSTOM_ELEMENTS_PARTS * 3) + 4 bytes including elementIndex. elementIndex must be < MAX_CUSTOM_ELEMENTS. Each partType must be < CUSTOM_ELEMENT_TYPE_END. Firmware NUL-terminates elementText internally. + +## `MSP2_INAV_OUTPUT_MAPPING_EXT2 (8461 / 0x210d)` +**Description:** Retrieves the full extended output mapping configuration (timer ID, full 32-bit usage flags, and pin label). Supersedes `MSP2_INAV_OUTPUT_MAPPING_EXT`. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `timerId` | `uint8_t` | 1 | - | Hardware timer identifier (e.g., `TIM1`, `TIM2`). SITL uses index | +| `usageFlags` | `uint32_t` | 4 | - | Full 32-bit timer usage flags (`TIM_USE_*`) | +| `pinLabel` | `uint8_t` | 1 | [pinLabel_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-pinlabel_e) | Label for special pin usage (`PIN_LABEL_*` enum, e.g., `PIN_LABEL_LED`). 0 (`PIN_LABEL_NONE`) otherwise | + +**Notes:** Provides complete usage flags and helps identify pins repurposed for functions like LED strip. + +## `MSP2_INAV_SERVO_CONFIG (8704 / 0x2200)` +**Description:** Retrieves the configuration parameters for all supported servos (min, max, middle, rate). Supersedes `MSP_SERVO_CONFIGURATIONS`. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `min` | `int16_t` | 2 | PWM | Minimum servo endpoint (`servoParams(i)->min`) | +| `max` | `int16_t` | 2 | PWM | Maximum servo endpoint (`servoParams(i)->max`) | +| `middle` | `int16_t` | 2 | PWM | Middle/Neutral servo position (`servoParams(i)->middle`) | +| `rate` | `int8_t` | 1 | % (-125 to 125) | Servo rate/scaling (`servoParams(i)->rate`) | + +## `MSP2_INAV_SET_SERVO_CONFIG (8705 / 0x2201)` +**Description:** Sets the configuration parameters for a single servo. Supersedes `MSP_SET_SERVO_CONFIGURATION`. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `servoIndex` | `uint8_t` | 1 | Index | Index of the servo to configure (0 to `MAX_SUPPORTED_SERVOS - 1`) | +| `min` | `int16_t` | 2 | PWM | Sets minimum servo endpoint | +| `max` | `int16_t` | 2 | PWM | Sets maximum servo endpoint | +| `middle` | `int16_t` | 2 | PWM | Sets middle/neutral servo position | +| `rate` | `int8_t` | 1 | % (-125 to 125) | Sets servo rate/scaling | + +**Reply Payload:** **None** + +**Notes:** Expects 8 bytes. Returns error if index invalid. Calls `servoComputeScalingFactors()`. + +## `MSP2_INAV_GEOZONE (8720 / 0x2210)` +**Description:** Get configuration for a specific Geozone. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `geozoneIndex` | `uint8_t` | 1 | Index of the geozone (0 to `MAX_GEOZONES_IN_CONFIG - 1`) | + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `geozoneIndex` | `uint8_t` | 1 | - | Index requested | +| `type` | `uint8_t` | 1 | - | Define (`GEOZONE_TYPE_EXCLUSIVE/INCLUSIVE`): Zone type (Inclusion/Exclusion) | +| `shape` | `uint8_t` | 1 | - | Define (`GEOZONE_SHAPE_CIRCULAR/POLYGON`): Zone shape (Polygon/Circular) | +| `minAltitude` | `int32_t` | 4 | cm | Minimum allowed altitude within the zone (`geoZonesConfig(idx)->minAltitude`) | +| `maxAltitude` | `int32_t` | 4 | cm | Maximum allowed altitude within the zone (`geoZonesConfig(idx)->maxAltitude`) | +| `isSeaLevelRef` | `uint8_t` | 1 | - | Boolean: 1 if altitudes are relative to sea level, 0 if relative to home | +| `fenceAction` | `uint8_t` | 1 | [fenceAction_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-fenceaction_e) | Enum (`fenceAction_e`): Action to take upon boundary violation | +| `vertexCount` | `uint8_t` | 1 | - | Number of vertices defined for this zone | + +**Notes:** Requires `USE_GEOZONE`. Used by `mspFcGeozoneOutCommand`. + +## `MSP2_INAV_SET_GEOZONE (8721 / 0x2211)` +**Description:** Sets the main configuration for a specific Geozone (type, shape, altitude, action). **This command resets (clears) all vertices associated with the zone.** + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `geozoneIndex` | `uint8_t` | 1 | - | Index of the geozone (0 to `MAX_GEOZONES_IN_CONFIG - 1`) | +| `type` | `uint8_t` | 1 | - | Define (`GEOZONE_TYPE_EXCLUSIVE/INCLUSIVE`): Zone type (Inclusion/Exclusion) | +| `shape` | `uint8_t` | 1 | - | Define (`GEOZONE_SHAPE_CIRCULAR/POLYGON`): Zone shape (Polygon/Circular) | +| `minAltitude` | `int32_t` | 4 | cm | Minimum allowed altitude (`geoZonesConfigMutable()->minAltitude`) | +| `maxAltitude` | `int32_t` | 4 | cm | Maximum allowed altitude (`geoZonesConfigMutable()->maxAltitude`) | +| `isSeaLevelRef` | `uint8_t` | 1 | - | Boolean: Altitude reference | +| `fenceAction` | `uint8_t` | 1 | [fenceAction_e](https://github.com/iNavFlight/inav/wiki/Enums-reference#enum-fenceaction_e) | Enum (`fenceAction_e`): Action to take upon boundary violation | +| `vertexCount` | `uint8_t` | 1 | - | Number of vertices to be defined (used for validation later) | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_GEOZONE`. Expects 14 bytes. Returns error if index invalid. Calls `geozoneResetVertices()`. Vertices must be set subsequently using `MSP2_INAV_SET_GEOZONE_VERTEX`. + +## `MSP2_INAV_GEOZONE_VERTEX (8722 / 0x2212)` +**Description:** Get a specific vertex (or center+radius for circular zones) of a Geozone. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Description| +|---|---|---|---| +| `geozoneIndex` | `uint8_t` | 1 | Index of the geozone | +| `vertexId` | `uint8_t` | 1 | Index of the vertex within the zone (0-based). For circles, 0 = center | + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `geozoneIndex` | `uint8_t` | 1 | Index | Geozone index requested | +| `vertexId` | `uint8_t` | 1 | Index | Vertex index requested | +| `latitude` | `int32_t` | 4 | deg * 1e7 | Vertex latitude | +| `longitude` | `int32_t` | 4 | deg * 1e7 | Vertex longitude | +| `radius` | `int32_t` | 4 | cm | If vertex is circle, Radius of the circular zone | + +**Notes:** Requires `USE_GEOZONE`. Returns error if indexes are invalid or vertex doesn't exist. For circular zones, the radius is stored internally as the 'latitude' of the vertex with index 1. + +## `MSP2_INAV_SET_GEOZONE_VERTEX (8723 / 0x2213)` +**Description:** Sets a specific vertex (or center+radius for circular zones) for a Geozone. +#### Variant: `polygon` + +**Description:** Polygonal Geozone + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `geozoneIndex` | `uint8_t` | 1 | Index | Geozone index requested | +| `vertexId` | `uint8_t` | 1 | Index | Vertex index requested | +| `latitude` | `int32_t` | 4 | deg * 1e7 | Vertex latitude | +| `longitude` | `int32_t` | 4 | deg * 1e7 | Vertex longitude | + +**Reply Payload:** **None** + +#### Variant: `circle` + +**Description:** Circular Geozone + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `geozoneIndex` | `uint8_t` | 1 | Index | Geozone index requested | +| `vertexId` | `uint8_t` | 1 | Index | Vertex index requested | +| `latitude` | `int32_t` | 4 | deg * 1e7 | Vertex/Center latitude | +| `longitude` | `int32_t` | 4 | deg * 1e7 | Vertex/Center longitude | +| `radius` | `int32_t` | 4 | cm | Radius of the circular zone | + +**Reply Payload:** **None** + + +**Notes:** Requires `USE_GEOZONE`. Expects 10 bytes (Polygon) or 14 bytes (Circular). Returns error if indexes invalid or if trying to set vertex beyond `vertexCount` defined in `MSP2_INAV_SET_GEOZONE`. Calls `geozoneSetVertex()`. For circular zones, sets center (vertex 0) and radius (vertex 1's latitude). + +## `MSP2_INAV_SET_GVAR (8724 / 0x2214)` +**Description:** Sets the specified Global Variable (GVAR) to the provided value. + +**Request Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `gvarIndex` | `uint8_t` | 1 | Index | Index of the Global Variable to set | +| `value` | `int32_t` | 4 | - | New value to store (clamped to configured min/max by `gvSet()`) | + +**Reply Payload:** **None** + +**Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. Expects 5 bytes. Returns error if index is outside `MAX_GLOBAL_VARIABLES`. + +## `MSP2_INAV_FULL_LOCAL_POSE (8736 / 0x2220)` +**Description:** Provides estimates of current attitude, local NEU position, and velocity. + +**Request Payload:** **None** + +**Reply Payload:** +|Field|C Type|Size (Bytes)|Units|Description| +|---|---|---|---|---| +| `roll` | `int16_t` | 2 | deci-degrees | Roll angle (`attitude.values.roll`) | +| `pitch` | `int16_t` | 2 | deci-degrees | Pitch angle (`attitude.values.pitch`) | +| `yaw` | `int16_t` | 2 | deci-degrees | Yaw/Heading angle (`attitude.values.yaw`) | +| `localPositionNorth` | `int32_t` | 4 | cm | Estimated North coordinate in local NEU frame (`posControl.actualState.abs.pos.x`) | +| `localVelocityNorth` | `int16_t` | 2 | cm/s | Estimated North component of velocity in local NEU frame (`posControl.actualState.abs.vel.x`) | +| `localPositionEast` | `int32_t` | 4 | cm | Estimated East coordinate in local NEU frame (`posControl.actualState.abs.pos.y`) | +| `localVelocityEast` | `int16_t` | 2 | cm/s | Estimated East component of velocity in local NEU frame (`posControl.actualState.abs.vel.y`) | +| `localPositionUp` | `int32_t` | 4 | cm | Estimated Up coordinate in local NEU frame (`posControl.actualState.abs.pos.z`) | +| `localVelocityUp` | `int16_t` | 2 | cm/s | Estimated Up component of velocity in local NEU frame (`posControl.actualState.abs.vel.z`) | + +**Notes:** All attitude angles are in deci-degrees. + +## `MSP2_BETAFLIGHT_BIND (12288 / 0x3000)` +**Description:** Initiates the receiver binding procedure for supported serial protocols (CRSF, SRXL2). + +**Request Payload:** **None** + +**Reply Payload:** **None** + +**Notes:** Requires `rxConfig()->receiverType == RX_TYPE_SERIAL`. Requires `USE_SERIALRX_CRSF` or `USE_SERIALRX_SRXL2`. Calls `crsfBind()` or `srxl2Bind()` respectively. Returns error if receiver type or provider is not supported for binding. + diff --git a/docs/development/msp/original_msp_ref.md b/docs/development/msp/original_msp_ref.md new file mode 100644 index 00000000000..d635bbc6843 --- /dev/null +++ b/docs/development/msp/original_msp_ref.md @@ -0,0 +1,3514 @@ + +# WARNING: DEPRECATED, OBSOLETE, FULL OF ERRORS, DO NOT USE AS REFERENCE!!! +# (OBSOLETE) INAV MSP Messages reference + +**Warning: Work in progress**\ +**Generated with Gemini 2.5 Pro Preview O3-25 on source code files**\ +**Verification needed, exercise caution until completely verified for accuracy and cleared**\ +**Refer to source for absolute certainty** + +For details on the structure of MSP, see [The wiki page](https://github.com/iNavFlight/inav/wiki/MSP-V2) + + +**Basic Concepts:** + +* **MSP Versions:** + * **MSPv1:** The original protocol. Uses command IDs from 0 to 254. + * **MSPv2:** An extended version. Uses command IDs from 0x1000 onwards. Can be encapsulated within an MSPv1 frame (`MSP_V2_FRAME` ID 255) or used natively. +* **Direction:** + * **Out:** Message sent *from* the Flight Controller (FC) *to* the Ground Control Station (GCS), OSD, or other peripheral. Usually a request for data or status. + * **In:** Message sent *from* the GCS/OSD *to* the FC. Usually a command to set a parameter, perform an action, or provide data to the FC. + * **In/Out:** Can function in both directions, often used for getting/setting related data where the request might specify a subset (e.g., get specific waypoint, get specific setting info). +* **Payload:** The data carried by the message, following the command ID. The structure (order, type, size of fields) is critical. +* **Data Types:** Common C data types are used (`uint8_t`, `int16_t`, `uint32_t`, `float`, etc.). Pay close attention to signed vs. unsigned types and sizes. +* **Packing:** Data fields are packed sequentially in the order listed. `__attribute__((packed))` is often used in struct definitions to prevent compiler padding. + +--- + +## MSPv1 Core & Versioning Commands (0-5) + +### `MSP_API_VERSION` (1 / 0x01) + +* **Direction:** Out +* **Description:** Provides the MSP protocol version and the INAV API version. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `mspProtocolVersion` | `uint8_t` | 1 | MSP Protocol version (`MSP_PROTOCOL_VERSION`, typically 0) | + | `apiVersionMajor` | `uint8_t` | 1 | INAV API Major version (`API_VERSION_MAJOR`) | + | `apiVersionMinor` | `uint8_t` | 1 | INAV API Minor version (`API_VERSION_MINOR`) | +* **Notes:** Used by configurators to check compatibility. + +### `MSP_FC_VARIANT` (2 / 0x02) + +* **Direction:** Out +* **Description:** Identifies the flight controller firmware variant (e.g., INAV, Betaflight). +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `fcVariantIdentifier` | `char[4]` | 4 | 4-character identifier string (e.g., "INAV"). Defined by `flightControllerIdentifier` | +* **Notes:** See `FLIGHT_CONTROLLER_IDENTIFIER_LENGTH`. + +### `MSP_FC_VERSION` (3 / 0x03) + +* **Direction:** Out +* **Description:** Provides the specific version number of the flight controller firmware. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `fcVersionMajor` | `uint8_t` | 1 | Firmware Major version (`FC_VERSION_MAJOR`) | + | `fcVersionMinor` | `uint8_t` | 1 | Firmware Minor version (`FC_VERSION_MINOR`) | + | `fcVersionPatch` | `uint8_t` | 1 | Firmware Patch level (`FC_VERSION_PATCH_LEVEL`) | + +### `MSP_BOARD_INFO` (4 / 0x04) + +* **Direction:** Out +* **Description:** Provides information about the specific hardware board and its capabilities. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `boardIdentifier` | `char[4]` | 4 | 4-character UPPER CASE board identifier (`TARGET_BOARD_IDENTIFIER`) | + | `hardwareRevision` | `uint16_t` | 2 | Hardware revision number. 0 if not detected (`USE_HARDWARE_REVISION_DETECTION`) | + | `osdSupport` | `uint8_t` | 1 | OSD chip type: 0=None, 2=Onboard (`USE_OSD`). INAV does not support slave OSD (1) | + | `commCapabilities` | `uint8_t` | 1 | Communication capabilities bitmask: Bit 0=VCP support (`USE_VCP`), Bit 1=SoftSerial support (`USE_SOFTSERIAL1`/`2`) | + | `targetNameLength` | `uint8_t` | 1 | Length of the target name string that follows | + | `targetName` | `char[]` | Variable | Target name string (e.g., "MATEKF405"). Length given by previous field | +* **Notes:** `BOARD_IDENTIFIER_LENGTH` is 4. + +### `MSP_BUILD_INFO` (5 / 0x05) + +* **Direction:** Out +* **Description:** Provides build date, time, and Git revision of the firmware. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `buildDate` | `char[11]` | 11 | Build date string (e.g., "Dec 31 2023"). `BUILD_DATE_LENGTH` | + | `buildTime` | `char[8]` | 8 | Build time string (e.g., "23:59:59"). `BUILD_TIME_LENGTH` | + | `gitRevision` | `char[7]` | 7 | Short Git revision string. `GIT_SHORT_REVISION_LENGTH` | + +--- + +## MSPv1 INAV Configuration Commands (6-24) + +### `MSP_INAV_PID` (6 / 0x06) + +* **Direction:** Out +* **Description:** Retrieves legacy INAV-specific PID controller related settings. Many fields are now obsolete or placeholders. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `legacyAsyncProcessing` | `uint8_t` | 1 | - | Legacy, unused. Always 0 | + | `legacyAsyncValue1` | `uint16_t` | 2 | - | Legacy, unused. Always 0 | + | `legacyAsyncValue2` | `uint16_t` | 2 | - | Legacy, unused. Always 0 | + | `headingHoldRateLimit` | `uint8_t` | 1 | deg/s | Max rate for heading hold P term (`pidProfile()->heading_hold_rate_limit`) | + | `headingHoldLpfFreq` | `uint8_t` | 1 | Hz | Fixed LPF frequency for heading hold error (`HEADING_HOLD_ERROR_LPF_FREQ`) | + | `legacyYawJumpLimit` | `uint16_t` | 2 | - | Legacy, unused. Always 0 | + | `legacyGyroLpf` | `uint8_t` | 1 | Hz | Fixed value `GYRO_LPF_256HZ` | + | `accLpfHz` | `uint8_t` | 1 | Hz | Accelerometer LPF frequency (`accelerometerConfig()->acc_lpf_hz`) cutoff frequency for the low pass filter used on the acc z-axis for althold in Hz | + | `reserved1` | `uint8_t` | 1 | - | Reserved. Always 0 | + | `reserved2` | `uint8_t` | 1 | - | Reserved. Always 0 | + | `reserved3` | `uint8_t` | 1 | - | Reserved. Always 0 | + | `reserved4` | `uint8_t` | 1 | - | Reserved. Always 0 | +* **Notes:** Superseded by `MSP2_PID` for core PIDs and other specific messages for filter settings. + +### `MSP_SET_INAV_PID` (7 / 0x07) + +* **Direction:** In +* **Description:** Sets legacy INAV-specific PID controller related settings. +* **Payload:** (Matches `MSP_INAV_PID` structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `legacyAsyncProcessing` | `uint8_t` | 1 | - | Legacy, ignored | + | `legacyAsyncValue1` | `uint16_t` | 2 | - | Legacy, ignored | + | `legacyAsyncValue2` | `uint16_t` | 2 | - | Legacy, ignored | + | `headingHoldRateLimit` | `uint8_t` | 1 | deg/s | Sets `pidProfileMutable()->heading_hold_rate_limit` | + | `headingHoldLpfFreq` | `uint8_t` | 1 | Hz | Ignored (fixed value `HEADING_HOLD_ERROR_LPF_FREQ` used) | + | `legacyYawJumpLimit` | `uint16_t` | 2 | - | Legacy, ignored | + | `legacyGyroLpf` | `uint8_t` | 1 | Enum | Ignored (was gyro LPF) | + | `accLpfHz` | `uint8_t` | 1 | Hz | Sets `accelerometerConfigMutable()->acc_lpf_hz` | + | `reserved1` | `uint8_t` | 1 | - | Ignored | + | `reserved2` | `uint8_t` | 1 | - | Ignored | + | `reserved3` | `uint8_t` | 1 | - | Ignored | + | `reserved4` | `uint8_t` | 1 | - | Ignored | +* **Notes:** Expects 15 bytes. + +### `MSP_NAME` (10 / 0x0A) + +* **Direction:** Out +* **Description:** Returns the user-defined craft name. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `craftName` | `char[]` | Variable | The craft name string (`systemConfig()->craftName`). Null termination is *not* explicitly sent, the length is determined by the payload size | + +### `MSP_SET_NAME` (11 / 0x0B) + +* **Direction:** In +* **Description:** Sets the user-defined craft name. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `craftName` | `char[]` | 1 to `MAX_NAME_LENGTH` | The new craft name string. Automatically null-terminated by the FC | +* **Notes:** Maximum length is `MAX_NAME_LENGTH`. + +### `MSP_NAV_POSHOLD` (12 / 0x0C) + +* **Direction:** Out +* **Description:** Retrieves navigation position hold and general manual/auto flight parameters. Some parameters depend on the platform type (Multirotor vs Fixed Wing). +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `userControlMode` | `uint8_t` | 1 | - | Navigation user control mode NAV_GPS_ATTI (0) or NAV_GPS_CRUISE (1) | + | `maxAutoSpeed` | `uint16_t` | 2 | cm/s | Max speed in autonomous modes (`navConfig()->general.max_auto_speed`) | + | `maxAutoClimbRate` | `uint16_t` | 2 | cm/s | Max climb rate in autonomous modes (uses `fw.max_auto_climb_rate` or `mc.max_auto_climb_rate` based on platform) | + | `maxManualSpeed` | `uint16_t` | 2 | cm/s | Max speed in manual modes with GPS aiding (`navConfig()->general.max_manual_speed`) | + | `maxManualClimbRate` | `uint16_t` | 2 | cm/s | Max climb rate in manual modes with GPS aiding (uses `fw.max_manual_climb_rate` or `mc.max_manual_climb_rate`) | + | `mcMaxBankAngle` | `uint8_t` | 1 | degrees | Max bank angle for multirotor position hold (`navConfig()->mc.max_bank_angle`) | + | `mcAltHoldThrottleType` | `uint8_t` | 1 | Enum | Enum `navMcAltHoldThrottle_e` Sets 'navConfigMutable()->mc.althold_throttle_type' | + | `mcHoverThrottle` | `uint16_t` | 2 | PWM | Multirotor hover throttle (`currentBatteryProfile->nav.mc.hover_throttle`) | + +### `MSP_SET_NAV_POSHOLD` (13 / 0x0D) + +* **Direction:** In +* **Description:** Sets navigation position hold and general manual/auto flight parameters. +* **Payload:** (Matches `MSP_NAV_POSHOLD` structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `userControlMode` | `uint8_t` | 1 | Enum | Sets `navConfigMutable()->general.flags.user_control_mode` | + | `maxAutoSpeed` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->general.max_auto_speed` | + | `maxAutoClimbRate` | `uint16_t` | 2 | cm/s | Sets `fw.max_auto_climb_rate` or `mc.max_auto_climb_rate` based on current platform type | + | `maxManualSpeed` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->general.max_manual_speed` | + | `maxManualClimbRate` | `uint16_t` | 2 | cm/s | Sets `fw.max_manual_climb_rate` or `mc.max_manual_climb_rate` | + | `mcMaxBankAngle` | `uint8_t` | 1 | degrees | Sets `navConfigMutable()->mc.max_bank_angle` | + | `mcAltHoldThrottleType` | `uint8_t` | 1 | Enum | Enum `navMcAltHoldThrottle_e` Sets 'navConfigMutable()->mc.althold_throttle_type' | + | `mcHoverThrottle` | `uint16_t` | 2 | PWM | Sets `currentBatteryProfileMutable->nav.mc.hover_throttle` | +* **Notes:** Expects 13 bytes. + +### `MSP_CALIBRATION_DATA` (14 / 0x0E) + +* **Direction:** Out +* **Description:** Retrieves sensor calibration data (Accelerometer zero/gain, Magnetometer zero/gain, Optical Flow scale). +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `accCalibAxisFlags` | `uint8_t` | 1 | Bitmask | Flags indicating which axes of the accelerometer have been calibrated (`accGetCalibrationAxisFlags()`) | + | `accZeroX` | `uint16_t` | 2 | Raw ADC | Accelerometer zero offset for X-axis (`accelerometerConfig()->accZero.raw[X]`) | + | `accZeroY` | `uint16_t` | 2 | Raw ADC | Accelerometer zero offset for Y-axis (`accelerometerConfig()->accZero.raw[Y]`) | + | `accZeroZ` | `uint16_t` | 2 | Raw ADC | Accelerometer zero offset for Z-axis (`accelerometerConfig()->accZero.raw[Z]`) | + | `accGainX` | `uint16_t` | 2 | Raw ADC | Accelerometer gain/scale for X-axis (`accelerometerConfig()->accGain.raw[X]`) | + | `accGainY` | `uint16_t` | 2 | Raw ADC | Accelerometer gain/scale for Y-axis (`accelerometerConfig()->accGain.raw[Y]`) | + | `accGainZ` | `uint16_t` | 2 | Raw ADC | Accelerometer gain/scale for Z-axis (`accelerometerConfig()->accGain.raw[Z]`) | + | `magZeroX` | `uint16_t` | 2 | Raw ADC | Magnetometer zero offset for X-axis (`compassConfig()->magZero.raw[X]`). 0 if `USE_MAG` disabled | + | `magZeroY` | `uint16_t` | 2 | Raw ADC | Magnetometer zero offset for Y-axis (`compassConfig()->magZero.raw[Y]`). 0 if `USE_MAG` disabled | + | `magZeroZ` | `uint16_t` | 2 | Raw ADC | Magnetometer zero offset for Z-axis (`compassConfig()->magZero.raw[Z]`). 0 if `USE_MAG` disabled | + | `opflowScale` | `uint16_t` | 2 | Scale * 256 | Optical flow scale factor (`opticalFlowConfig()->opflow_scale * 256`). 0 if `USE_OPFLOW` disabled | + | `magGainX` | `uint16_t` | 2 | Raw ADC | Magnetometer gain/scale for X-axis (`compassConfig()->magGain[X]`). 0 if `USE_MAG` disabled | + | `magGainY` | `uint16_t` | 2 | Raw ADC | Magnetometer gain/scale for Y-axis (`compassConfig()->magGain[Y]`). 0 if `USE_MAG` disabled | + | `magGainZ` | `uint16_t` | 2 | Raw ADC | Magnetometer gain/scale for Z-axis (`compassConfig()->magGain[Z]`). 0 if `USE_MAG` disabled | +* **Notes:** Total size 27 bytes. Fields related to optional sensors are zero if the sensor is not used. + +### `MSP_SET_CALIBRATION_DATA` (15 / 0x0F) + +* **Direction:** In +* **Description:** Sets sensor calibration data. +* **Payload:** (Matches `MSP_CALIBRATION_DATA` structure, excluding `accCalibAxisFlags`) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `accZeroX` | `uint16_t` | 2 | Raw ADC | Sets `accelerometerConfigMutable()->accZero.raw[X]` | + | `accZeroY` | `uint16_t` | 2 | Raw ADC | Sets `accelerometerConfigMutable()->accZero.raw[Y]` | + | `accZeroZ` | `uint16_t` | 2 | Raw ADC | Sets `accelerometerConfigMutable()->accZero.raw[Z]` | + | `accGainX` | `uint16_t` | 2 | Raw ADC | Sets `accelerometerConfigMutable()->accGain.raw[X]` | + | `accGainY` | `uint16_t` | 2 | Raw ADC | Sets `accelerometerConfigMutable()->accGain.raw[Y]` | + | `accGainZ` | `uint16_t` | 2 | Raw ADC | Sets `accelerometerConfigMutable()->accGain.raw[Z]` | + | `magZeroX` | `uint16_t` | 2 | Raw ADC | Sets `compassConfigMutable()->magZero.raw[X]` (if `USE_MAG`) | + | `magZeroY` | `uint16_t` | 2 | Raw ADC | Sets `compassConfigMutable()->magZero.raw[Y]` (if `USE_MAG`) | + | `magZeroZ` | `uint16_t` | 2 | Raw ADC | Sets `compassConfigMutable()->magZero.raw[Z]` (if `USE_MAG`) | + | `opflowScale` | `uint16_t` | 2 | Scale * 256 | Sets `opticalFlowConfigMutable()->opflow_scale = value / 256.0f` (if `USE_OPFLOW`) | + | `magGainX` | `uint16_t` | 2 | Raw ADC | Sets `compassConfigMutable()->magGain[X]` (if `USE_MAG`) | + | `magGainY` | `uint16_t` | 2 | Raw ADC | Sets `compassConfigMutable()->magGain[Y]` (if `USE_MAG`) | + | `magGainZ` | `uint16_t` | 2 | Raw ADC | Sets `compassConfigMutable()->magGain[Z]` (if `USE_MAG`) | +* **Notes:** Expects 26 bytes. Ignores values for sensors not enabled by `USE_*` defines. + +### `MSP_POSITION_ESTIMATION_CONFIG` (16 / 0x10) + +* **Direction:** Out +* **Description:** Retrieves parameters related to the INAV position estimation fusion weights and GPS minimum satellite count. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `weightZBaroP` | `uint16_t` | 2 | Weight * 100 | Barometer Z position fusion weight (`positionEstimationConfig()->w_z_baro_p * 100`) | + | `weightZGPSP` | `uint16_t` | 2 | Weight * 100 | GPS Z position fusion weight (`positionEstimationConfig()->w_z_gps_p * 100`) | + | `weightZGPSV` | `uint16_t` | 2 | Weight * 100 | GPS Z velocity fusion weight (`positionEstimationConfig()->w_z_gps_v * 100`) | + | `weightXYGPSP` | `uint16_t` | 2 | Weight * 100 | GPS XY position fusion weight (`positionEstimationConfig()->w_xy_gps_p * 100`) | + | `weightXYGPSV` | `uint16_t` | 2 | Weight * 100 | GPS XY velocity fusion weight (`positionEstimationConfig()->w_xy_gps_v * 100`) | + | `minSats` | `uint8_t` | 1 | Count | Minimum satellites required for GPS use (`gpsConfigMutable()->gpsMinSats`) | + | `useGPSVelNED` | `uint8_t` | 1 | Boolean | Legacy flag, always 1 (GPS velocity is always used if available) | + +### `MSP_SET_POSITION_ESTIMATION_CONFIG` (17 / 0x11) + +* **Direction:** In +* **Description:** Sets parameters related to the INAV position estimation fusion weights and GPS minimum satellite count. +* **Payload:** (Matches `MSP_POSITION_ESTIMATION_CONFIG` structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `weightZBaroP` | `uint16_t` | 2 | Weight * 100 | Sets `positionEstimationConfigMutable()->w_z_baro_p = value / 100.0f` (constrained 0.0-10.0) | + | `weightZGPSP` | `uint16_t` | 2 | Weight * 100 | Sets `positionEstimationConfigMutable()->w_z_gps_p = value / 100.0f` (constrained 0.0-10.0) | + | `weightZGPSV` | `uint16_t` | 2 | Weight * 100 | Sets `positionEstimationConfigMutable()->w_z_gps_v = value / 100.0f` (constrained 0.0-10.0) | + | `weightXYGPSP` | `uint16_t` | 2 | Weight * 100 | Sets `positionEstimationConfigMutable()->w_xy_gps_p = value / 100.0f` (constrained 0.0-10.0) | + | `weightXYGPSV` | `uint16_t` | 2 | Weight * 100 | Sets `positionEstimationConfigMutable()->w_xy_gps_v = value / 100.0f` (constrained 0.0-10.0) | + | `minSats` | `uint8_t` | 1 | Count | Sets `gpsConfigMutable()->gpsMinSats` (constrained 5-10) | + | `useGPSVelNED` | `uint8_t` | 1 | Boolean | Legacy flag, ignored | +* **Notes:** Expects 12 bytes. + +### `MSP_WP_MISSION_LOAD` (18 / 0x12) + +* **Direction:** In +* **Description:** Commands the FC to load the waypoint mission stored in non-volatile memory (e.g., EEPROM or FlashFS) into the active mission buffer. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `missionID` | `uint8_t` | 1 | Reserved for future use, currently ignored | +* **Notes:** Only functional if `NAV_NON_VOLATILE_WAYPOINT_STORAGE` is defined. Requires 1 byte payload. Returns error if loading fails. + +### `MSP_WP_MISSION_SAVE` (19 / 0x13) + +* **Direction:** In +* **Description:** Commands the FC to save the currently active waypoint mission from RAM to non-volatile memory (e.g., EEPROM or FlashFS). +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `missionID` | `uint8_t` | 1 | Reserved for future use, currently ignored | +* **Notes:** Only functional if `NAV_NON_VOLATILE_WAYPOINT_STORAGE` is defined. Requires 1 byte payload. Returns error if saving fails. + +### `MSP_WP_GETINFO` (20 / 0x14) + +* **Direction:** Out +* **Description:** Retrieves information about the waypoint mission capabilities and the status of the currently loaded mission. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `wpCapabilities` | `uint8_t` | 1 | Reserved for future waypoint capabilities flags. Currently always 0 | + | `maxWaypoints` | `uint8_t` | 1 | Maximum number of waypoints supported (`NAV_MAX_WAYPOINTS`) | + | `missionValid` | `uint8_t` | 1 | Boolean flag indicating if the current mission in RAM is valid (`isWaypointListValid()`) | + | `waypointCount` | `uint8_t` | 1 | Number of waypoints currently defined in the mission (`getWaypointCount()`) | + +### `MSP_RTH_AND_LAND_CONFIG` (21 / 0x15) + +* **Direction:** Out +* **Description:** Retrieves configuration parameters related to Return-to-Home (RTH) and automatic landing behaviors. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `minRthDistance` | `uint16_t` | 2 | meters | Minimum distance from home required for RTH to engage (`navConfig()->general.min_rth_distance`) | + | `rthClimbFirst` | `uint8_t` | 1 | Boolean | Flag: Climb to RTH altitude before returning (`navConfig()->general.flags.rth_climb_first`) | + | `rthClimbIgnoreEmerg` | `uint8_t` | 1 | Boolean | Flag: Climb even in emergency RTH (`navConfig()->general.flags.rth_climb_ignore_emerg`) | + | `rthTailFirst` | `uint8_t` | 1 | Boolean | Flag: Multirotor returns tail-first (`navConfig()->general.flags.rth_tail_first`) | + | `rthAllowLanding` | `uint8_t` | 1 | Boolean | Flag: Allow automatic landing after RTH (`navConfig()->general.flags.rth_allow_landing`) | + | `rthAltControlMode` | `uint8_t` | 1 | Enum | RTH altitude control mode (`navConfig()->general.flags.rth_alt_control_mode`) | + | `rthAbortThreshold` | `uint16_t` | 2 | cm/s | Stick input threshold to abort RTH (`navConfig()->general.rth_abort_threshold`) | + | `rthAltitude` | `uint16_t` | 2 | meters | Target RTH altitude (`navConfig()->general.rth_altitude`) | + | `landMinAltVspd` | `uint16_t` | 2 | cm/s | Landing vertical speed at minimum slowdown altitude (`navConfig()->general.land_minalt_vspd`) | + | `landMaxAltVspd` | `uint16_t` | 2 | cm/s | Landing vertical speed at maximum slowdown altitude (`navConfig()->general.land_maxalt_vspd`) | + | `landSlowdownMinAlt` | `uint16_t` | 2 | meters | Altitude below which `landMinAltVspd` applies (`navConfig()->general.land_slowdown_minalt`) | + | `landSlowdownMaxAlt` | `uint16_t` | 2 | meters | Altitude above which `landMaxAltVspd` applies (`navConfig()->general.land_slowdown_maxalt`) | + | `emergDescentRate` | `uint16_t` | 2 | cm/s | Vertical speed during emergency landing descent (`navConfig()->general.emerg_descent_rate`) | + +### `MSP_SET_RTH_AND_LAND_CONFIG` (22 / 0x16) + +* **Direction:** In +* **Description:** Sets configuration parameters related to Return-to-Home (RTH) and automatic landing behaviors. +* **Payload:** (Matches `MSP_RTH_AND_LAND_CONFIG` structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `minRthDistance` | `uint16_t` | 2 | meters | Sets `navConfigMutable()->general.min_rth_distance` | + | `rthClimbFirst` | `uint8_t` | 1 | Boolean | Sets `navConfigMutable()->general.flags.rth_climb_first` | + | `rthClimbIgnoreEmerg` | `uint8_t` | 1 | Boolean | Sets `navConfigMutable()->general.flags.rth_climb_ignore_emerg` | + | `rthTailFirst` | `uint8_t` | 1 | Boolean | Sets `navConfigMutable()->general.flags.rth_tail_first` | + | `rthAllowLanding` | `uint8_t` | 1 | Boolean | Sets `navConfigMutable()->general.flags.rth_allow_landing` | + | `rthAltControlMode` | `uint8_t` | 1 | Enum | Sets `navConfigMutable()->general.flags.rth_alt_control_mode` | + | `rthAbortThreshold` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->general.rth_abort_threshold` | + | `rthAltitude` | `uint16_t` | 2 | meters | Sets `navConfigMutable()->general.rth_altitude` | + | `landMinAltVspd` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->general.land_minalt_vspd` | + | `landMaxAltVspd` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->general.land_maxalt_vspd` | + | `landSlowdownMinAlt` | `uint16_t` | 2 | meters | Sets `navConfigMutable()->general.land_slowdown_minalt` | + | `landSlowdownMaxAlt` | `uint16_t` | 2 | meters | Sets `navConfigMutable()->general.land_slowdown_maxalt` | + | `emergDescentRate` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->general.emerg_descent_rate` | +* **Notes:** Expects 21 bytes. + +### `MSP_FW_CONFIG` (23 / 0x17) + +* **Direction:** Out +* **Description:** Retrieves configuration parameters specific to Fixed Wing navigation. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `cruiseThrottle` | `uint16_t` | 2 | PWM | Cruise throttle level (`currentBatteryProfile->nav.fw.cruise_throttle`) | + | `minThrottle` | `uint16_t` | 2 | PWM | Minimum throttle during autonomous flight (`currentBatteryProfile->nav.fw.min_throttle`) | + | `maxThrottle` | `uint16_t` | 2 | PWM | Maximum throttle during autonomous flight (`currentBatteryProfile->nav.fw.max_throttle`) | + | `maxBankAngle` | `uint8_t` | 1 | degrees | Maximum bank angle allowed (`navConfig()->fw.max_bank_angle`) | + | `maxClimbAngle` | `uint8_t` | 1 | degrees | Maximum pitch angle during climb (`navConfig()->fw.max_climb_angle`) | + | `maxDiveAngle` | `uint8_t` | 1 | degrees | Maximum negative pitch angle during descent (`navConfig()->fw.max_dive_angle`) | + | `pitchToThrottle` | `uint8_t` | 1 | Ratio (%) | Pitch-to-throttle feed-forward ratio (`currentBatteryProfile->nav.fw.pitch_to_throttle`) | + | `loiterRadius` | `uint16_t` | 2 | meters | Default loiter radius (`navConfig()->fw.loiter_radius`) | + +### `MSP_SET_FW_CONFIG` (24 / 0x18) + +* **Direction:** In +* **Description:** Sets configuration parameters specific to Fixed Wing navigation. +* **Payload:** (Matches `MSP_FW_CONFIG` structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `cruiseThrottle` | `uint16_t` | 2 | PWM | Sets `currentBatteryProfileMutable->nav.fw.cruise_throttle` | + | `minThrottle` | `uint16_t` | 2 | PWM | Sets `currentBatteryProfileMutable->nav.fw.min_throttle` | + | `maxThrottle` | `uint16_t` | 2 | PWM | Sets `currentBatteryProfileMutable->nav.fw.max_throttle` | + | `maxBankAngle` | `uint8_t` | 1 | degrees | Sets `navConfigMutable()->fw.max_bank_angle` | + | `maxClimbAngle` | `uint8_t` | 1 | degrees | Sets `navConfigMutable()->fw.max_climb_angle` | + | `maxDiveAngle` | `uint8_t` | 1 | degrees | Sets `navConfigMutable()->fw.max_dive_angle` | + | `pitchToThrottle` | `uint8_t` | 1 | Ratio (%) | Sets `currentBatteryProfileMutable->nav.fw.pitch_to_throttle` | + | `loiterRadius` | `uint16_t` | 2 | meters | Sets `navConfigMutable()->fw.loiter_radius` | +* **Notes:** Expects 12 bytes. + +--- + +## MSPv1 Cleanflight/Betaflight/INAV Feature Commands (34-58) + +These commands were often introduced by Cleanflight or Betaflight and adopted/adapted by INAV. + +### `MSP_MODE_RANGES` (34 / 0x22) + +* **Direction:** Out +* **Description:** Returns all defined mode activation ranges (aux channel assignments for flight modes). +* **Payload:** Repeated `MAX_MODE_ACTIVATION_CONDITION_COUNT` times: + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `modePermanentId` | `uint8_t` | 1 | ID | Permanent ID of the flight mode (maps to `boxId` via `findBoxByActiveBoxId`). 0 if entry unused | + | `auxChannelIndex` | `uint8_t` | 1 | Index | 0-based index of the AUX channel used for activation | + | `rangeStartStep` | `uint8_t` | 1 | 0-20 | Start step (corresponding to channel value range 900-2100 in steps of 50/25, depends on steps calculation) | + | `rangeEndStep` | `uint8_t` | 1 | 0-20 | End step for the activation range | +* **Notes:** The number of steps and mapping to PWM values depends on internal range calculations. + +### `MSP_SET_MODE_RANGE` (35 / 0x23) + +* **Direction:** In +* **Description:** Sets a single mode activation range by its index. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `rangeIndex` | `uint8_t` | 1 | Index | Index of the mode range to set (0 to `MAX_MODE_ACTIVATION_CONDITION_COUNT - 1`) | + | `modePermanentId` | `uint8_t` | 1 | ID | Permanent ID of the flight mode to assign | + | `auxChannelIndex` | `uint8_t` | 1 | Index | 0-based index of the AUX channel | + | `rangeStartStep` | `uint8_t` | 1 | 0-20 | Start step for activation | + | `rangeEndStep` | `uint8_t` | 1 | 0-20 | End step for activation | +* **Notes:** Expects 5 bytes. Updates the mode configuration and recalculates used mode flags. Returns error if `rangeIndex` or `modePermanentId` is invalid. + +### `MSP_FEATURE` (36 / 0x24) + +* **Direction:** Out +* **Description:** Returns a bitmask of enabled features. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `featureMask` | `uint32_t` | 4 | Bitmask of active features (see `featureMask()`) | +* **Notes:** Feature bits are defined in `feature.h`. + +### `MSP_SET_FEATURE` (37 / 0x25) + +* **Direction:** In +* **Description:** Sets the enabled features using a bitmask. Clears all previous features first. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `featureMask` | `uint32_t` | 4 | Bitmask of features to enable | +* **Notes:** Expects 4 bytes. Updates feature configuration and related settings (e.g., RSSI source). + +### `MSP_BOARD_ALIGNMENT` (38 / 0x26) + +* **Direction:** Out +* **Description:** Returns the sensor board alignment angles relative to the craft frame. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `rollAlign` | `uint16_t` | 2 | deci-degrees | Board alignment roll angle (`boardAlignment()->rollDeciDegrees`) | + | `pitchAlign` | `uint16_t` | 2 | deci-degrees | Board alignment pitch angle (`boardAlignment()->pitchDeciDegrees`) | + | `yawAlign` | `uint16_t` | 2 | deci-degrees | Board alignment yaw angle (`boardAlignment()->yawDeciDegrees`) | + +### `MSP_SET_BOARD_ALIGNMENT` (39 / 0x27) + +* **Direction:** In +* **Description:** Sets the sensor board alignment angles. +* **Payload:** (Matches `MSP_BOARD_ALIGNMENT` structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `rollAlign` | `uint16_t` | 2 | deci-degrees | Sets `boardAlignmentMutable()->rollDeciDegrees` | + | `pitchAlign` | `uint16_t` | 2 | deci-degrees | Sets `boardAlignmentMutable()->pitchDeciDegrees` | + | `yawAlign` | `uint16_t` | 2 | deci-degrees | Sets `boardAlignmentMutable()->yawDeciDegrees` | +* **Notes:** Expects 6 bytes. + +### `MSP_CURRENT_METER_CONFIG` (40 / 0x28) + +* **Direction:** Out +* **Description:** Retrieves the configuration for the current sensor. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `scale` | `uint16_t` | 2 | mV/10A or similar | Current sensor scale factor (`batteryMetersConfig()->current.scale`). Units depend on sensor type | + | `offset` | `uint16_t` | 2 | mV | Current sensor offset (`batteryMetersConfig()->current.offset`) | + | `type` | `uint8_t` | 1 | Enum | Enum `currentSensor_e` Type of current sensor hardware | + | `capacity` | `uint16_t` | 2 | mAh (legacy) | Battery capacity (constrained 0-65535) (`currentBatteryProfile->capacity.value`). Note: This is legacy, use `MSP2_INAV_BATTERY_CONFIG` for full 32-bit capacity | + +### `MSP_SET_CURRENT_METER_CONFIG` (41 / 0x29) + +* **Direction:** In +* **Description:** Sets the configuration for the current sensor. +* **Payload:** (Matches `MSP_CURRENT_METER_CONFIG` structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `scale` | `uint16_t` | 2 | mV/10A or similar | Sets `batteryMetersConfigMutable()->current.scale` | + | `offset` | `uint16_t` | 2 | mV | Sets `batteryMetersConfigMutable()->current.offset` | + | `type` | `uint8_t` | 1 | Enum | Enum `currentSensor_e` Sets 'batteryMetersConfigMutable()->current.type' | + | `capacity` | `uint16_t` | 2 | mAh (legacy) | Sets `currentBatteryProfileMutable->capacity.value` (truncated to 16 bits) | +* **Notes:** Expects 7 bytes. + +### `MSP_MIXER` (42 / 0x2A) + +* **Direction:** Out +* **Description:** Retrieves the mixer type (Legacy, INAV always returns QuadX). +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `mixerMode` | `uint8_t` | 1 | Always 3 (QuadX) in INAV for compatibility | +* **Notes:** This command is largely obsolete. Mixer configuration is handled differently in INAV (presets, custom mixes). See `MSP2_INAV_MIXER`. + +### `MSP_SET_MIXER` (43 / 0x2B) + +* **Direction:** In +* **Description:** Sets the mixer type (Legacy, ignored by INAV). +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `mixerMode` | `uint8_t` | 1 | Mixer mode to set (ignored by INAV) | +* **Notes:** Expects 1 byte. Calls `mixerUpdateStateFlags()` for potential side effects related to presets. + +### `MSP_RX_CONFIG` (44 / 0x2C) + +* **Direction:** Out +* **Description:** Retrieves receiver configuration settings. Some fields are Betaflight compatibility placeholders. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `serialRxProvider` | `uint8_t` | 1 | Enum | Enum `rxSerialReceiverType_e` sets Serial RX provider type ('rxConfig()->serialrx_provider') | + | `maxCheck` | `uint16_t` | 2 | PWM | Upper channel value threshold for stick commands (`rxConfig()->maxcheck`) | + | `midRc` | `uint16_t` | 2 | PWM | Center channel value (`PWM_RANGE_MIDDLE`, typically 1500) | + | `minCheck` | `uint16_t` | 2 | PWM | Lower channel value threshold for stick commands (`rxConfig()->mincheck`) | + | `spektrumSatBind` | `uint8_t` | 1 | Count/Flag | Spektrum bind pulses (`rxConfig()->spektrum_sat_bind`). 0 if `USE_SPEKTRUM_BIND` disabled | + | `rxMinUsec` | `uint16_t` | 2 | µs | Minimum expected pulse width (`rxConfig()->rx_min_usec`) | + | `rxMaxUsec` | `uint16_t` | 2 | µs | Maximum expected pulse width (`rxConfig()->rx_max_usec`) | + | `bfCompatRcInterpolation` | `uint8_t` | 1 | - | BF compatibility. Always 0 | + | `bfCompatRcInterpolationInt` | `uint8_t` | 1 | - | BF compatibility. Always 0 | + | `bfCompatAirModeThreshold` | `uint16_t` | 2 | - | BF compatibility. Always 0 | + | `reserved1` | `uint8_t` | 1 | - | Reserved/Padding. Always 0 | + | `reserved2` | `uint32_t` | 4 | - | Reserved/Padding. Always 0 | + | `reserved3` | `uint8_t` | 1 | - | Reserved/Padding. Always 0 | + | `bfCompatFpvCamAngle` | `uint8_t` | 1 | - | BF compatibility. Always 0 | + | `receiverType` | `uint8_t` | 1 | Enum | Enum `rxReceiverType_e` Receiver type (Parallel PWM, PPM, Serial) ('rxConfig()->receiverType') | + +### `MSP_SET_RX_CONFIG` (45 / 0x2D) + +* **Direction:** In +* **Description:** Sets receiver configuration settings. +* **Payload:** (Matches `MSP_RX_CONFIG` structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `serialRxProvider` | `uint8_t` | 1 | Enum | Enum `rxSerialReceiverType_e` Serial RX provider type ('rxConfig()->serialrx_provider') | + | `maxCheck` | `uint16_t` | 2 | PWM | Sets `rxConfigMutable()->maxcheck` | + | `midRc` | `uint16_t` | 2 | PWM | Ignored (`PWM_RANGE_MIDDLE` is used) | + | `minCheck` | `uint16_t` | 2 | PWM | Sets `rxConfigMutable()->mincheck` | + | `spektrumSatBind` | `uint8_t` | 1 | Count/Flag | Sets `rxConfigMutable()->spektrum_sat_bind` (if `USE_SPEKTRUM_BIND`) | + | `rxMinUsec` | `uint16_t` | 2 | µs | Sets `rxConfigMutable()->rx_min_usec` | + | `rxMaxUsec` | `uint16_t` | 2 | µs | Sets `rxConfigMutable()->rx_max_usec` | + | `bfCompatRcInterpolation` | `uint8_t` | 1 | - | Ignored | + | `bfCompatRcInterpolationInt` | `uint8_t` | 1 | - | Ignored | + | `bfCompatAirModeThreshold` | `uint16_t` | 2 | - | Ignored | + | `reserved1` | `uint8_t` | 1 | - | Ignored | + | `reserved2` | `uint32_t` | 4 | - | Ignored | + | `reserved3` | `uint8_t` | 1 | - | Ignored | + | `bfCompatFpvCamAngle` | `uint8_t` | 1 | - | Ignored | + | `receiverType` | `uint8_t` | 1 | Enum | Enum `rxReceiverType_e` Sets 'rxConfigMutable()->receiverType' | +* **Notes:** Expects 24 bytes. + +### `MSP_LED_COLORS` (46 / 0x2E) + +* **Direction:** Out +* **Description:** Retrieves the HSV color definitions for configurable LED colors. +* **Payload:** Repeated `LED_CONFIGURABLE_COLOR_COUNT` times: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `hue` | `uint16_t` | 2 | Hue value (0-359) | + | `saturation` | `uint8_t` | 1 | Saturation value (0-255) | + | `value` | `uint8_t` | 1 | Value/Brightness (0-255) | +* **Notes:** Only available if `USE_LED_STRIP` is defined. + +### `MSP_SET_LED_COLORS` (47 / 0x2F) + +* **Direction:** In +* **Description:** Sets the HSV color definitions for configurable LED colors. +* **Payload:** Repeated `LED_CONFIGURABLE_COLOR_COUNT` times: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `hue` | `uint16_t` | 2 | Hue value (0-359) | + | `saturation` | `uint8_t` | 1 | Saturation value (0-255) | + | `value` | `uint8_t` | 1 | Value/Brightness (0-255) | +* **Notes:** Only available if `USE_LED_STRIP` is defined. Expects `LED_CONFIGURABLE_COLOR_COUNT * 4` bytes. + +### `MSP_LED_STRIP_CONFIG` (48 / 0x30) + +* **Direction:** Out +* **Description:** Retrieves the configuration for each LED on the strip (legacy packed format). +* **Payload:** Repeated `LED_MAX_STRIP_LENGTH` times: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `legacyLedConfig` | `uint32_t` | 4 | Packed LED configuration (position, function, overlay, color, direction, params). See C code for bit packing details | +* **Notes:** Only available if `USE_LED_STRIP` is defined. Superseded by `MSP2_INAV_LED_STRIP_CONFIG_EX` which uses a clearer struct. + +### `MSP_SET_LED_STRIP_CONFIG` (49 / 0x31) + +* **Direction:** In +* **Description:** Sets the configuration for a single LED on the strip using the legacy packed format. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `ledIndex` | `uint8_t` | 1 | Index of the LED to configure (0 to `LED_MAX_STRIP_LENGTH - 1`) | + | `legacyLedConfig` | `uint32_t` | 4 | Packed LED configuration to set | +* **Notes:** Only available if `USE_LED_STRIP` is defined. Expects 5 bytes. Calls `reevaluateLedConfig()`. Superseded by `MSP2_INAV_SET_LED_STRIP_CONFIG_EX`. + +### `MSP_RSSI_CONFIG` (50 / 0x32) + +* **Direction:** Out +* **Description:** Retrieves the channel used for analog RSSI input. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `rssiChannel` | `uint8_t` | 1 | AUX channel index (1-based) used for RSSI, or 0 if disabled (`rxConfig()->rssi_channel`) | + +### `MSP_SET_RSSI_CONFIG` (51 / 0x33) + +* **Direction:** In +* **Description:** Sets the channel used for analog RSSI input. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `rssiChannel` | `uint8_t` | 1 | AUX channel index (1-based) to use for RSSI, or 0 to disable | +* **Notes:** Expects 1 byte. Input value is constrained 0 to `MAX_SUPPORTED_RC_CHANNEL_COUNT`. Updates the effective RSSI source. + +### `MSP_ADJUSTMENT_RANGES` (52 / 0x34) + +* **Direction:** Out +* **Description:** Returns all defined RC adjustment ranges (tuning via aux channels). +* **Payload:** Repeated `MAX_ADJUSTMENT_RANGE_COUNT` times: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `adjustmentIndex` | `uint8_t` | 1 | Index of the adjustment slot (0 to `MAX_SIMULTANEOUS_ADJUSTMENT_COUNT - 1`) | + | `auxChannelIndex` | `uint8_t` | 1 | 0-based index of the AUX channel controlling the adjustment value | + | `rangeStartStep` | `uint8_t` | 1 | Start step (0-20) of the control channel range | + | `rangeEndStep` | `uint8_t` | 1 | End step (0-20) of the control channel range | + | `adjustmentFunction` | `uint8_t` | 1 | Function/parameter being adjusted (e.g., PID gain, rate). See `rcAdjustments.h` | + | `auxSwitchChannelIndex` | `uint8_t` | 1 | 0-based index of the AUX channel acting as an enable switch (or 0 if always enabled) | +* **Notes:** See `adjustmentRange_t`. + +### `MSP_SET_ADJUSTMENT_RANGE` (53 / 0x35) + +* **Direction:** In +* **Description:** Sets a single RC adjustment range configuration by its index. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `rangeIndex` | `uint8_t` | 1 | Index of the adjustment range to set (0 to `MAX_ADJUSTMENT_RANGE_COUNT - 1`) | + | `adjustmentIndex` | `uint8_t` | 1 | Adjustment slot index (0 to `MAX_SIMULTANEOUS_ADJUSTMENT_COUNT - 1`) | + | `auxChannelIndex` | `uint8_t` | 1 | 0-based index of the control AUX channel | + | `rangeStartStep` | `uint8_t` | 1 | Start step (0-20) | + | `rangeEndStep` | `uint8_t` | 1 | End step (0-20) | + | `adjustmentFunction` | `uint8_t` | 1 | Function/parameter being adjusted | + | `auxSwitchChannelIndex` | `uint8_t` | 1 | 0-based index of the enable switch AUX channel (or 0) | +* **Notes:** Expects 7 bytes. Returns error if `rangeIndex` or `adjustmentIndex` is invalid. + +### `MSP_CF_SERIAL_CONFIG` (54 / 0x36) + +* **Direction:** Out +* **Description:** Deprecated command to get serial port configuration. +* **Notes:** Not implemented in INAV `fc_msp.c`. Use `MSP2_COMMON_SERIAL_CONFIG`. + +### `MSP_SET_CF_SERIAL_CONFIG` (55 / 0x37) + +* **Direction:** In +* **Description:** Deprecated command to set serial port configuration. +* **Notes:** Not implemented in INAV `fc_msp.c`. Use `MSP2_COMMON_SET_SERIAL_CONFIG`. + +### `MSP_VOLTAGE_METER_CONFIG` (56 / 0x38) + +* **Direction:** Out +* **Description:** Retrieves legacy voltage meter configuration (scaled values). +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `vbatScale` | `uint8_t` | 1 | Scale / 10 | Voltage sensor scale factor / 10 (`batteryMetersConfig()->voltage.scale / 10`). 0 if `USE_ADC` disabled | + | `vbatMinCell` | `uint8_t` | 1 | 0.1V | Minimum cell voltage / 10 (`currentBatteryProfile->voltage.cellMin / 10`). 0 if `USE_ADC` disabled | + | `vbatMaxCell` | `uint8_t` | 1 | 0.1V | Maximum cell voltage / 10 (`currentBatteryProfile->voltage.cellMax / 10`). 0 if `USE_ADC` disabled | + | `vbatWarningCell` | `uint8_t` | 1 | 0.1V | Warning cell voltage / 10 (`currentBatteryProfile->voltage.cellWarning / 10`). 0 if `USE_ADC` disabled | +* **Notes:** Superseded by `MSP2_INAV_BATTERY_CONFIG`. + +### `MSP_SET_VOLTAGE_METER_CONFIG` (57 / 0x39) + +* **Direction:** In +* **Description:** Sets legacy voltage meter configuration (scaled values). +* **Payload:** (Matches `MSP_VOLTAGE_METER_CONFIG` structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `vbatScale` | `uint8_t` | 1 | Scale / 10 | Sets `batteryMetersConfigMutable()->voltage.scale = value * 10` (if `USE_ADC`) | + | `vbatMinCell` | `uint8_t` | 1 | 0.1V | Sets `currentBatteryProfileMutable->voltage.cellMin = value * 10` (if `USE_ADC`) | + | `vbatMaxCell` | `uint8_t` | 1 | 0.1V | Sets `currentBatteryProfileMutable->voltage.cellMax = value * 10` (if `USE_ADC`) | + | `vbatWarningCell` | `uint8_t` | 1 | 0.1V | Sets `currentBatteryProfileMutable->voltage.cellWarning = value * 10` (if `USE_ADC`) | +* **Notes:** Expects 4 bytes. Superseded by `MSP2_INAV_SET_BATTERY_CONFIG`. + +### `MSP_SONAR_ALTITUDE` (58 / 0x3A) + +* **Direction:** Out +* **Description:** Retrieves the altitude measured by the primary rangefinder (sonar or lidar). +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `rangefinderAltitude` | `uint32_t` | 4 | cm | Latest altitude reading from the rangefinder (`rangefinderGetLatestAltitude()`). 0 if `USE_RANGEFINDER` disabled or no reading | + +--- + +## MSPv1 Baseflight/INAV Commands (64-99, plus others) + +These commands originated in Baseflight or were added later in similar ranges. + +### `MSP_RX_MAP` (64 / 0x40) + +* **Direction:** Out +* **Description:** Retrieves the RC channel mapping array (AETR, etc.). +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `rcMap` | `uint8_t[MAX_MAPPABLE_RX_INPUTS]` | `MAX_MAPPABLE_RX_INPUTS` | Array defining the mapping from input channel index to logical function (Roll, Pitch, Yaw, Throttle, Aux1...) | +* **Notes:** `MAX_MAPPABLE_RX_INPUTS` is typically 8 or more. + +### `MSP_SET_RX_MAP` (65 / 0x41) + +* **Direction:** In +* **Description:** Sets the RC channel mapping array. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `rcMap` | `uint8_t[MAX_MAPPABLE_RX_INPUTS]` | `MAX_MAPPABLE_RX_INPUTS` | Array defining the new channel mapping | +* **Notes:** Expects `MAX_MAPPABLE_RX_INPUTS` bytes. + +### `MSP_REBOOT` (68 / 0x44) + +* **Direction:** Out (but triggers an action) +* **Description:** Commands the flight controller to reboot. +* **Payload:** None +* **Notes:** The FC sends an ACK *before* rebooting. The `mspPostProcessFn` is set to `mspRebootFn` to perform the reboot after the reply is sent. Will fail if the craft is armed. + +### `MSP_DATAFLASH_SUMMARY` (70 / 0x46) + +* **Direction:** Out +* **Description:** Retrieves summary information about the onboard dataflash chip (if present and used for Blackbox via FlashFS). +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `flashReady` | `uint8_t` | 1 | Boolean: 1 if flash chip is ready, 0 otherwise. (`flashIsReady()`). 0 if `USE_FLASHFS` disabled | + | `sectorCount` | `uint32_t` | 4 | Total number of sectors on the flash chip (`geometry->sectors`). 0 if `USE_FLASHFS` disabled | + | `totalSize` | `uint32_t` | 4 | Total size of the flash chip in bytes (`geometry->totalSize`). 0 if `USE_FLASHFS` disabled | + | `usedSize` | `uint32_t` | 4 | Currently used size in bytes (FlashFS offset) (`flashfsGetOffset()`). 0 if `USE_FLASHFS` disabled | +* **Notes:** Requires `USE_FLASHFS`. + +### `MSP_DATAFLASH_READ` (71 / 0x47) + +* **Direction:** In/Out +* **Description:** Reads a block of data from the onboard dataflash (FlashFS). +* **Request Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `address` | `uint32_t` | 4 | Starting address to read from within the FlashFS volume | + | `size` | `uint16_t` | 2 | (Optional) Number of bytes to read. Defaults to 128 if not provided | +* **Reply Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `address` | `uint32_t` | 4 | The starting address from which data was actually read | + | `data` | `uint8_t[]` | Variable | The data read from flash. Length is MIN(requested size, remaining buffer space, remaining flashfs data) | +* **Notes:** Requires `USE_FLASHFS`. Read length may be truncated by buffer size or end of flashfs volume. + +### `MSP_DATAFLASH_ERASE` (72 / 0x48) + +* **Direction:** In +* **Description:** Erases the entire onboard dataflash chip (FlashFS volume). +* **Payload:** None +* **Notes:** Requires `USE_FLASHFS`. This is a potentially long operation. Use with caution. + +### `MSP_LOOP_TIME` (73 / 0x49) + +* **Direction:** Out +* **Description:** Retrieves the configured loop time (PID loop frequency denominator). +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `looptime` | `uint16_t` | 2 | µs | Configured loop time (`gyroConfig()->looptime`) | +* **Notes:** This is the *configured* target loop time, not necessarily the *actual* measured cycle time (see `MSP_STATUS`). + +### `MSP_SET_LOOP_TIME` (74 / 0x4A) + +* **Direction:** In +* **Description:** Sets the configured loop time. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `looptime` | `uint16_t` | 2 | µs | New loop time to set (`gyroConfigMutable()->looptime`) | +* **Notes:** Expects 2 bytes. + +### `MSP_FAILSAFE_CONFIG` (75 / 0x4B) + +* **Direction:** Out +* **Description:** Retrieves the failsafe configuration settings. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `failsafeDelay` | `uint8_t` | 1 | 0.1s | Delay before failsafe stage 1 activates (`failsafeConfig()->failsafe_delay`) | + | `failsafeOffDelay` | `uint8_t` | 1 | 0.1s | Delay after signal recovery before returning control (`failsafeConfig()->failsafe_off_delay`) | + | `failsafeThrottle` | `uint16_t` | 2 | PWM | Throttle level during failsafe stage 2 (`currentBatteryProfile->failsafe_throttle`) | + | `legacyKillSwitch` | `uint8_t` | 1 | - | Legacy flag, always 0 | + | `failsafeThrottleLowDelay` | `uint16_t` | 2 | ms | Delay for throttle-based failsafe detection (`failsafeConfig()->failsafe_throttle_low_delay`) | + | `failsafeProcedure` | `uint8_t` | 1 | Enum | Enum `failsafeProcedure_e` Failsafe procedure (Drop, RTH, Land, etc.) ('failsafeConfig()->failsafe_procedure') | + | `failsafeRecoveryDelay` | `uint8_t` | 1 | 0.1s | Delay after RTH finishes before attempting recovery (`failsafeConfig()->failsafe_recovery_delay`) | + | `failsafeFWRollAngle` | `uint16_t` | 2 | deci-degrees | Fixed wing failsafe roll angle (`failsafeConfig()->failsafe_fw_roll_angle`) | + | `failsafeFWPitchAngle` | `uint16_t` | 2 | deci-degrees | Fixed wing failsafe pitch angle (`failsafeConfig()->failsafe_fw_pitch_angle`) | + | `failsafeFWYawRate` | `uint16_t` | 2 | deg/s | Fixed wing failsafe yaw rate (`failsafeConfig()->failsafe_fw_yaw_rate`) | + | `failsafeStickThreshold` | `uint16_t` | 2 | PWM units | Stick movement threshold to exit failsafe (`failsafeConfig()->failsafe_stick_motion_threshold`) | + | `failsafeMinDistance` | `uint16_t` | 2 | meters | Minimum distance from home for RTH failsafe (`failsafeConfig()->failsafe_min_distance`) | + | `failsafeMinDistanceProc` | `uint8_t` | 1 | Enum | Enum `failsafeProcedure_e` Failsafe procedure if below min distance ('failsafeConfig()->failsafe_min_distance_procedure') | + +### `MSP_SET_FAILSAFE_CONFIG` (76 / 0x4C) + +* **Direction:** In +* **Description:** Sets the failsafe configuration settings. +* **Payload:** (Matches `MSP_FAILSAFE_CONFIG` structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `failsafeDelay` | `uint8_t` | 1 | 0.1s | Sets `failsafeConfigMutable()->failsafe_delay` | + | `failsafeOffDelay` | `uint8_t` | 1 | 0.1s | Sets `failsafeConfigMutable()->failsafe_off_delay` | + | `failsafeThrottle` | `uint16_t` | 2 | PWM | Sets `currentBatteryProfileMutable->failsafe_throttle` | + | `legacyKillSwitch` | `uint8_t` | 1 | - | Ignored | + | `failsafeThrottleLowDelay` | `uint16_t` | 2 | ms | Sets `failsafeConfigMutable()->failsafe_throttle_low_delay` | + | `failsafeProcedure` | `uint8_t` | 1 | Enum | Enum `failsafeProcedure_e` Sets 'failsafeConfigMutable()->failsafe_procedure' | + | `failsafeRecoveryDelay` | `uint8_t` | 1 | 0.1s | Sets `failsafeConfigMutable()->failsafe_recovery_delay` | + | `failsafeFWRollAngle` | `uint16_t` | 2 | deci-degrees | Sets `failsafeConfigMutable()->failsafe_fw_roll_angle` (casted to `int16_t`) | + | `failsafeFWPitchAngle` | `uint16_t` | 2 | deci-degrees | Sets `failsafeConfigMutable()->failsafe_fw_pitch_angle` (casted to `int16_t`) | + | `failsafeFWYawRate` | `uint16_t` | 2 | deg/s | Sets `failsafeConfigMutable()->failsafe_fw_yaw_rate` (casted to `int16_t`) | + | `failsafeStickThreshold` | `uint16_t` | 2 | PWM units | Sets `failsafeConfigMutable()->failsafe_stick_motion_threshold` | + | `failsafeMinDistance` | `uint16_t` | 2 | meters | Sets `failsafeConfigMutable()->failsafe_min_distance` | + | `failsafeMinDistanceProc` | `uint8_t` | 1 | Enum | Enum `failsafeProcedure_e` Sets 'failsafeConfigMutable()->failsafe_min_distance_procedure' | +* **Notes:** Expects 20 bytes. + +### `MSP_SDCARD_SUMMARY` (79 / 0x4F) + +* **Direction:** Out +* **Description:** Retrieves summary information about the SD card status and filesystem. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `sdCardSupported` | `uint8_t` | 1 | Bitmask: Bit 0 = 1 if SD card support compiled in (`USE_SDCARD`) | + | `sdCardState` | `uint8_t` | 1 | Enum (`mspSDCardState_e`): Current state (Not Present, Fatal, Card Init, FS Init, Ready). 0 if `USE_SDCARD` disabled | + | `fsError` | `uint8_t` | 1 | Last filesystem error code (`afatfs_getLastError()`). 0 if `USE_SDCARD` disabled | + | `freeSpaceKB` | `uint32_t` | 4 | Free space in KiB (`afatfs_getContiguousFreeSpace() / 1024`). 0 if `USE_SDCARD` disabled | + | `totalSpaceKB` | `uint32_t` | 4 | Total space in KiB (`sdcard_getMetadata()->numBlocks / 2`). 0 if `USE_SDCARD` disabled | +* **Notes:** Requires `USE_SDCARD` and `USE_ASYNCFATFS`. + +### `MSP_BLACKBOX_CONFIG` (80 / 0x50) + +* **Direction:** Out +* **Description:** Legacy command to retrieve Blackbox configuration. Superseded by `MSP2_BLACKBOX_CONFIG`. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `blackboxDevice` | `uint8_t` | 1 | Always 0 (API no longer supported) | + | `blackboxRateNum` | `uint8_t` | 1 | Always 0 | + | `blackboxRateDenom` | `uint8_t` | 1 | Always 0 | + | `blackboxPDenom` | `uint8_t` | 1 | Always 0 | +* **Notes:** Returns fixed zero values. Use `MSP2_BLACKBOX_CONFIG`. + +### `MSP_SET_BLACKBOX_CONFIG` (81 / 0x51) + +* **Direction:** In +* **Description:** Legacy command to set Blackbox configuration. Superseded by `MSP2_SET_BLACKBOX_CONFIG`. +* **Payload:** (Ignored) +* **Notes:** Not implemented in `fc_msp.c`. Use `MSP2_SET_BLACKBOX_CONFIG`. + +### `MSP_TRANSPONDER_CONFIG` (82 / 0x52) + +* **Direction:** Out +* **Description:** Get VTX Transponder settings (likely specific to RaceFlight/Betaflight, not standard INAV VTX). +* **Notes:** Not implemented in INAV `fc_msp.c`. + +### `MSP_SET_TRANSPONDER_CONFIG` (83 / 0x53) + +* **Direction:** In +* **Description:** Set VTX Transponder settings. +* **Notes:** Not implemented in INAV `fc_msp.c`. + +### `MSP_OSD_CONFIG` (84 / 0x54) + +* **Direction:** Out +* **Description:** Retrieves OSD configuration settings and layout for screen 0. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `osdDriverType` | `uint8_t` | 1 | Enum | Enum `OSD_DRIVER_MAX7456` if `USE_OSD`, else `OSD_DRIVER_NONE` | + | `videoSystem` | `uint8_t` | 1 | Enum | Enum `videoSystem_e`: Video system (Auto/PAL/NTSC) (`osdConfig()->video_system`). Sent even if OSD disabled | + | `units` | `uint8_t` | 1 | Enum | Enum `osd_unit_e` Measurement units (Metric/Imperial) (`osdConfig()->units`). Sent even if OSD disabled | + | `rssiAlarm` | `uint8_t` | 1 | % | RSSI alarm threshold (`osdConfig()->rssi_alarm`). Sent even if OSD disabled | + | `capAlarm` | `uint16_t` | 2 | mAh/mWh | Capacity alarm threshold (`currentBatteryProfile->capacity.warning`). Sent even if OSD disabled | + | `timerAlarm` | `uint16_t` | 2 | seconds | Timer alarm threshold (`osdConfig()->time_alarm`). Sent even if OSD disabled | + | `altAlarm` | `uint16_t` | 2 | meters | Altitude alarm threshold (`osdConfig()->alt_alarm`). Sent even if OSD disabled | + | `distAlarm` | `uint16_t` | 2 | meters | Distance alarm threshold (`osdConfig()->dist_alarm`). Sent even if OSD disabled | + | `negAltAlarm` | `uint16_t` | 2 | meters | Negative altitude alarm threshold (`osdConfig()->neg_alt_alarm`). Sent even if OSD disabled | + | `itemPositions` | `uint16_t[OSD_ITEM_COUNT]` | `OSD_ITEM_COUNT * 2` | Coordinates | Packed X/Y position for each OSD item on screen 0 (`osdLayoutsConfig()->item_pos[0][i]`). Sent even if OSD disabled | +* **Notes:** Requires `USE_OSD` for meaningful data, but payload is always sent. Coordinates are packed: `(Y << 8) | X`. See `MSP2_INAV_OSD_*` commands for more detail and multi-layout support. + +### `MSP_SET_OSD_CONFIG` (85 / 0x55) + +* **Direction:** In +* **Description:** Sets OSD configuration or a single item's position on screen 0. +* **Payload Format 1 (Set General Config):** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `addr` | `uint8_t` | 1 | - | Must be 0xFF (-1) | + | `videoSystem` | `uint8_t` | 1 | Enum | Enum `videoSystem_e` Sets `osdConfigMutable()->video_system` | + | `units` | `uint8_t` | 1 | Enum | Enum `osd_unit_e` Sets `osdConfigMutable()->units` | + | `rssiAlarm` | `uint8_t` | 1 | % | Sets `osdConfigMutable()->rssi_alarm` | + | `capAlarm` | `uint16_t` | 2 | mAh/mWh | Sets `currentBatteryProfileMutable->capacity.warning` | + | `timerAlarm` | `uint16_t` | 2 | seconds | Sets `osdConfigMutable()->time_alarm` | + | `altAlarm` | `uint16_t` | 2 | meters | Sets `osdConfigMutable()->alt_alarm` | + | `distAlarm` | `uint16_t` | 2 | meters | (Optional) Sets `osdConfigMutable()->dist_alarm` | + | `negAltAlarm` | `uint16_t` | 2 | meters | (Optional) Sets `osdConfigMutable()->neg_alt_alarm` | +* **Payload Format 2 (Set Item Position):** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `itemIndex` | `uint8_t` | 1 | Index | Index of the OSD item to position (0 to `OSD_ITEM_COUNT - 1`) | + | `itemPosition` | `uint16_t` | 2 | Coordinates | Packed X/Y position (`(Y << 8) | X`) for the item on screen 0 | +* **Notes:** Requires `USE_OSD`. Distinguishes formats based on the first byte. Format 1 requires at least 10 bytes. Format 2 requires 3 bytes. Triggers an OSD redraw. See `MSP2_INAV_OSD_SET_*` for more advanced control. + +### `MSP_OSD_CHAR_READ` (86 / 0x56) + +* **Direction:** Out +* **Description:** Reads character data from the OSD font memory. +* **Notes:** Not implemented in INAV `fc_msp.c`. Requires direct hardware access, typically done via DisplayPort. + +### `MSP_OSD_CHAR_WRITE` (87 / 0x57) + +* **Direction:** In +* **Description:** Writes character data to the OSD font memory. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `address` | `uint8_t` or `uint16_t` | 1 or 2 | Starting address in font memory. Size depends on total payload size | + | `charData` | `uint8_t[]` | Variable | Character bitmap data (54 or 64 bytes per char, depending on format) | +* **Notes:** Requires `USE_OSD`. Payload size determines address size (8/16 bit) and character data size (visible bytes only or full char with metadata). Uses `displayWriteFontCharacter()`. Requires OSD hardware (like MAX7456) to be present and functional. + +### `MSP_VTX_CONFIG` (88 / 0x58) + +* **Direction:** Out +* **Description:** Retrieves the current VTX (Video Transmitter) configuration and capabilities. +* **Payload:** (Only sent if `USE_VTX_CONTROL` is defined and a VTX device is configured) + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `vtxDeviceType` | `uint8_t` | 1 | Enum (`vtxDevType_e`): Type of VTX device detected/configured. `VTXDEV_UNKNOWN` if none | + | `band` | `uint8_t` | 1 | VTX band number (from `vtxSettingsConfig`) | + | `channel` | `uint8_t` | 1 | VTX channel number (from `vtxSettingsConfig`) | + | `power` | `uint8_t` | 1 | VTX power level index (from `vtxSettingsConfig`) | + | `pitMode` | `uint8_t` | 1 | Boolean: 1 if VTX is currently in pit mode, 0 otherwise | + | `vtxReady` | `uint8_t` | 1 | Boolean: 1 if VTX device reported ready, 0 otherwise | + | `lowPowerDisarm` | `uint8_t` | 1 | Boolean: 1 if low power on disarm is enabled (from `vtxSettingsConfig`) | + | `vtxTableAvailable` | `uint8_t` | 1 | Boolean: 1 if VTX tables (band/power) are available for query | + | `bandCount` | `uint8_t` | 1 | Number of bands supported by the VTX device | + | `channelCount` | `uint8_t` | 1 | Number of channels per band supported by the VTX device | + | `powerCount` | `uint8_t` | 1 | Number of power levels supported by the VTX device | +* **Notes:** BF compatibility field `frequency` (uint16) is missing compared to some BF versions. Use `MSP_VTXTABLE_BAND` and `MSP_VTXTABLE_POWERLEVEL` for details. + +### `MSP_SET_VTX_CONFIG` (89 / 0x59) + +* **Direction:** In +* **Description:** Sets the VTX configuration (band, channel, power, pit mode). Supports multiple protocol versions/extensions based on payload size. +* **Payload (Minimum):** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `bandChannelEncoded` | `uint16_t` | 2 | Encoded band/channel value: `(band-1)*8 + (channel-1)`. If <= `VTXCOMMON_MSP_BANDCHAN_CHKVAL` | +* **Payload (Extended):** (Fields added sequentially based on size) + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `power` | `uint8_t` | 1 | Power level index to set (`vtxSettingsConfigMutable()->power`) | + | `pitMode` | `uint8_t` | 1 | Pit mode state to set (0=off, 1=on). Directly calls `vtxCommonSetPitMode` | + | `lowPowerDisarm` | `uint8_t` | 1 | Low power on disarm setting (`vtxSettingsConfigMutable()->lowPowerDisarm`) | + | `pitModeFreq` | `uint16_t` | 2 | *Ignored*. Betaflight extension | + | `band` | `uint8_t` | 1 | Explicit band number to set (`vtxSettingsConfigMutable()->band`). Overrides encoded value if present | + | `channel` | `uint8_t` | 1 | Explicit channel number to set (`vtxSettingsConfigMutable()->channel`). Overrides encoded value if present | + | `frequency` | `uint16_t` | 2 | *Ignored*. Betaflight extension | + | `bandCount` | `uint8_t` | 1 | *Ignored*. Betaflight extension | + | `channelCount` | `uint8_t` | 1 | *Ignored*. Betaflight extension | + | `powerCount` | `uint8_t` | 1 | *Ignored*. Betaflight extension (can potentially reduce reported power count if valid) | +* **Notes:** Requires `USE_VTX_CONTROL`. Minimum size 2 bytes. Applies settings to `vtxSettingsConfig` and potentially directly to the device (pit mode). + +### `MSP_ADVANCED_CONFIG` (90 / 0x5A) + +* **Direction:** Out +* **Description:** Retrieves advanced hardware-related configuration (PWM protocols, rates). Some fields are BF compatibility placeholders. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `gyroSyncDenom` | `uint8_t` | 1 | Always 1 (BF compatibility) | + | `pidProcessDenom` | `uint8_t` | 1 | Always 1 (BF compatibility) | + | `useUnsyncedPwm` | `uint8_t` | 1 | Always 1 (BF compatibility, INAV uses async PWM based on protocol) | + | `motorPwmProtocol` | `uint8_t` | 1 | Motor PWM protocol type (`motorConfig()->motorPwmProtocol`) | + | `motorPwmRate` | `uint16_t` | 2 | Hz: Motor PWM rate (if applicable) (`motorConfig()->motorPwmRate`) | + | `servoPwmRate` | `uint16_t` | 2 | Hz: Servo PWM rate (`servoConfig()->servoPwmRate`) | + | `legacyGyroSync` | `uint8_t` | 1 | Always 0 (BF compatibility) | + +### `MSP_SET_ADVANCED_CONFIG` (91 / 0x5B) + +* **Direction:** In +* **Description:** Sets advanced hardware-related configuration (PWM protocols, rates). +* **Payload:** (Matches `MSP_ADVANCED_CONFIG` structure) + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `gyroSyncDenom` | `uint8_t` | 1 | Ignored | + | `pidProcessDenom` | `uint8_t` | 1 | Ignored | + | `useUnsyncedPwm` | `uint8_t` | 1 | Ignored | + | `motorPwmProtocol` | `uint8_t` | 1 | Sets `motorConfigMutable()->motorPwmProtocol` | + | `motorPwmRate` | `uint16_t` | 2 | Sets `motorConfigMutable()->motorPwmRate` | + | `servoPwmRate` | `uint16_t` | 2 | Sets `servoConfigMutable()->servoPwmRate` | + | `legacyGyroSync` | `uint8_t` | 1 | Ignored | +* **Notes:** Expects 9 bytes. + +### `MSP_FILTER_CONFIG` (92 / 0x5C) + +* **Direction:** Out +* **Description:** Retrieves filter configuration settings (Gyro, D-term, Yaw, Accel). Some fields are BF compatibility placeholders or legacy. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `gyroMainLpfHz` | `uint8_t` | 1 | Hz | Gyro main low-pass filter cutoff frequency (`gyroConfig()->gyro_main_lpf_hz`) | + | `dtermLpfHz` | `uint16_t` | 2 | Hz | D-term low-pass filter cutoff frequency (`pidProfile()->dterm_lpf_hz`) | + | `yawLpfHz` | `uint16_t` | 2 | Hz | Yaw low-pass filter cutoff frequency (`pidProfile()->yaw_lpf_hz`) | + | `legacyGyroNotchHz` | `uint16_t` | 2 | - | Always 0 (Legacy) | + | `legacyGyroNotchCutoff` | `uint16_t` | 2 | - | Always 1 (Legacy) | + | `bfCompatDtermNotchHz` | `uint16_t` | 2 | - | Always 0 (BF compatibility) | + | `bfCompatDtermNotchCutoff` | `uint16_t` | 2 | - | Always 1 (BF compatibility) | + | `bfCompatGyroNotch2Hz` | `uint16_t` | 2 | - | Always 0 (BF compatibility) | + | `bfCompatGyroNotch2Cutoff` | `uint16_t` | 2 | - | Always 1 (BF compatibility) | + | `accNotchHz` | `uint16_t` | 2 | Hz | Accelerometer notch filter center frequency (`accelerometerConfig()->acc_notch_hz`) | + | `accNotchCutoff` | `uint16_t` | 2 | Hz | Accelerometer notch filter cutoff frequency (`accelerometerConfig()->acc_notch_cutoff`) | + | `legacyGyroStage2LpfHz` | `uint16_t` | 2 | - | Always 0 (Legacy) | + +### `MSP_SET_FILTER_CONFIG` (93 / 0x5D) + +* **Direction:** In +* **Description:** Sets filter configuration settings. Handles different payload lengths for backward compatibility. +* **Payload:** (Fields added sequentially based on size) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `gyroMainLpfHz` | `uint8_t` | 1 | Hz | Sets `gyroConfigMutable()->gyro_main_lpf_hz`. (Size >= 5) | + | `dtermLpfHz` | `uint16_t` | 2 | Hz | Sets `pidProfileMutable()->dterm_lpf_hz` (constrained 0-500). (Size >= 5) | + | `yawLpfHz` | `uint16_t` | 2 | Hz | Sets `pidProfileMutable()->yaw_lpf_hz` (constrained 0-255). (Size >= 5) | + | `legacyGyroNotchHz` | `uint16_t` | 2 | - | Ignored. (Size >= 9) | + | `legacyGyroNotchCutoff` | `uint16_t` | 2 | - | Ignored. (Size >= 9) | + | `bfCompatDtermNotchHz` | `uint16_t` | 2 | - | Ignored. (Size >= 13) | + | `bfCompatDtermNotchCutoff` | `uint16_t` | 2 | - | Ignored. (Size >= 13) | + | `bfCompatGyroNotch2Hz` | `uint16_t` | 2 | - | Ignored. (Size >= 17) | + | `bfCompatGyroNotch2Cutoff` | `uint16_t` | 2 | - | Ignored. (Size >= 17) | + | `accNotchHz` | `uint16_t` | 2 | Hz | Sets `accelerometerConfigMutable()->acc_notch_hz` (constrained 0-255). (Size >= 21) | + | `accNotchCutoff` | `uint16_t` | 2 | Hz | Sets `accelerometerConfigMutable()->acc_notch_cutoff` (constrained 1-255). (Size >= 21) | + | `legacyGyroStage2LpfHz` | `uint16_t` | 2 | - | Ignored. (Size >= 22) | +* **Notes:** Requires specific payload sizes (5, 9, 13, 17, 21, or 22 bytes) to be accepted. Calls `pidInitFilters()` if size >= 13. + +### `MSP_PID_ADVANCED` (94 / 0x5E) + +* **Direction:** Out +* **Description:** Retrieves advanced PID tuning parameters. Many fields are BF compatibility placeholders. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `legacyRollPitchItermIgnore` | `uint16_t` | 2 | - | Always 0 (Legacy) | + | `legacyYawItermIgnore` | `uint16_t` | 2 | - | Always 0 (Legacy) | + | `legacyYawPLimit` | `uint16_t` | 2 | - | Always 0 (Legacy) | + | `bfCompatDeltaMethod` | `uint8_t` | 1 | - | Always 0 (BF compatibility) | + | `bfCompatVbatPidComp` | `uint8_t` | 1 | - | Always 0 (BF compatibility) | + | `bfCompatSetpointRelaxRatio` | `uint8_t` | 1 | - | Always 0 (BF compatibility) | + | `reserved1` | `uint8_t` | 1 | - | Always 0 | + | `legacyPidSumLimit` | `uint16_t` | 2 | - | Always 0 (Legacy) | + | `bfCompatItermThrottleGain` | `uint8_t` | 1 | - | Always 0 (BF compatibility) | + | `accelLimitRollPitch` | `uint16_t` | 2 | dps / 10 | Axis acceleration limit for Roll/Pitch / 10 (`pidProfile()->axisAccelerationLimitRollPitch / 10`) | + | `accelLimitYaw` | `uint16_t` | 2 | dps / 10 | Axis acceleration limit for Yaw / 10 (`pidProfile()->axisAccelerationLimitYaw / 10`) | +* **Notes:** Acceleration limits are scaled by 10 for compatibility. + +### `MSP_SET_PID_ADVANCED` (95 / 0x5F) + +* **Direction:** In +* **Description:** Sets advanced PID tuning parameters. +* **Payload:** (Matches `MSP_PID_ADVANCED` structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `legacyRollPitchItermIgnore` | `uint16_t` | 2 | - | Ignored | + | `legacyYawItermIgnore` | `uint16_t` | 2 | - | Ignored | + | `legacyYawPLimit` | `uint16_t` | 2 | - | Ignored | + | `bfCompatDeltaMethod` | `uint8_t` | 1 | - | Ignored | + | `bfCompatVbatPidComp` | `uint8_t` | 1 | - | Ignored | + | `bfCompatSetpointRelaxRatio` | `uint8_t` | 1 | - | Ignored | + | `reserved1` | `uint8_t` | 1 | - | Ignored | + | `legacyPidSumLimit` | `uint16_t` | 2 | - | Ignored | + | `bfCompatItermThrottleGain` | `uint8_t` | 1 | - | Ignored | + | `accelLimitRollPitch` | `uint16_t` | 2 | dps / 10 | Sets `pidProfileMutable()->axisAccelerationLimitRollPitch = value * 10` | + | `accelLimitYaw` | `uint16_t` | 2 | dps / 10 | Sets `pidProfileMutable()->axisAccelerationLimitYaw = value * 10` | +* **Notes:** Expects 17 bytes. + +### `MSP_SENSOR_CONFIG` (96 / 0x60) + +* **Direction:** Out +* **Description:** Retrieves the configured hardware type for various sensors. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `accHardware` | `uint8_t` | 1 | Enum (`accHardware_e`): Accelerometer hardware type (`accelerometerConfig()->acc_hardware`) | + | `baroHardware` | `uint8_t` | 1 | Enum (`baroHardware_e`): Barometer hardware type (`barometerConfig()->baro_hardware`). 0 if `USE_BARO` disabled | + | `magHardware` | `uint8_t` | 1 | Enum (`magHardware_e`): Magnetometer hardware type (`compassConfig()->mag_hardware`). 0 if `USE_MAG` disabled | + | `pitotHardware` | `uint8_t` | 1 | Enum (`pitotHardware_e`): Pitot tube hardware type (`pitotmeterConfig()->pitot_hardware`). 0 if `USE_PITOT` disabled | + | `rangefinderHardware` | `uint8_t` | 1 | Enum (`rangefinderHardware_e`): Rangefinder hardware type (`rangefinderConfig()->rangefinder_hardware`). 0 if `USE_RANGEFINDER` disabled | + | `opflowHardware` | `uint8_t` | 1 | Enum (`opticalFlowHardware_e`): Optical flow hardware type (`opticalFlowConfig()->opflow_hardware`). 0 if `USE_OPFLOW` disabled | + +### `MSP_SET_SENSOR_CONFIG` (97 / 0x61) + +* **Direction:** In +* **Description:** Sets the configured hardware type for various sensors. +* **Payload:** (Matches `MSP_SENSOR_CONFIG` structure) + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `accHardware` | `uint8_t` | 1 | Sets `accelerometerConfigMutable()->acc_hardware` | + | `baroHardware` | `uint8_t` | 1 | Sets `barometerConfigMutable()->baro_hardware` (if `USE_BARO`) | + | `magHardware` | `uint8_t` | 1 | Sets `compassConfigMutable()->mag_hardware` (if `USE_MAG`) | + | `pitotHardware` | `uint8_t` | 1 | Sets `pitotmeterConfigMutable()->pitot_hardware` (if `USE_PITOT`) | + | `rangefinderHardware` | `uint8_t` | 1 | Sets `rangefinderConfigMutable()->rangefinder_hardware` (if `USE_RANGEFINDER`) | + | `opflowHardware` | `uint8_t` | 1 | Sets `opticalFlowConfigMutable()->opflow_hardware` (if `USE_OPFLOW`) | +* **Notes:** Expects 6 bytes. + +### `MSP_SPECIAL_PARAMETERS` (98 / 0x62) + +* **Direction:** Out +* **Description:** Betaflight specific, likely unused/unimplemented in INAV. +* **Notes:** Not implemented in INAV `fc_msp.c`. + +### `MSP_SET_SPECIAL_PARAMETERS` (99 / 0x63) + +* **Direction:** In +* **Description:** Betaflight specific, likely unused/unimplemented in INAV. +* **Notes:** Not implemented in INAV `fc_msp.c`. + +--- + +## MSPv1 MultiWii Original Commands (100-127, 130) + +These are commands originating from the MultiWii project. + +### `MSP_IDENT` (100 / 0x64) + +* **Direction:** Out +* **Description:** Provides basic flight controller identity information. Not implemented in modern INAV, but used by legacy versions and MultiWii. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | MultiWii version | uint8_t | 1 | n/a | Scaled version major*100+minor | + | Mixer Mode | uint8_t | 1 | Enum | Mixer type | + | MSP Version | uint8_t | 1 | n/a | Scaled version major*100+minor | + | Platform Capability | uint32_t | | Bitmask of MW capabilities | +* **Notes:** Obsolete. Listed for legacy compatibility only. + +### `MSP_STATUS` (101 / 0x65) + +* **Direction:** Out +* **Description:** Provides basic flight controller status including cycle time, errors, sensor status, active modes (first 32), and the current configuration profile. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `cycleTime` | `uint16_t` | 2 | µs | Main loop cycle time (`cycleTime`) | + | `i2cErrors` | `uint16_t` | 2 | Count | Number of I2C errors encountered (`i2cGetErrorCounter()`). 0 if `USE_I2C` not defined | + | `sensorStatus` | `uint16_t` | 2 | Bitmask | Bitmask indicating available/active sensors (`packSensorStatus()`). See notes | + | `activeModesLow` | `uint32_t` | 4 | Bitmask | First 32 bits of the active flight modes bitmask (`packBoxModeFlags()`) | + | `profile` | `uint8_t` | 1 | Index | Current configuration profile index (0-based) (`getConfigProfile()`) | +* **Notes:** Superseded by `MSP_STATUS_EX` and `MSP2_INAV_STATUS`. `sensorStatus` bitmask: (Bit 0: ACC, 1: BARO, 2: MAG, 3: GPS, 4: RANGEFINDER, 5: GYRO). `activeModesLow` only contains the first 32 modes; use `MSP_ACTIVEBOXES` for the full set. + +### `MSP_RAW_IMU` (102 / 0x66) + +* **Direction:** Out +* **Description:** Provides raw sensor readings from the IMU (Accelerometer, Gyroscope, Magnetometer). +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `accX` | `int16_t` | 2 | ~1/512 G | Raw accelerometer X reading, scaled (`acc.accADCf[X] * 512`) | + | `accY` | `int16_t` | 2 | ~1/512 G | Raw accelerometer Y reading, scaled (`acc.accADCf[Y] * 512`) | + | `accZ` | `int16_t` | 2 | ~1/512 G | Raw accelerometer Z reading, scaled (`acc.accADCf[Z] * 512`) | + | `gyroX` | `int16_t` | 2 | deg/s | Gyroscope X-axis rate (`gyroRateDps(X)`) | + | `gyroY` | `int16_t` | 2 | deg/s | Gyroscope Y-axis rate (`gyroRateDps(Y)`) | + | `gyroZ` | `int16_t` | 2 | deg/s | Gyroscope Z-axis rate (`gyroRateDps(Z)`) | + | `magX` | `int16_t` | 2 | Raw units | Raw magnetometer X reading (`mag.magADC[X]`). 0 if `USE_MAG` disabled | + | `magY` | `int16_t` | 2 | Raw units | Raw magnetometer Y reading (`mag.magADC[Y]`). 0 if `USE_MAG` disabled | + | `magZ` | `int16_t` | 2 | Raw units | Raw magnetometer Z reading (`mag.magADC[Z]`). 0 if `USE_MAG` disabled | +* **Notes:** Acc scaling is approximate (512 LSB/G). Mag units depend on the sensor. + +### `MSP_SERVO` (103 / 0x67) + +* **Direction:** Out +* **Description:** Provides the current output values for all supported servos. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `servoOutputs` | `int16_t[MAX_SUPPORTED_SERVOS]` | `MAX_SUPPORTED_SERVOS * 2` | PWM | Array of current servo output values (typically 1000-2000) | + +### `MSP_MOTOR` (104 / 0x68) + +* **Direction:** Out +* **Description:** Provides the current output values for the first 8 motors. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `motorOutputs` | `uint16_t[8]` | 16 | PWM | Array of current motor output values (typically 1000-2000). Values beyond `MAX_SUPPORTED_MOTORS` are 0 | + +### `MSP_RC` (105 / 0x69) + +* **Direction:** Out +* **Description:** Provides the current values of the received RC channels. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `rcChannels` | `uint16_t[]` | `rxRuntimeConfig.channelCount * 2` | PWM | Array of current RC channel values (typically 1000-2000). Length depends on detected channels | + +### `MSP_RAW_GPS` (106 / 0x6A) + +* **Direction:** Out +* **Description:** Provides raw GPS data (fix status, coordinates, altitude, speed, course). +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `fixType` | `uint8_t` | 1 | Enum | Enum `gpsFixType_e` GPS fix type (`gpsSol.fixType`) | + | `numSat` | `uint8_t` | 1 | Count | Number of satellites used in solution (`gpsSol.numSat`) | + | `latitude` | `int32_t` | 4 | deg * 1e7 | Latitude (`gpsSol.llh.lat`) | + | `longitude` | `int32_t` | 4 | deg * 1e7 | Longitude (`gpsSol.llh.lon`) | + | `altitude` | `int16_t` | 2 | meters | Altitude above MSL (`gpsSol.llh.alt / 100`) | + | `speed` | `uint16_t` | 2 | cm/s | Ground speed (`gpsSol.groundSpeed`) | + | `groundCourse` | `uint16_t` | 2 | deci-degrees | Ground course (`gpsSol.groundCourse`) | + | `hdop` | `uint16_t` | 2 | HDOP * 100 | Horizontal Dilution of Precision (`gpsSol.hdop`) | +* **Notes:** Only available if `USE_GPS` is defined. Altitude is truncated to meters. + +### `MSP_COMP_GPS` (107 / 0x6B) + +* **Direction:** Out +* **Description:** Provides computed GPS values: distance and direction to home. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `distanceToHome` | `uint16_t` | 2 | meters | Distance to the home point (`GPS_distanceToHome`) | + | `directionToHome` | `uint16_t` | 2 | degrees | Direction to the home point (0-360) (`GPS_directionToHome`) | + | `gpsHeartbeat` | `uint8_t` | 1 | Boolean | Indicates if GPS data is being received (`gpsSol.flags.gpsHeartbeat`) | +* **Notes:** Only available if `USE_GPS` is defined. + +### `MSP_ATTITUDE` (108 / 0x6C) + +* **Direction:** Out +* **Description:** Provides the current attitude estimate (roll, pitch, yaw). +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `roll` | `int16_t` | 2 | deci-degrees | Roll angle (`attitude.values.roll`) | + | `pitch` | `int16_t` | 2 | deci-degrees | Pitch angle (`attitude.values.pitch`) | + | `yaw` | `int16_t` | 2 | degrees | Yaw/Heading angle (`DECIDEGREES_TO_DEGREES(attitude.values.yaw)`) | +* **Notes:** Yaw is converted from deci-degrees to degrees. + +### `MSP_ALTITUDE` (109 / 0x6D) + +* **Direction:** Out +* **Description:** Provides estimated altitude, vertical speed (variometer), and raw barometric altitude. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `estimatedAltitude` | `int32_t` | 4 | cm | Estimated altitude above home/sea level (`getEstimatedActualPosition(Z)`) | + | `variometer` | `int16_t` | 2 | cm/s | Estimated vertical speed (`getEstimatedActualVelocity(Z)`) | + | `baroAltitude` | `int32_t` | 4 | cm | Latest raw altitude from barometer (`baroGetLatestAltitude()`). 0 if `USE_BARO` disabled | + +### `MSP_ANALOG` (110 / 0x6E) + +* **Direction:** Out +* **Description:** Provides analog sensor readings: battery voltage, current consumption (mAh), RSSI, and current draw (Amps). +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `vbat` | `uint8_t` | 1 | 0.1V | Battery voltage, scaled (`getBatteryVoltage() / 10`), constrained 0-255 | + | `mAhDrawn` | `uint16_t` | 2 | mAh | Consumed battery capacity (`getMAhDrawn()`), constrained 0-65535 | + | `rssi` | `uint16_t` | 2 | 0-1023 or % | Received Signal Strength Indicator (`getRSSI()`). Units depend on source | + | `amperage` | `int16_t` | 2 | 0.01A | Current draw (`getAmperage()`), constrained -32768 to 32767 | +* **Notes:** Superseded by `MSP2_INAV_ANALOG` which provides higher precision and more fields. + +### `MSP_RC_TUNING` (111 / 0x6F) + +* **Direction:** Out +* **Description:** Retrieves RC tuning parameters (rates, expos, TPA) for the current control rate profile. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `legacyRcRate` | `uint8_t` | 1 | Always 100 (Legacy, unused) | + | `rcExpo` | `uint8_t` | 1 | Roll/Pitch RC Expo (`currentControlRateProfile->stabilized.rcExpo8`) | + | `rollRate` | `uint8_t` | 1 | Roll Rate (`currentControlRateProfile->stabilized.rates[FD_ROLL]`) | + | `pitchRate` | `uint8_t` | 1 | Pitch Rate (`currentControlRateProfile->stabilized.rates[FD_PITCH]`) | + | `yawRate` | `uint8_t` | 1 | Yaw Rate (`currentControlRateProfile->stabilized.rates[FD_YAW]`) | + | `dynamicThrottlePID` | `uint8_t` | 1 | Dynamic Throttle PID (TPA) value (`currentControlRateProfile->throttle.dynPID`) | + | `throttleMid` | `uint8_t` | 1 | Throttle Midpoint (`currentControlRateProfile->throttle.rcMid8`) | + | `throttleExpo` | `uint8_t` | 1 | Throttle Expo (`currentControlRateProfile->throttle.rcExpo8`) | + | `tpaBreakpoint` | `uint16_t` | 2 | Throttle PID Attenuation (TPA) breakpoint (`currentControlRateProfile->throttle.pa_breakpoint`) | + | `rcYawExpo` | `uint8_t` | 1 | Yaw RC Expo (`currentControlRateProfile->stabilized.rcYawExpo8`) | +* **Notes:** Superseded by `MSP2_INAV_RATE_PROFILE` which includes manual rates/expos. + +### `MSP_ACTIVEBOXES` (113 / 0x71) + +* **Direction:** Out +* **Description:** Provides the full bitmask of currently active flight modes (boxes). +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `activeModes` | `boxBitmask_t` | `sizeof(boxBitmask_t)` | Bitmask of all active modes (`packBoxModeFlags()`). Size depends on `boxBitmask_t` definition | +* **Notes:** Use this instead of `MSP_STATUS` or `MSP_STATUS_EX` if more than 32 modes are possible. + +### `MSP_MISC` (114 / 0x72) + +* **Direction:** Out +* **Description:** Retrieves miscellaneous configuration settings, mostly related to RC, GPS, Mag, and Battery voltage (legacy formats). +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `midRc` | `uint16_t` | 2 | PWM | Mid RC value (`PWM_RANGE_MIDDLE`, typically 1500) | + | `legacyMinThrottle` | `uint16_t` | 2 | - | Always 0 (Legacy) | + | `maxThrottle` | `uint16_t` | 2 | PWM | Maximum throttle command (`getMaxThrottle()`) | + | `minCommand` | `uint16_t` | 2 | PWM | Minimum motor command when disarmed (`motorConfig()->mincommand`) | + | `failsafeThrottle` | `uint16_t` | 2 | PWM | Failsafe throttle level (`currentBatteryProfile->failsafe_throttle`) | + | `gpsType` | `uint8_t` | 1 | Enum | Enum `gpsProvider_e` GPS provider type (`gpsConfig()->provider`). 0 if `USE_GPS` disabled | + | `legacyGpsBaud` | `uint8_t` | 1 | - | Always 0 (Legacy) | + | `gpsSbasMode` | `uint8_t` | 1 | Enum | Enum `sbasMode_e` GPS SBAS mode (`gpsConfig()->sbasMode`). 0 if `USE_GPS` disabled | + | `legacyMwCurrentOut` | `uint8_t` | 1 | - | Always 0 (Legacy) | + | `rssiChannel` | `uint8_t` | 1 | Index | RSSI channel index (1-based) (`rxConfig()->rssi_channel`) | + | `reserved1` | `uint8_t` | 1 | - | Always 0 | + | `magDeclination` | `uint16_t` | 2 | 0.1 degrees | Magnetic declination / 10 (`compassConfig()->mag_declination / 10`). 0 if `USE_MAG` disabled | + | `vbatScale` | `uint8_t` | 1 | Scale / 10 | Voltage scale / 10 (`batteryMetersConfig()->voltage.scale / 10`). 0 if `USE_ADC` disabled | + | `vbatMinCell` | `uint8_t` | 1 | 0.1V | Min cell voltage / 10 (`currentBatteryProfile->voltage.cellMin / 10`). 0 if `USE_ADC` disabled | + | `vbatMaxCell` | `uint8_t` | 1 | 0.1V | Max cell voltage / 10 (`currentBatteryProfile->voltage.cellMax / 10`). 0 if `USE_ADC` disabled | + | `vbatWarningCell` | `uint8_t` | 1 | 0.1V | Warning cell voltage / 10 (`currentBatteryProfile->voltage.cellWarning / 10`). 0 if `USE_ADC` disabled | +* **Notes:** Superseded by `MSP2_INAV_MISC` and other specific commands which offer better precision and more fields. + +### `MSP_BOXNAMES` (116 / 0x74) + +* **Direction:** Out +* **Description:** Provides a semicolon-separated string containing the names of all available flight modes (boxes). +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `boxNamesString` | `char[]` | Variable | String containing mode names separated by ';'. Null termination not guaranteed by MSP, relies on payload size. (`serializeBoxNamesReply()`) | +* **Notes:** The exact set of names depends on compiled features and configuration. Due to the size of the payload, it is recommended that [`MSP_BOXIDS`](#msp_boxids-119--0x77) is used instead. + +### `MSP_PIDNAMES` (117 / 0x75) + +* **Direction:** Out +* **Description:** Provides a semicolon-separated string containing the names of the PID controllers. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `pidNamesString` | `char[]` | Variable | String "ROLL;PITCH;YAW;ALT;Pos;PosR;NavR;LEVEL;MAG;VEL;". Null termination not guaranteed by MSP | + +### `MSP_WP` (118 / 0x76) + +* **Direction:** In/Out +* **Description:** Get/Set a single waypoint from the mission plan. +* **Request Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `waypointIndex` | `uint8_t` | 1 | Index of the waypoint to retrieve (0 to `NAV_MAX_WAYPOINTS - 1`) | +* **Reply Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `waypointIndex` | `uint8_t` | 1 | Index | Index of the returned waypoint | + | `action` | `uint8_t` | 1 | Enum | Enum `navWaypointActions_e` Waypoint action type | + | `latitude` | `int32_t` | 4 | deg * 1e7 | Latitude coordinate | + | `longitude` | `int32_t` | 4 | deg * 1e7 | Longitude coordinate | + | `altitude` | `int32_t` | 4 | cm | Altitude coordinate (relative to home or sea level, see flag) | + | `param1` | `uint16_t` | 2 | Varies | Parameter 1 (meaning depends on action) | + | `param2` | `uint16_t` | 2 | Varies | Parameter 2 (meaning depends on action) | + | `param3` | `uint16_t` | 2 | Varies | Parameter 3 (meaning depends on action) | + | `flag` | `uint8_t` | 1 | Bitmask | Waypoint flags (`NAV_WP_FLAG_*`) | +* **Notes:** See `navWaypoint_t` and `navWaypointActions_e`. + +### `MSP_BOXIDS` (119 / 0x77) + +* **Direction:** Out +* **Description:** Provides a list of permanent IDs associated with the available flight modes (boxes). +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `boxIds` | `uint8_t[]` | Variable | Array of permanent IDs for each configured box (`serializeBoxReply()`). Length depends on number of boxes | +* **Notes:** Useful for mapping mode range configurations (`MSP_MODE_RANGES`) back to user-understandable modes via `MSP_BOXNAMES`. + +### `MSP_SERVO_CONFIGURATIONS` (120 / 0x78) + +* **Direction:** Out +* **Description:** Retrieves the configuration parameters for all supported servos (min, max, middle, rate). Legacy format with unused fields. +* **Payload:** Repeated `MAX_SUPPORTED_SERVOS` times: + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `min` | `uint16_t` | 2 | PWM | Minimum servo endpoint (`servoParams(i)->min`) | + | `max` | `uint16_t` | 2 | PWM | Maximum servo endpoint (`servoParams(i)->max`) | + | `middle` | `uint16_t` | 2 | PWM | Middle/Neutral servo position (`servoParams(i)->middle`) | + | `rate` | `uint8_t` | 1 | % (-100 to 100) | Servo rate/scaling (`servoParams(i)->rate`) | + | `reserved1` | `uint8_t` | 1 | - | Always 0 | + | `reserved2` | `uint8_t` | 1 | - | Always 0 | + | `legacyForwardChan` | `uint8_t` | 1 | - | Always 255 (Legacy) | + | `legacyReversedSources` | `uint32_t` | 4 | - | Always 0 (Legacy) | +* **Notes:** Superseded by `MSP2_INAV_SERVO_CONFIG` which has a cleaner structure. + +### `MSP_NAV_STATUS` (121 / 0x79) + +* **Direction:** Out +* **Description:** Retrieves the current status of the navigation system. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `navMode` | `uint8_t` | 1 | Enum (`NAV_MODE_*`): Current navigation mode (None, RTH, WP, Hold, etc.) (`NAV_Status.mode`) | + | `navState` | `uint8_t` | 1 | Enum (`NAV_STATE_*`): Current navigation state (`NAV_Status.state`) | + | `activeWpAction` | `uint8_t` | 1 | Enum (`navWaypointActions_e`): Action of the currently executing waypoint (`NAV_Status.activeWpAction`) | + | `activeWpNumber` | `uint8_t` | 1 | Index: Index of the currently executing waypoint (`NAV_Status.activeWpNumber`) | + | `navError` | `uint8_t` | 1 | Enum (`NAV_ERROR_*`): Current navigation error code (`NAV_Status.error`) | + | `targetHeading` | `int16_t` | 2 | degrees: Target heading for heading controller (`getHeadingHoldTarget()`) | +* **Notes:** Requires `USE_GPS`. + +### `MSP_NAV_CONFIG` (122 / 0x7A) + +* **Not implemented** + +### `MSP_3D` (124 / 0x7C) + +* **Direction:** Out +* **Description:** Retrieves settings related to 3D/reversible motor operation. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `deadbandLow` | `uint16_t` | 2 | PWM | Lower deadband limit for 3D mode (`reversibleMotorsConfig()->deadband_low`) | + | `deadbandHigh` | `uint16_t` | 2 | PWM | Upper deadband limit for 3D mode (`reversibleMotorsConfig()->deadband_high`) | + | `neutral` | `uint16_t` | 2 | PWM | Neutral throttle point for 3D mode (`reversibleMotorsConfig()->neutral`) | +* **Notes:** Requires reversible motor support. + +### `MSP_RC_DEADBAND` (125 / 0x7D) + +* **Direction:** Out +* **Description:** Retrieves RC input deadband settings. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `deadband` | `uint8_t` | 1 | PWM | General RC deadband for Roll/Pitch (`rcControlsConfig()->deadband`) | + | `yawDeadband` | `uint8_t` | 1 | PWM | Specific deadband for Yaw (`rcControlsConfig()->yaw_deadband`) | + | `altHoldDeadband` | `uint8_t` | 1 | PWM | Deadband for altitude hold adjustments (`rcControlsConfig()->alt_hold_deadband`) | + | `throttleDeadband` | `uint16_t` | 2 | PWM | Deadband around throttle mid-stick (`rcControlsConfig()->mid_throttle_deadband`) | + +### `MSP_SENSOR_ALIGNMENT` (126 / 0x7E) + +* **Direction:** Out +* **Description:** Retrieves sensor alignment settings (legacy format). +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `gyroAlign` | `uint8_t` | 1 | Always 0 (Legacy alignment enum) | + | `accAlign` | `uint8_t` | 1 | Always 0 (Legacy alignment enum) | + | `magAlign` | `uint8_t` | 1 | Magnetometer alignment (`compassConfig()->mag_align`). 0 if `USE_MAG` disabled | + | `opflowAlign` | `uint8_t` | 1 | Optical flow alignment (`opticalFlowConfig()->opflow_align`). 0 if `USE_OPFLOW` disabled | +* **Notes:** Board alignment is now typically handled by `MSP_BOARD_ALIGNMENT`. This returns legacy enum values where applicable. + +### `MSP_LED_STRIP_MODECOLOR` (127 / 0x7F) + +* **Direction:** Out +* **Description:** Retrieves the color index assigned to each LED mode and function/direction combination, including special colors. +* **Payload:** Repeated (`LED_MODE_COUNT * LED_DIRECTION_COUNT` + `LED_SPECIAL_COLOR_COUNT`) times: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `modeIndex` | `uint8_t` | 1 | Index of the LED mode Enum (`ledModeIndex_e`). `LED_MODE_COUNT` for special colors | + | `directionOrSpecialIndex` | `uint8_t` | 1 | Index of the direction Enum (`ledDirection_e`) or special color (`ledSpecialColor_e`) | + | `colorIndex` | `uint8_t` | 1 | Index of the color assigned from `ledStripConfig()->colors` | +* **Notes:** Only available if `USE_LED_STRIP` is defined. Allows mapping modes/directions/specials to configured colors. + +### `MSP_BATTERY_STATE` (130 / 0x82) + +* **Direction:** Out +* **Description:** Provides battery state information, formatted primarily for DJI FPV Goggles compatibility. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `cellCount` | `uint8_t` | 1 | Count | Number of battery cells (`getBatteryCellCount()`) | + | `capacity` | `uint16_t` | 2 | mAh | Battery capacity (`currentBatteryProfile->capacity.value`) | + | `vbatScaled` | `uint8_t` | 1 | 0.1V | Battery voltage / 10 (`getBatteryVoltage() / 10`) | + | `mAhDrawn` | `uint16_t` | 2 | mAh | Consumed capacity (`getMAhDrawn()`) | + | `amperage` | `int16_t` | 2 | 0.01A | Current draw (`getAmperage()`) | + | `batteryState` | `uint8_t` | 1 | Enum | Enum `batteryState_e` Current battery state (`getBatteryState()`, see `BATTERY_STATE_*`) | + | `vbatActual` | `uint16_t` | 2 | 0.01V | Actual battery voltage (`getBatteryVoltage()`) | +* **Notes:** Only available if `USE_DJI_HD_OSD` or `USE_MSP_DISPLAYPORT` is defined. Some values are duplicated from `MSP_ANALOG` / `MSP2_INAV_ANALOG` but potentially with different scaling/types. + +### `MSP_VTXTABLE_BAND` (137 / 0x89) + +* **Direction:** In/Out (?) +* **Description:** Retrieves information about a specific VTX band from the VTX table. (Implementation missing in provided `fc_msp.c`) +* **Notes:** The ID is defined, but no handler exists in the provided C code. Likely intended to query band names and frequencies. + +### `MSP_VTXTABLE_POWERLEVEL` (138 / 0x8A) + +* **Direction:** In/Out +* **Description:** Retrieves information about a specific VTX power level from the VTX table. +* **Request Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `powerLevelIndex` | `uint8_t` | 1 | 1-based index of the power level to query | +* **Reply Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `powerLevelIndex` | `uint8_t` | 1 | 1-based index of the returned power level | + | `powerValue` | `uint16_t` | 2 | Always 0 (Actual power value in mW is not stored/returned via MSP) | + | `labelLength` | `uint8_t` | 1 | Length of the power level label string that follows | + | `label` | `char[]` | Variable | Power level label string (e.g., "25", "200"). Length given by previous field | +* **Notes:** Requires `USE_VTX_CONTROL`. Returns error if index is out of bounds. The `powerValue` field is unused. + +--- + +## MSPv1 MultiWii Original Input Commands (200-221) + +These commands are sent *to* the FC. + +### `MSP_SET_RAW_RC` (200 / 0xC8) + +* **Direction:** In +* **Description:** Provides raw RC channel data to the flight controller, typically used when the receiver is connected via MSP (e.g., MSP RX feature). +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `rcChannels` | `uint16_t[]` | Variable (2 * channelCount) | PWM | Array of RC channel values (typically 1000-2000). Number of channels determined by payload size | +* **Notes:** Requires `USE_RX_MSP`. Maximum channels `MAX_SUPPORTED_RC_CHANNEL_COUNT`. Calls `rxMspFrameReceive()`. + +### `MSP_SET_RAW_GPS` (201 / 0xC9) + +* **Direction:** In +* **Description:** Provides raw GPS data to the flight controller, typically for simulation or external GPS injection. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `fixType` | `uint8_t` | 1 | Enum | Enum `gpsFixType_e` GPS fix type | + | `numSat` | `uint8_t` | 1 | Count | Number of satellites | + | `latitude` | `int32_t` | 4 | deg * 1e7 | Latitude | + | `longitude` | `int32_t` | 4 | deg * 1e7 | Longitude | + | `altitude` | `int16_t` | 2 | meters | Altitude (converted to cm internally) | + | `speed` | `uint16_t` | 2 | cm/s | Ground speed | + | `groundCourse` | `uint16_t` | 2 | ??? | Ground course (units unclear from code, likely degrees or deci-degrees, ignored in current code) | +* **Notes:** Requires `USE_GPS`. Expects 14 bytes. Updates `gpsSol` structure and calls `onNewGPSData()`. Note the altitude unit mismatch (meters in MSP, cm internal). Does not provide velocity components. + +### `MSP_SET_BOX` (203 / 0xCB) + +* **Direction:** In +* **Description:** Sets the state of flight modes (boxes). (Likely unused/obsolete in INAV). +* **Notes:** Not implemented in INAV `fc_msp.c`. Mode changes are typically handled via RC channels (`MSP_MODE_RANGES`). + +### `MSP_SET_RC_TUNING` (204 / 0xCC) + +* **Direction:** In +* **Description:** Sets RC tuning parameters (rates, expos, TPA) for the current control rate profile. +* **Payload:** (Matches `MSP_RC_TUNING` outgoing structure) + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `legacyRcRate` | `uint8_t` | 1 | Ignored | + | `rcExpo` | `uint8_t` | 1 | Sets `currentControlRateProfile->stabilized.rcExpo8` | + | `rollRate` | `uint8_t` | 1 | Sets `currentControlRateProfile->stabilized.rates[FD_ROLL]` (constrained) | + | `pitchRate` | `uint8_t` | 1 | Sets `currentControlRateProfile->stabilized.rates[FD_PITCH]` (constrained) | + | `yawRate` | `uint8_t` | 1 | Sets `currentControlRateProfile->stabilized.rates[FD_YAW]` (constrained) | + | `dynamicThrottlePID` | `uint8_t` | 1 | Sets `currentControlRateProfile->throttle.dynPID` (constrained) | + | `throttleMid` | `uint8_t` | 1 | Sets `currentControlRateProfile->throttle.rcMid8` | + | `throttleExpo` | `uint8_t` | 1 | Sets `currentControlRateProfile->throttle.rcExpo8` | + | `tpaBreakpoint` | `uint16_t` | 2 | Sets `currentControlRateProfile->throttle.pa_breakpoint` | + | `rcYawExpo` | `uint8_t` | 1 | (Optional) Sets `currentControlRateProfile->stabilized.rcYawExpo8` | +* **Notes:** Expects 10 or 11 bytes. Calls `schedulePidGainsUpdate()`. Superseded by `MSP2_INAV_SET_RATE_PROFILE`. + +### `MSP_ACC_CALIBRATION` (205 / 0xCD) + +* **Direction:** In +* **Description:** Starts the accelerometer calibration procedure. +* **Payload:** None +* **Notes:** Will fail if armed. Calls `accStartCalibration()`. + +### `MSP_MAG_CALIBRATION` (206 / 0xCE) + +* **Direction:** In +* **Description:** Starts the magnetometer calibration procedure. +* **Payload:** None +* **Notes:** Will fail if armed. Enables the `CALIBRATE_MAG` state flag. + +### `MSP_SET_MISC` (207 / 0xCF) + +* **Direction:** In +* **Description:** Sets miscellaneous configuration settings (legacy formats/scaling). +* **Payload:** (Matches `MSP_MISC` outgoing structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `midRc` | `uint16_t` | 2 | PWM | Ignored | + | `legacyMinThrottle` | `uint16_t` | 2 | - | Ignored | + | `legacyMaxThrottle` | `uint16_t` | 2 | - | Ignored | + | `minCommand` | `uint16_t` | 2 | PWM | Sets `motorConfigMutable()->mincommand` (constrained 0-PWM_RANGE_MAX) | + | `failsafeThrottle` | `uint16_t` | 2 | PWM | Sets `currentBatteryProfileMutable->failsafe_throttle` (constrained PWM_RANGE_MIN/MAX) | + | `gpsType` | `uint8_t` | 1 | Enum | Enum `gpsProvider_e` (Sets `gpsConfigMutable()->provider`)| + | `legacyGpsBaud` | `uint8_t` | 1 | - | Ignored | + | `gpsSbasMode` | `uint8_t` | 1 | Enum | Enum `sbasMode_e` (Sets `gpsConfigMutable()->sbasMode`) | + | `legacyMwCurrentOut` | `uint8_t` | 1 | - | Ignored | + | `rssiChannel` | `uint8_t` | 1 | Index | Sets `rxConfigMutable()->rssi_channel` (constrained 0-MAX_SUPPORTED_RC_CHANNEL_COUNT). Updates source | + | `reserved1` | `uint8_t` | 1 | - | Ignored | + | `magDeclination` | `uint16_t` | 2 | 0.1 degrees | Sets `compassConfigMutable()->mag_declination = value * 10` (if `USE_MAG`) | + | `vbatScale` | `uint8_t` | 1 | Scale / 10 | Sets `batteryMetersConfigMutable()->voltage.scale = value * 10` (if `USE_ADC`) | + | `vbatMinCell` | `uint8_t` | 1 | 0.1V | Sets `currentBatteryProfileMutable->voltage.cellMin = value * 10` (if `USE_ADC`) | + | `vbatMaxCell` | `uint8_t` | 1 | 0.1V | Sets `currentBatteryProfileMutable->voltage.cellMax = value * 10` (if `USE_ADC`) | + | `vbatWarningCell` | `uint8_t` | 1 | 0.1V | Sets `currentBatteryProfileMutable->voltage.cellWarning = value * 10` (if `USE_ADC`) | +* **Notes:** Expects 22 bytes. Superseded by `MSP2_INAV_SET_MISC`. + +### `MSP_RESET_CONF` (208 / 0xD0) + +* **Direction:** In +* **Description:** Resets all configuration settings to their default values and saves to EEPROM. +* **Payload:** None +* **Notes:** Will fail if armed. Suspends RX, calls `resetEEPROM()`, `writeEEPROM()`, `readEEPROM()`, resumes RX. Use with caution! + +### `MSP_SET_WP` (209 / 0xD1) + +* **Direction:** In +* **Description:** Sets a single waypoint in the mission plan. +* **Payload:** (Matches `MSP_WP` reply structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `waypointIndex` | `uint8_t` | 1 | Index | Index of the waypoint to set (0 to `NAV_MAX_WAYPOINTS - 1`) | + | `action` | `uint8_t` | 1 | Enum | Enum `navWaypointActions_e` Waypoint action type | + | `latitude` | `int32_t` | 4 | deg * 1e7 | Latitude coordinate | + | `longitude` | `int32_t` | 4 | deg * 1e7 | Longitude coordinate | + | `altitude` | `int32_t` | 4 | cm | Altitude coordinate | + | `param1` | `uint16_t` | 2 | Varies | Parameter 1 | + | `param2` | `uint16_t` | 2 | Varies | Parameter 2 | + | `param3` | `uint16_t` | 2 | Varies | Parameter 3 | + | `flag` | `uint8_t` | 1 | Bitmask | Waypoint flags | +* **Notes:** Expects 21 bytes. Calls `setWaypoint()`. If `USE_FW_AUTOLAND` is enabled, this also interacts with autoland approach settings based on waypoint index and flags. + +### `MSP_SELECT_SETTING` (210 / 0xD2) + +* **Direction:** In +* **Description:** Selects the active configuration profile and saves it. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `profileIndex` | `uint8_t` | 1 | Index of the profile to activate (0-based) | +* **Notes:** Will fail if armed. Calls `setConfigProfileAndWriteEEPROM()`. + +### `MSP_SET_HEAD` (211 / 0xD3) + +* **Direction:** In +* **Description:** Sets the target heading for the heading hold controller (e.g., during MAG mode). +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `heading` | `int16_t` | 2 | degrees | Target heading (0-359) | +* **Notes:** Expects 2 bytes. Calls `updateHeadingHoldTarget()`. + +### `MSP_SET_SERVO_CONFIGURATION` (212 / 0xD4) + +* **Direction:** In +* **Description:** Sets the configuration for a single servo (legacy format). +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `servoIndex` | `uint8_t` | 1 | Index | Index of the servo to configure (0 to `MAX_SUPPORTED_SERVOS - 1`) | + | `min` | `uint16_t` | 2 | PWM | Minimum servo endpoint | + | `max` | `uint16_t` | 2 | PWM | Maximum servo endpoint | + | `middle` | `uint16_t` | 2 | PWM | Middle/Neutral servo position | + | `rate` | `uint8_t` | 1 | % | Servo rate/scaling | + | `reserved1` | `uint8_t` | 1 | - | Ignored | + | `reserved2` | `uint8_t` | 1 | - | Ignored | + | `legacyForwardChan` | `uint8_t` | 1 | - | Ignored | + | `legacyReversedSources` | `uint32_t` | 4 | - | Ignored | +* **Notes:** Expects 15 bytes. Returns error if index is invalid. Calls `servoComputeScalingFactors()`. Superseded by `MSP2_INAV_SET_SERVO_CONFIG`. + +### `MSP_SET_MOTOR` (214 / 0xD6) + +* **Direction:** In +* **Description:** Sets the disarmed motor values, typically used for motor testing or propeller balancing functions in a configurator. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `motorValues` | `uint16_t[8]` | 16 | PWM | Array of motor values to set when disarmed. Only affects first `MAX_SUPPORTED_MOTORS` | +* **Notes:** Expects 16 bytes. Modifies the `motor_disarmed` array. These values are *not* saved persistently. + +### `MSP_SET_NAV_CONFIG` (215 / 0xD7) + +* **Not implemented** + +### `MSP_SET_3D` (217 / 0xD9) + +* **Direction:** In +* **Description:** Sets parameters related to 3D/reversible motor operation. +* **Payload:** (Matches `MSP_3D` outgoing structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `deadbandLow` | `uint16_t` | 2 | PWM | Sets `reversibleMotorsConfigMutable()->deadband_low` | + | `deadbandHigh` | `uint16_t` | 2 | PWM | Sets `reversibleMotorsConfigMutable()->deadband_high` | + | `neutral` | `uint16_t` | 2 | PWM | Sets `reversibleMotorsConfigMutable()->neutral` | +* **Notes:** Expects 6 bytes. Requires reversible motor support. + +### `MSP_SET_RC_DEADBAND` (218 / 0xDA) + +* **Direction:** In +* **Description:** Sets RC input deadband values. +* **Payload:** (Matches `MSP_RC_DEADBAND` outgoing structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `deadband` | `uint8_t` | 1 | PWM | Sets `rcControlsConfigMutable()->deadband` | + | `yawDeadband` | `uint8_t` | 1 | PWM | Sets `rcControlsConfigMutable()->yaw_deadband` | + | `altHoldDeadband` | `uint8_t` | 1 | PWM | Sets `rcControlsConfigMutable()->alt_hold_deadband` | + | `throttleDeadband` | `uint16_t` | 2 | PWM | Sets `rcControlsConfigMutable()->mid_throttle_deadband` | +* **Notes:** Expects 5 bytes. + +### `MSP_SET_RESET_CURR_PID` (219 / 0xDB) + +* **Direction:** In +* **Description:** Resets the PIDs of the *current* profile to their default values. Does not save. +* **Payload:** None +* **Notes:** Calls `PG_RESET_CURRENT(pidProfile)`. To save, follow with `MSP_EEPROM_WRITE`. + +### `MSP_SET_SENSOR_ALIGNMENT` (220 / 0xDC) + +* **Direction:** In +* **Description:** Sets sensor alignment (legacy format). +* **Payload:** (Matches `MSP_SENSOR_ALIGNMENT` outgoing structure) + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `gyroAlign` | `uint8_t` | 1 | Ignored | + | `accAlign` | `uint8_t` | 1 | Ignored | + | `magAlign` | `uint8_t` | 1 | Sets `compassConfigMutable()->mag_align` (if `USE_MAG`) | + | `opflowAlign` | `uint8_t` | 1 | Sets `opticalFlowConfigMutable()->opflow_align` (if `USE_OPFLOW`) | +* **Notes:** Expects 4 bytes. Use `MSP_SET_BOARD_ALIGNMENT` for primary board orientation. + +### `MSP_SET_LED_STRIP_MODECOLOR` (221 / 0xDD) + +* **Direction:** In +* **Description:** Sets the color index for a specific LED mode/function combination. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `modeIndex` | `uint8_t` | 1 | Index of the LED mode (`ledModeIndex_e` or `LED_MODE_COUNT` for special) | + | `directionOrSpecialIndex` | `uint8_t` | 1 | Index of the direction or special color | + | `colorIndex` | `uint8_t` | 1 | Index of the color to assign from `ledStripConfig()->colors` | +* **Notes:** Only available if `USE_LED_STRIP` is defined. Expects 3 bytes. Returns error if setting fails (invalid index). + +--- + +## MSPv1 System & Debug Commands (239-254) + +### `MSP_SET_ACC_TRIM` (239 / 0xEF) + +* **Direction:** In +* **Description:** Sets the accelerometer trim values (leveling calibration). +* **Notes:** Not implemented in INAV `fc_msp.c`. Use `MSP_ACC_CALIBRATION`. + +### `MSP_ACC_TRIM` (240 / 0xF0) + +* **Direction:** Out +* **Description:** Gets the accelerometer trim values. +* **Notes:** Not implemented in INAV `fc_msp.c`. Calibration data via `MSP_CALIBRATION_DATA`. + +### `MSP_SERVO_MIX_RULES` (241 / 0xF1) + +* **Direction:** Out +* **Description:** Retrieves the custom servo mixer rules (legacy format). +* **Payload:** Repeated `MAX_SERVO_RULES` times: + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `targetChannel` | `uint8_t` | 1 | Index | Servo output channel index (0-based) | + | `inputSource` | `uint8_t` | 1 | Enum | Enum `inputSource_e` Input source for the mix (RC chan, Roll, Pitch...) | + | `rate` | `uint16_t` | 2 | % * 100? | Mixing rate/weight. Needs scaling check | + | `speed` | `uint8_t` | 1 | 0-100 | Speed/Slew rate limit | + | `reserved1` | `uint8_t` | 1 | - | Always 0 | + | `legacyMax` | `uint8_t` | 1 | - | Always 100 (Legacy) | + | `legacyBox` | `uint8_t` | 1 | - | Always 0 (Legacy) | +* **Notes:** Superseded by `MSP2_INAV_SERVO_MIXER`. + +### `MSP_SET_SERVO_MIX_RULE` (242 / 0xF2) + +* **Direction:** In +* **Description:** Sets a single custom servo mixer rule (legacy format). +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `ruleIndex` | `uint8_t` | 1 | Index | Index of the rule to set (0 to `MAX_SERVO_RULES - 1`) | + | `targetChannel` | `uint8_t` | 1 | Index | Servo output channel index | + | `inputSource` | `uint8_t` | 1 | Enum | Enum `inputSource_e` Input source for the mix | + | `rate` | `uint16_t` | 2 | % * 100? | Mixing rate/weight | + | `speed` | `uint8_t` | 1 | 0-100 | Speed/Slew rate limit | + | `legacyMinMax` | `uint16_t` | 2 | - | Ignored | + | `legacyBox` | `uint8_t` | 1 | - | Ignored | +* **Notes:** Expects 9 bytes. Returns error if index invalid. Calls `loadCustomServoMixer()`. Superseded by `MSP2_INAV_SET_SERVO_MIXER`. + +### `MSP_SET_PASSTHROUGH` (245 / 0xF5) + +* **Direction:** In/Out (Special: In command triggers passthrough mode, Reply confirms start) +* **Description:** Enables serial passthrough mode to peripherals like ESCs (BLHeli 4-way) or other serial devices. +* **Request Payload (Legacy - 4way):** None +* **Request Payload (Extended):** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `passthroughMode` | `uint8_t` | 1 | Type of passthrough Enum (`mspPassthroughType_e`: Serial ID, Serial Function, ESC 4way) | + | `passthroughArgument` | `uint8_t` | 1 | Argument for the mode (e.g., Serial Port Identifier, Serial Function ID). Defaults to 0 if not sent | +* **Reply Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `status` | `uint8_t` | 1 | 1 if passthrough started successfully, 0 on error (e.g., port not found). For 4way, returns number of ESCs found | +* **Notes:** If successful, sets `mspPostProcessFn` to the appropriate handler (`mspSerialPassthroughFn` or `esc4wayProcess`). This handler takes over the serial port after the reply is sent. Requires `USE_SERIAL_4WAY_BLHELI_INTERFACE` for ESC passthrough. + +### `MSP_RTC` (246 / 0xF6) + +* **Direction:** Out +* **Description:** Retrieves the current Real-Time Clock time. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `seconds` | `int32_t` | 4 | Seconds | Seconds since epoch (or relative time if not set). 0 if RTC time unknown | + | `millis` | `uint16_t` | 2 | Milliseconds | Millisecond part of the time. 0 if RTC time unknown | +* **Notes:** Requires RTC hardware/support. Returns (0, 0) if time is not available/set. + +### `MSP_SET_RTC` (247 / 0xF7) + +* **Direction:** In +* **Description:** Sets the Real-Time Clock time. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `seconds` | `int32_t` | 4 | Seconds | Seconds component of time to set | + | `millis` | `uint16_t` | 2 | Milliseconds | Millisecond component of time to set | +* **Notes:** Requires RTC hardware/support. Expects 6 bytes. Uses `rtcSet()`. + +### `MSP_EEPROM_WRITE` (250 / 0xFA) + +* **Direction:** In +* **Description:** Saves the current configuration from RAM to non-volatile memory (EEPROM/Flash). +* **Payload:** None +* **Notes:** Will fail if armed. Suspends RX, calls `writeEEPROM()`, `readEEPROM()`, resumes RX. + +### `MSP_DEBUGMSG` (253 / 0xFD) + +* **Direction:** Out +* **Description:** Retrieves debug ("serial printf") messages from the firmware. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | Message Text | `char[]` | Variable | `NUL` terminated [debug message](https://github.com/iNavFlight/inav/blob/master/docs/development/serial_printf_debugging.md) text | + +### `MSP_DEBUG` (254 / 0xFE) + +* **Direction:** Out +* **Description:** Retrieves values from the firmware's `debug[]` array (legacy 16-bit version). +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `debugValues` | `uint16_t[4]` | 8 | First 4 values from the `debug` array | +* **Notes:** Useful for developers. See `MSP2_INAV_DEBUG` for 32-bit values. + +### `MSP_V2_FRAME` (255 / 0xFF) + +* **Direction:** N/A (Indicator) +* **Description:** This ID is used as a *payload indicator* within an MSPv1 message structure (`$M>`) to signify that the following payload conforms to the MSPv2 format. It's not a command itself. +* **Notes:** See MSPv2 documentation for the actual frame structure that follows this indicator. + +--- + +## MSPv1 Extended/INAV Commands (150-166) + +### `MSP_STATUS_EX` (150 / 0x96) + +* **Direction:** Out +* **Description:** Provides extended flight controller status, including CPU load, arming flags, and calibration status, in addition to `MSP_STATUS` fields. +* **Payload:** (Starts with `MSP_STATUS` fields) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `cycleTime` | `uint16_t` | 2 | µs | Main loop cycle time | + | `i2cErrors` | `uint16_t` | 2 | Count | I2C errors | + | `sensorStatus` | `uint16_t` | 2 | Bitmask | Sensor status bitmask | + | `activeModesLow` | `uint32_t` | 4 | Bitmask | First 32 active modes | + | `profile` | `uint8_t` | 1 | Index | Current config profile index | + | `cpuLoad` | `uint16_t` | 2 | % | Average system load percentage (`averageSystemLoadPercent`) | + | `armingFlags` | `uint16_t` | 2 | Bitmask | Flight controller arming flags (`armingFlags`). Note: Truncated to 16 bits | + | `accCalibAxisFlags` | `uint8_t` | 1 | Bitmask | Accelerometer calibrated axes flags (`accGetCalibrationAxisFlags()`) | +* **Notes:** Superseded by `MSP2_INAV_STATUS` which provides the full 32-bit `armingFlags` and other enhancements. + +### `MSP_SENSOR_STATUS` (151 / 0x97) + +* **Direction:** Out +* **Description:** Provides the hardware status for each individual sensor system. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `overallHealth` | `uint8_t` | 1 | Boolean | 1 if all essential hardware is healthy, 0 otherwise (`isHardwareHealthy()`) | + | `gyroStatus` | `uint8_t` | 1 | Enum | Enum `hardwareSensorStatus_e` Gyro hardware status (`getHwGyroStatus()`) | + | `accStatus` | `uint8_t` | 1 | Enum | Enum `hardwareSensorStatus_e` Accelerometer hardware status (`getHwAccelerometerStatus()`) | + | `magStatus` | `uint8_t` | 1 | Enum | Enum `hardwareSensorStatus_e` Compass hardware status (`getHwCompassStatus()`) | + | `baroStatus` | `uint8_t` | 1 | Enum | Enum `hardwareSensorStatus_e` Barometer hardware status (`getHwBarometerStatus()`) | + | `gpsStatus` | `uint8_t` | 1 | Enum | Enum `hardwareSensorStatus_e` GPS hardware status (`getHwGPSStatus()`) | + | `rangefinderStatus` | `uint8_t` | 1 | Enum | Enum `hardwareSensorStatus_e` Rangefinder hardware status (`getHwRangefinderStatus()`) | + | `pitotStatus` | `uint8_t` | 1 | Enum | Enum `hardwareSensorStatus_e` Pitot hardware status (`getHwPitotmeterStatus()`) | + | `opflowStatus` | `uint8_t` | 1 | Enum | Enum `hardwareSensorStatus_e` Optical Flow hardware status (`getHwOpticalFlowStatus()`) | +* **Notes:** Status values likely correspond to `SENSOR_STATUS_*` enums (e.g., OK, Unhealthy, Not Present). + +### `MSP_UID` (160 / 0xA0) + +* **Direction:** Out +* **Description:** Provides the unique identifier of the microcontroller. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `uid0` | `uint32_t` | 4 | First 32 bits of the unique ID (`U_ID_0`) | + | `uid1` | `uint32_t` | 4 | Middle 32 bits of the unique ID (`U_ID_1`) | + | `uid2` | `uint32_t` | 4 | Last 32 bits of the unique ID (`U_ID_2`) | +* **Notes:** Total 12 bytes, representing a 96-bit unique ID. + +### `MSP_GPSSVINFO` (164 / 0xA4) + +* **Direction:** Out +* **Description:** Provides satellite signal strength information (legacy U-Blox compatibility stub). +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `protocolVersion` | `uint8_t` | 1 | Always 1 (Stub version) | + | `numChannels` | `uint8_t` | 1 | Always 0 (Number of SV info channels reported) | + | `hdopHundreds` | `uint8_t` | 1 | HDOP / 100 (`gpsSol.hdop / 100`) | + | `hdopUnits` | `uint8_t` | 1 | HDOP / 100 (`gpsSol.hdop / 100`) | +* **Notes:** Requires `USE_GPS`. This is just a stub in INAV and does not provide actual per-satellite signal info. `hdopUnits` duplicates `hdopHundreds`. + +### `MSP_GPSSTATISTICS` (166 / 0xA6) + +* **Direction:** Out +* **Description:** Provides debugging statistics for the GPS communication link. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `lastMessageDt` | `uint16_t` | 2 | ms | Time since last valid GPS message (`gpsStats.lastMessageDt`) | + | `errors` | `uint32_t` | 4 | Count | Number of GPS communication errors (`gpsStats.errors`) | + | `timeouts` | `uint32_t` | 4 | Count | Number of GPS communication timeouts (`gpsStats.timeouts`) | + | `packetCount` | `uint32_t` | 4 | Count | Number of valid GPS packets received (`gpsStats.packetCount`) | + | `hdop` | `uint16_t` | 2 | HDOP * 100 | Horizontal Dilution of Precision (`gpsSol.hdop`) | + | `eph` | `uint16_t` | 2 | cm | Estimated Horizontal Position Accuracy (`gpsSol.eph`) | + | `epv` | `uint16_t` | 2 | cm | Estimated Vertical Position Accuracy (`gpsSol.epv`) | +* **Notes:** Requires `USE_GPS`. + +### `MSP_TX_INFO` (187 / 0xBB) + +* **Direction:** Out +* **Description:** Provides information potentially useful for transmitter LUA scripts. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `rssiSource` | `uint8_t` | 1 | Enum: Source of the RSSI value (`getRSSISource()`) | + | `rtcDateTimeIsSet` | `uint8_t` | 1 | Boolean: 1 if the RTC has been set, 0 otherwise | +* **Notes:** See `rssiSource_e`. + +### `MSP_SET_TX_INFO` (186 / 0xBA) + +* **Direction:** In +* **Description:** Allows a transmitter LUA script (or similar) to send runtime information (currently only RSSI) to the firmware. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `rssi` | `uint8_t` | 1 | % | RSSI value (0-100) provided by the external source | +* **Notes:** Calls `setRSSIFromMSP()`. Expects 1 byte. + +--- + +## MSPv2 Common Commands (0x1000 Range) + +These commands are part of the MSPv2 specification and are intended for general configuration and interaction. + +### `MSP2_COMMON_TZ` (0x1001 / 4097) + +* **Direction:** Out +* **Description:** Gets the time zone offset configuration. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `tzOffsetMinutes` | `int16_t` | 2 | Minutes | Time zone offset from UTC (`timeConfig()->tz_offset`) | + | `tzAutoDst` | `uint8_t` | 1 | Boolean | Automatic daylight saving time enabled (`timeConfig()->tz_automatic_dst`) | + +### `MSP2_COMMON_SET_TZ` (0x1002 / 4098) + +* **Direction:** In +* **Description:** Sets the time zone offset configuration. +* **Payload (Format 1 - Offset only):** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `tzOffsetMinutes` | `int16_t` | 2 | Minutes | Sets `timeConfigMutable()->tz_offset` | +* **Payload (Format 2 - Offset + DST):** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `tzOffsetMinutes` | `int16_t` | 2 | Minutes | Sets `timeConfigMutable()->tz_offset` | + | `tzAutoDst` | `uint8_t` | 1 | Boolean | Sets `timeConfigMutable()->tz_automatic_dst` | +* **Notes:** Accepts 2 or 3 bytes. + +### `MSP2_COMMON_SETTING` (0x1003 / 4099) + +* **Direction:** In/Out +* **Description:** Gets the value of a specific configuration setting, identified by name or index. +* **Request Payload (By Name):** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `settingName` | `char[]` | Variable | Null-terminated string containing the setting name (e.g., "gyro_main_lpf_hz") | +* **Request Payload (By Index):** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `zeroByte` | `uint8_t` | 1 | Must be 0 | + | `settingIndex` | `uint16_t` | 2 | Absolute index of the setting | +* **Reply Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `settingValue` | `uint8_t[]` | Variable | Raw byte value of the setting. Size depends on the setting's type (`settingGetValueSize()`) | +* **Notes:** Returns error if setting not found. Use `MSP2_COMMON_SETTING_INFO` to discover settings, types, and sizes. + +### `MSP2_COMMON_SET_SETTING` (0x1004 / 4100) + +* **Direction:** In +* **Description:** Sets the value of a specific configuration setting, identified by name or index. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `settingIdentifier` | Varies | Variable | Setting name (null-terminated string) OR Index (0x00 followed by `uint16_t` index) | + | `settingValue` | `uint8_t[]` | Variable | Raw byte value to set for the setting. Size must match the setting's type | +* **Notes:** Performs type checking and range validation (min/max). Returns error if setting not found, value size mismatch, or value out of range. Handles different data types (`uint8`, `int16`, `float`, `string`, etc.) internally. + +### `MSP2_COMMON_MOTOR_MIXER` (0x1005 / 4101) + +* **Direction:** Out +* **Description:** Retrieves the current motor mixer configuration (throttle, roll, pitch, yaw weights for each motor) for the primary and secondary mixer profiles. +* **Payload (Profile 1):** Repeated `MAX_SUPPORTED_MOTORS` times: + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `throttleWeight` | `uint16_t` | 2 | Scaled (0-4000) | Throttle weight * 1000, offset by 2000. (Range -2.0 to +2.0 -> 0 to 4000) | + | `rollWeight` | `uint16_t` | 2 | Scaled (0-4000) | Roll weight * 1000, offset by 2000 | + | `pitchWeight` | `uint16_t` | 2 | Scaled (0-4000) | Pitch weight * 1000, offset by 2000 | + | `yawWeight` | `uint16_t` | 2 | Scaled (0-4000) | Yaw weight * 1000, offset by 2000 | + | `throttleWeight` | `uint16_t` | 2 | (Optional) Scaled (0-4000) | Profile 2 Throttle weight | + | `rollWeight` | `uint16_t` | 2 | (Optional) Scaled (0-4000) | Profile 2 Roll weight | + | `pitchWeight` | `uint16_t` | 2 | (Optional) Scaled (0-4000) | Profile 2 Pitch weight | + | `yawWeight` | `uint16_t` | 2 | (Optional) Scaled (0-4000) | Profile 2 Yaw weight | +* **Notes:** Scaling is `(float_weight + 2.0) * 1000`. `primaryMotorMixer()` provides the data. + +### `MSP2_COMMON_SET_MOTOR_MIXER` (0x1006 / 4102) + +* **Direction:** In +* **Description:** Sets the motor mixer weights for a single motor in the primary mixer profile. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `motorIndex` | `uint8_t` | 1 | Index | Index of the motor to configure (0 to `MAX_SUPPORTED_MOTORS - 1`) | + | `throttleWeight` | `uint16_t` | 2 | Scaled (0-4000) | Sets throttle weight from `(value / 1000.0) - 2.0` | + | `rollWeight` | `uint16_t` | 2 | Scaled (0-4000) | Sets roll weight from `(value / 1000.0) - 2.0` | + | `pitchWeight` | `uint16_t` | 2 | Scaled (0-4000) | Sets pitch weight from `(value / 1000.0) - 2.0` | + | `yawWeight` | `uint16_t` | 2 | Scaled (0-4000) | Sets yaw weight from `(value / 1000.0) - 2.0` | +* **Notes:** Expects 9 bytes. Modifies `primaryMotorMixerMutable()`. Returns error if index is invalid. + +### `MSP2_COMMON_SETTING_INFO` (0x1007 / 4103) + +* **Direction:** In/Out +* **Description:** Gets detailed information about a specific configuration setting (name, type, range, flags, current value, etc.). +* **Request Payload:** Same as `MSP2_COMMON_SETTING` request (name string or index). +* **Reply Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `settingName` | `char[]` | Variable | Null-terminated setting name | + | `pgn` | `uint16_t` | 2 | Parameter Group Number (PGN) ID | + | `type` | `uint8_t` | 1 | Variable type (`VAR_UINT8`, `VAR_FLOAT`, etc.) | + | `section` | `uint8_t` | 1 | Setting section (`MASTER_VALUE`, `PROFILE_VALUE`, etc.) | + | `mode` | `uint8_t` | 1 | Setting mode (`MODE_NORMAL`, `MODE_LOOKUP`, etc.) | + | `minValue` | `int32_t` | 4 | Minimum allowed value (as signed 32-bit) | + | `maxValue` | `uint32_t` | 4 | Maximum allowed value (as unsigned 32-bit) | + | `settingIndex` | `uint16_t` | 2 | Absolute index of the setting | + | `profileIndex` | `uint8_t` | 1 | Current profile index (if applicable, else 0) | + | `profileCount` | `uint8_t` | 1 | Total number of profiles (if applicable, else 0) | + | `lookupNames` | `char[]` | Variable | (If `mode == MODE_LOOKUP`) Series of null-terminated strings for each possible value from min to max | + | `settingValue` | `uint8_t[]` | Variable | Current raw byte value of the setting | +* **Notes:** + + * Very useful for configurators to dynamically build interfaces. Returns error if setting not found. + * This is a variable length message, depending on the name length, `mode`, and `type`. + +### `MSP2_COMMON_PG_LIST` (0x1008 / 4104) + +* **Direction:** In/Out +* **Description:** Gets a list of Parameter Group Numbers (PGNs) used by settings, along with the start and end setting indexes for each group. Can request info for a single PGN. +* **Request Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `pgn` | `uint16_t` | 2 | (Optional) PGN ID to query. If omitted, returns all used PGNs | +* **Reply Payload:** Repeated for each PGN found: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `pgn` | `uint16_t` | 2 | Parameter Group Number (PGN) ID | + | `startIndex` | `uint16_t` | 2 | Absolute index of the first setting in this group | + | `endIndex` | `uint16_t` | 2 | Absolute index of the last setting in this group | +* **Notes:** Allows efficient fetching of related settings by group. + +### `MSP2_COMMON_SERIAL_CONFIG` (0x1009 / 4105) + +* **Direction:** Out +* **Description:** Retrieves the configuration for all available serial ports. +* **Payload:** Repeated for each available serial port: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `identifier` | `uint8_t` | 1 | Port identifier Enum (`serialPortIdentifier_e`) | + | `functionMask` | `uint32_t` | 4 | Bitmask of enabled functions (`FUNCTION_*`) | + | `mspBaudIndex` | `uint8_t` | 1 | Baud rate index for MSP function | + | `gpsBaudIndex` | `uint8_t` | 1 | Baud rate index for GPS function | + | `telemetryBaudIndex` | `uint8_t` | 1 | Baud rate index for Telemetry function | + | `peripheralBaudIndex` | `uint8_t` | 1 | Baud rate index for other peripheral functions | +* **Notes:** Baud rate indexes map to actual baud rates (e.g., 9600, 115200). See `baudRates` array. + +### `MSP2_COMMON_SET_SERIAL_CONFIG` (0x100A / 4106) + +* **Direction:** In +* **Description:** Sets the configuration for one or more serial ports. +* **Payload:** Repeated for each port being configured: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `identifier` | `uint8_t` | 1 | Port identifier Enum (`serialPortIdentifier_e`) | + | `functionMask` | `uint32_t` | 4 | Bitmask of functions to enable | + | `mspBaudIndex` | `uint8_t` | 1 | Baud rate index for MSP | + | `gpsBaudIndex` | `uint8_t` | 1 | Baud rate index for GPS | + | `telemetryBaudIndex` | `uint8_t` | 1 | Baud rate index for Telemetry | + | `peripheralBaudIndex` | `uint8_t` | 1 | Baud rate index for peripherals | +* **Notes:** Payload size must be a multiple of the size of one port config entry (1 + 4 + 4 = 9 bytes). Returns error if identifier is invalid or size is incorrect. Baud rate indexes are constrained `BAUD_MIN` to `BAUD_MAX`. + +### `MSP2_COMMON_SET_RADAR_POS` (0x100B / 4107) + +* **Direction:** In +* **Description:** Sets the position and status information for a "radar" Point of Interest (POI). Used for displaying other craft/objects on the OSD map. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `poiIndex` | `uint8_t` | 1 | Index | Index of the POI slot (0 to `RADAR_MAX_POIS - 1`) | + | `state` | `uint8_t` | 1 | Enum | Status of the POI (0=undefined, 1=armed, 2=lost) | + | `latitude` | `int32_t` | 4 | deg * 1e7 | Latitude of the POI | + | `longitude` | `int32_t` | 4 | deg * 1e7 | Longitude of the POI | + | `altitude` | `int32_t` | 4 | cm | Altitude of the POI | + | `heading` | `int16_t` | 2 | degrees | Heading of the POI | + | `speed` | `uint16_t` | 2 | cm/s | Speed of the POI | + | `linkQuality` | `uint8_t` | 1 | 0-4 | Link quality indicator | +* **Notes:** Expects 19 bytes. Updates the `radar_pois` array. + +### `MSP2_COMMON_SET_RADAR_ITD` (0x100C / 4108) + +* **Direction:** In +* **Description:** Sets radar information to display (likely internal/unused). +* **Notes:** Not implemented in INAV `fc_msp.c`. + +### `MSP2_COMMON_SET_MSP_RC_LINK_STATS` (0x100D / 4109) + +* **Direction:** In +* **Description:** Provides RC link statistics (RSSI, LQ) to the FC, typically from an MSP-based RC link (like ExpressLRS). Sent periodically by the RC link. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `sublinkID` | `uint8_t` | 1 | - | Sublink identifier (usually 0) | + | `validLink` | `uint8_t` | 1 | Boolean | Indicates if the link is currently valid (not in failsafe) | + | `rssiPercent` | `uint8_t` | 1 | % | Uplink RSSI percentage (0-100) | + | `uplinkRSSI_dBm` | `uint8_t` | 1 | -dBm | Uplink RSSI in dBm (sent as positive, e.g., 70 means -70dBm) | + | `downlinkLQ` | `uint8_t` | 1 | % | Downlink Link Quality (0-100) | + | `uplinkLQ` | `uint8_t` | 1 | % | Uplink Link Quality (0-100) | + | `uplinkSNR` | `int8_t` | 1 | dB | Uplink Signal-to-Noise Ratio | +* **Notes:** Requires `USE_RX_MSP`. Expects at least 7 bytes. Updates `rxLinkStatistics` and sets RSSI via `setRSSIFromMSP_RC()` only if `sublinkID` is 0. This message expects **no reply** (`MSP_RESULT_NO_REPLY`). + +### `MSP2_COMMON_SET_MSP_RC_INFO` (0x100E / 4110) + +* **Direction:** In +* **Description:** Provides additional RC link information (power levels, band, mode) to the FC from an MSP-based RC link. Sent less frequently than link stats. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `sublinkID` | `uint8_t` | 1 | - | Sublink identifier (usually 0) | + | `uplinkTxPower` | `uint16_t` | 2 | mW? | Uplink transmitter power level | + | `downlinkTxPower` | `uint16_t` | 2 | mW? | Downlink transmitter power level | + | `band` | `char[4]` | 4 | - | Operating band string (e.g., "2G4", "900") | + | `mode` | `char[6]` | 6 | - | Operating mode/rate string (e.g., "100HZ", "F1000") | +* **Notes:** Requires `USE_RX_MSP`. Expects at least 15 bytes. Updates `rxLinkStatistics` only if `sublinkID` is 0. Converts band/mode strings to uppercase. This message expects **no reply** (`MSP_RESULT_NO_REPLY`). + +--- + +## MSPv2 INAV Specific Commands (0x2000 Range) + +These commands are specific extensions added by the INAV project. + +### `MSP2_INAV_STATUS` (0x2000 / 8192) + +* **Direction:** Out +* **Description:** Provides comprehensive flight controller status, extending `MSP_STATUS_EX` with full arming flags, battery profile, and mixer profile. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `cycleTime` | `uint16_t` | 2 | µs | Main loop cycle time | + | `i2cErrors` | `uint16_t` | 2 | Count | I2C errors | + | `sensorStatus` | `uint16_t` | 2 | Bitmask | Sensor status bitmask | + | `cpuLoad` | `uint16_t` | 2 | % | Average system load percentage | + | `profileAndBattProfile` | `uint8_t` | 1 | Packed | Bits 0-3: Config profile index (`getConfigProfile()`), Bits 4-7: Battery profile index (`getConfigBatteryProfile()`) | + | `armingFlags` | `uint32_t` | 4 | Bitmask | Full 32-bit flight controller arming flags (`armingFlags`) | + | `activeModes` | `boxBitmask_t` | `sizeof(boxBitmask_t)` | Bitmask | Full bitmask of active flight modes (`packBoxModeFlags()`) | + | `mixerProfile` | `uint8_t` | 1 | Index | Current mixer profile index (`getConfigMixerProfile()`) | + +### `MSP2_INAV_OPTICAL_FLOW` (0x2001 / 8193) + +* **Direction:** Out +* **Description:** Provides data from the optical flow sensor. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `quality` | `uint8_t` | 1 | 0-255 | Raw quality indicator from the sensor (`opflow.rawQuality`). 0 if `USE_OPFLOW` disabled | + | `flowRateX` | `int16_t` | 2 | degrees/s | Optical flow rate X (roll axis) (`RADIANS_TO_DEGREES(opflow.flowRate[X])`). 0 if `USE_OPFLOW` disabled | + | `flowRateY` | `int16_t` | 2 | degrees/s | Optical flow rate Y (pitch axis) (`RADIANS_TO_DEGREES(opflow.flowRate[Y])`). 0 if `USE_OPFLOW` disabled | + | `bodyRateX` | `int16_t` | 2 | degrees/s | Compensated body rate X (roll axis) (`RADIANS_TO_DEGREES(opflow.bodyRate[X])`). 0 if `USE_OPFLOW` disabled | + | `bodyRateY` | `int16_t` | 2 | degrees/s | Compensated body rate Y (pitch axis) (`RADIANS_TO_DEGREES(opflow.bodyRate[Y])`). 0 if `USE_OPFLOW` disabled | +* **Notes:** Requires `USE_OPFLOW`. + +### `MSP2_INAV_ANALOG` (0x2002 / 8194) + +* **Direction:** Out +* **Description:** Provides detailed analog sensor readings, superseding `MSP_ANALOG` with higher precision and additional fields. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `batteryFlags` | `uint8_t` | 1 | Bitmask | Battery status flags: Bit 0=Full on plug-in, Bit 1=Use capacity threshold, Bit 2-3=Battery State enum (`getBatteryState()`), Bit 4-7=Cell Count (`getBatteryCellCount()`) | + | `vbat` | `uint16_t` | 2 | 0.01V | Battery voltage (`getBatteryVoltage()`) | + | `amperage` | `uint16_t` | 2 | 0.01A | Current draw (`getAmperage()`) | + | `powerDraw` | `uint32_t` | 4 | mW | Power draw (`getPower()`) | + | `mAhDrawn` | `uint32_t` | 4 | mAh | Consumed capacity (`getMAhDrawn()`) | + | `mWhDrawn` | `uint32_t` | 4 | mWh | Consumed energy (`getMWhDrawn()`) | + | `remainingCapacity` | `uint32_t` | 4 | mAh/mWh | Estimated remaining capacity (`getBatteryRemainingCapacity()`) | + | `percentageRemaining` | `uint8_t` | 1 | % | Estimated remaining capacity percentage (`calculateBatteryPercentage()`) | + | `rssi` | `uint16_t` | 2 | 0-1023 or % | RSSI value (`getRSSI()`) | + +### `MSP2_INAV_MISC` (0x2003 / 8195) + +* **Direction:** Out +* **Description:** Retrieves miscellaneous configuration settings, superseding `MSP_MISC` with higher precision and capacity fields. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `midRc` | `uint16_t` | 2 | PWM | Mid RC value (`PWM_RANGE_MIDDLE`) | + | `legacyMinThrottle` | `uint16_t` | 2 | - | Always 0 (Legacy) | + | `maxThrottle` | `uint16_t` | 2 | PWM | Maximum throttle command (`getMaxThrottle()`) | + | `minCommand` | `uint16_t` | 2 | PWM | Minimum motor command (`motorConfig()->mincommand`) | + | `failsafeThrottle` | `uint16_t` | 2 | PWM | Failsafe throttle level (`currentBatteryProfile->failsafe_throttle`) | + | `gpsType` | `uint8_t` | 1 | Enum | Enum `gpsProvider_e` GPS provider type (`gpsConfig()->provider`). 0 if `USE_GPS` disabled | + | `legacyGpsBaud` | `uint8_t` | 1 | - | Always 0 (Legacy) | + | `gpsSbasMode` | `uint8_t` | 1 | Enum | Enum `sbasMode_e` GPS SBAS mode (`gpsConfig()->sbasMode`). 0 if `USE_GPS` disabled | + | `rssiChannel` | `uint8_t` | 1 | Index | RSSI channel index (1-based) (`rxConfig()->rssi_channel`) | + | `magDeclination` | `uint16_t` | 2 | 0.1 degrees | Magnetic declination / 10 (`compassConfig()->mag_declination / 10`). 0 if `USE_MAG` disabled | + | `vbatScale` | `uint16_t` | 2 | Scale | Voltage scale (`batteryMetersConfig()->voltage.scale`). 0 if `USE_ADC` disabled | + | `vbatSource` | `uint8_t` | 1 | Enum | Enum `batVoltageSource_e` Voltage source (`batteryMetersConfig()->voltageSource`). 0 if `USE_ADC` disabled | + | `cellCount` | `uint8_t` | 1 | Count | Configured cell count (`currentBatteryProfile->cells`). 0 if `USE_ADC` disabled | + | `vbatCellDetect` | `uint16_t` | 2 | 0.01V | Cell detection voltage (`currentBatteryProfile->voltage.cellDetect`). 0 if `USE_ADC` disabled | + | `vbatMinCell` | `uint16_t` | 2 | 0.01V | Min cell voltage (`currentBatteryProfile->voltage.cellMin`). 0 if `USE_ADC` disabled | + | `vbatMaxCell` | `uint16_t` | 2 | 0.01V | Max cell voltage (`currentBatteryProfile->voltage.cellMax`). 0 if `USE_ADC` disabled | + | `vbatWarningCell` | `uint16_t` | 2 | 0.01V | Warning cell voltage (`currentBatteryProfile->voltage.cellWarning`). 0 if `USE_ADC` disabled | + | `capacityValue` | `uint32_t` | 4 | mAh/mWh | Battery capacity (`currentBatteryProfile->capacity.value`) | + | `capacityWarning` | `uint32_t` | 4 | mAh/mWh | Capacity warning threshold (`currentBatteryProfile->capacity.warning`) | + | `capacityCritical` | `uint32_t` | 4 | mAh/mWh | Capacity critical threshold (`currentBatteryProfile->capacity.critical`) | + | `capacityUnit` | `uint8_t` | 1 | Enum | Enum `batCapacityUnit_e` Capacity unit ('batteryMetersConfig()->capacity_unit') | + +### `MSP2_INAV_SET_MISC` (0x2004 / 8196) + +* **Direction:** In +* **Description:** Sets miscellaneous configuration settings, superseding `MSP_SET_MISC`. +* **Payload:** (Matches `MSP2_INAV_MISC` structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `midRc` | `uint16_t` | 2 | PWM | Ignored | + | `legacyMinThrottle` | `uint16_t` | 2 | - | Ignored | + | `legacyMaxThrottle` | `uint16_t` | 2 | - | Ignored | + | `minCommand` | `uint16_t` | 2 | PWM | Sets `motorConfigMutable()->mincommand` (constrained) | + | `failsafeThrottle` | `uint16_t` | 2 | PWM | Sets `currentBatteryProfileMutable->failsafe_throttle` (constrained) | + | `gpsType` | `uint8_t` | 1 | Enum | Enum `gpsProvider_e` Sets `gpsConfigMutable()->provider` (if `USE_GPS`) | + | `legacyGpsBaud` | `uint8_t` | 1 | - | Ignored | + | `gpsSbasMode` | `uint8_t` | 1 | Enum | Enum `sbasMode_e` Sets `gpsConfigMutable()->sbasMode` (if `USE_GPS`) | + | `rssiChannel` | `uint8_t` | 1 | Index | Sets `rxConfigMutable()->rssi_channel` (constrained). Updates source | + | `magDeclination` | `uint16_t` | 2 | 0.1 degrees | Sets `compassConfigMutable()->mag_declination = value * 10` (if `USE_MAG`) | + | `vbatScale` | `uint16_t` | 2 | Scale | Sets `batteryMetersConfigMutable()->voltage.scale` (if `USE_ADC`) | + | `vbatSource` | `uint8_t` | 1 | Enum | Enum `batVoltageSource_e` Sets `batteryMetersConfigMutable()->voltageSource` (if `USE_ADC`, validated) | + | `cellCount` | `uint8_t` | 1 | Count | Sets `currentBatteryProfileMutable->cells` (if `USE_ADC`) | + | `vbatCellDetect` | `uint16_t` | 2 | 0.01V | Sets `currentBatteryProfileMutable->voltage.cellDetect` (if `USE_ADC`) | + | `vbatMinCell` | `uint16_t` | 2 | 0.01V | Sets `currentBatteryProfileMutable->voltage.cellMin` (if `USE_ADC`) | + | `vbatMaxCell` | `uint16_t` | 2 | 0.01V | Sets `currentBatteryProfileMutable->voltage.cellMax` (if `USE_ADC`) | + | `vbatWarningCell` | `uint16_t` | 2 | 0.01V | Sets `currentBatteryProfileMutable->voltage.cellWarning` (if `USE_ADC`) | + | `capacityValue` | `uint32_t` | 4 | mAh/mWh | Sets `currentBatteryProfileMutable->capacity.value` | + | `capacityWarning` | `uint32_t` | 4 | mAh/mWh | Sets `currentBatteryProfileMutable->capacity.warning` | + | `capacityCritical` | `uint32_t` | 4 | mAh/mWh | Sets `currentBatteryProfileMutable->capacity.critical` | + | `capacityUnit` | `uint8_t` | 1 | Enum | Enum `batCapacityUnit_e` sets Capacity unit ('batteryMetersConfig()->capacity_unit'). Updates OSD energy unit if changed | +* **Notes:** Expects 41 bytes. Performs validation on `vbatSource` and `capacityUnit`. + +### `MSP2_INAV_BATTERY_CONFIG` (0x2005 / 8197) + +* **Direction:** Out +* **Description:** Retrieves the configuration specific to the battery voltage and current sensors and capacity settings for the current battery profile. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `vbatScale` | `uint16_t` | 2 | Scale | Voltage scale (`batteryMetersConfig()->voltage.scale`) | + | `vbatSource` | `uint8_t` | 1 | Enum | Enum `batVoltageSource_e` Voltage source (`batteryMetersConfig()->voltageSource`) | + | `cellCount` | `uint8_t` | 1 | Count | Configured cell count (`currentBatteryProfile->cells`) | + | `vbatCellDetect` | `uint16_t` | 2 | 0.01V | Cell detection voltage (`currentBatteryProfile->voltage.cellDetect`) | + | `vbatMinCell` | `uint16_t` | 2 | 0.01V | Min cell voltage (`currentBatteryProfile->voltage.cellMin`) | + | `vbatMaxCell` | `uint16_t` | 2 | 0.01V | Max cell voltage (`currentBatteryProfile->voltage.cellMax`) | + | `vbatWarningCell` | `uint16_t` | 2 | 0.01V | Warning cell voltage (`currentBatteryProfile->voltage.cellWarning`) | + | `currentOffset` | `uint16_t` | 2 | mV | Current sensor offset (`batteryMetersConfig()->current.offset`) | + | `currentScale` | `uint16_t` | 2 | Scale | Current sensor scale (`batteryMetersConfig()->current.scale`) | + | `capacityValue` | `uint32_t` | 4 | mAh/mWh | Battery capacity (`currentBatteryProfile->capacity.value`) | + | `capacityWarning` | `uint32_t` | 4 | mAh/mWh | Capacity warning threshold (`currentBatteryProfile->capacity.warning`) | + | `capacityCritical` | `uint32_t` | 4 | mAh/mWh | Capacity critical threshold (`currentBatteryProfile->capacity.critical`) | + | `capacityUnit` | `uint8_t` | 1 | Enum | Enum `batCapacityUnit_e` Capacity unit ('batteryMetersConfig()->capacity_unit') | +* **Notes:** Fields are 0 if `USE_ADC` is not defined. + +### `MSP2_INAV_SET_BATTERY_CONFIG` (0x2006 / 8198) + +* **Direction:** In +* **Description:** Sets the battery voltage/current sensor configuration and capacity settings for the current battery profile. +* **Payload:** (Matches `MSP2_INAV_BATTERY_CONFIG` structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `vbatScale` | `uint16_t` | 2 | Scale | Sets `batteryMetersConfigMutable()->voltage.scale` (if `USE_ADC`) | + | `vbatSource` | `uint8_t` | 1 | Enum | Enum `batVoltageSource_e` Sets `batteryMetersConfigMutable()->voltageSource` (if `USE_ADC`, validated) | + | `cellCount` | `uint8_t` | 1 | Count | Sets `currentBatteryProfileMutable->cells` (if `USE_ADC`) | + | `vbatCellDetect` | `uint16_t` | 2 | 0.01V | Sets `currentBatteryProfileMutable->voltage.cellDetect` (if `USE_ADC`) | + | `vbatMinCell` | `uint16_t` | 2 | 0.01V | Sets `currentBatteryProfileMutable->voltage.cellMin` (if `USE_ADC`) | + | `vbatMaxCell` | `uint16_t` | 2 | 0.01V | Sets `currentBatteryProfileMutable->voltage.cellMax` (if `USE_ADC`) | + | `vbatWarningCell` | `uint16_t` | 2 | 0.01V | Sets `currentBatteryProfileMutable->voltage.cellWarning` (if `USE_ADC`) | + | `currentOffset` | `uint16_t` | 2 | mV | Sets `batteryMetersConfigMutable()->current.offset` | + | `currentScale` | `uint16_t` | 2 | Scale | Sets `batteryMetersConfigMutable()->current.scale` | + | `capacityValue` | `uint32_t` | 4 | mAh/mWh | Sets `currentBatteryProfileMutable->capacity.value` | + | `capacityWarning` | `uint32_t` | 4 | mAh/mWh | Sets `currentBatteryProfileMutable->capacity.warning` | + | `capacityCritical` | `uint32_t` | 4 | mAh/mWh | Sets `currentBatteryProfileMutable->capacity.critical` | + | `capacityUnit` | `uint8_t` | 1 | Enum | Enum `batCapacityUnit_e` sets Capacity unit ('batteryMetersConfig()->capacity_unit') Updates OSD energy unit if changed | +* **Notes:** Expects 29 bytes. Performs validation on `vbatSource` and `capacityUnit`. + +### `MSP2_INAV_RATE_PROFILE` (0x2007 / 8199) + +* **Direction:** Out +* **Description:** Retrieves the rates and expos for the current control rate profile, including both stabilized and manual flight modes. Supersedes `MSP_RC_TUNING`. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `throttleMid` | `uint8_t` | 1 | Throttle Midpoint (`currentControlRateProfile->throttle.rcMid8`) | + | `throttleExpo` | `uint8_t` | 1 | Throttle Expo (`currentControlRateProfile->throttle.rcExpo8`) | + | `dynamicThrottlePID` | `uint8_t` | 1 | TPA value (`currentControlRateProfile->throttle.dynPID`) | + | `tpaBreakpoint` | `uint16_t` | 2 | TPA breakpoint (`currentControlRateProfile->throttle.pa_breakpoint`) | + | `stabRcExpo` | `uint8_t` | 1 | Stabilized Roll/Pitch Expo (`currentControlRateProfile->stabilized.rcExpo8`) | + | `stabRcYawExpo` | `uint8_t` | 1 | Stabilized Yaw Expo (`currentControlRateProfile->stabilized.rcYawExpo8`) | + | `stabRollRate` | `uint8_t` | 1 | Stabilized Roll Rate (`currentControlRateProfile->stabilized.rates[FD_ROLL]`) | + | `stabPitchRate` | `uint8_t` | 1 | Stabilized Pitch Rate (`currentControlRateProfile->stabilized.rates[FD_PITCH]`) | + | `stabYawRate` | `uint8_t` | 1 | Stabilized Yaw Rate (`currentControlRateProfile->stabilized.rates[FD_YAW]`) | + | `manualRcExpo` | `uint8_t` | 1 | Manual Roll/Pitch Expo (`currentControlRateProfile->manual.rcExpo8`) | + | `manualRcYawExpo` | `uint8_t` | 1 | Manual Yaw Expo (`currentControlRateProfile->manual.rcYawExpo8`) | + | `manualRollRate` | `uint8_t` | 1 | Manual Roll Rate (`currentControlRateProfile->manual.rates[FD_ROLL]`) | + | `manualPitchRate` | `uint8_t` | 1 | Manual Pitch Rate (`currentControlRateProfile->manual.rates[FD_PITCH]`) | + | `manualYawRate` | `uint8_t` | 1 | Manual Yaw Rate (`currentControlRateProfile->manual.rates[FD_YAW]`) | + +### `MSP2_INAV_SET_RATE_PROFILE` (0x2008 / 8200) + +* **Direction:** In +* **Description:** Sets the rates and expos for the current control rate profile (stabilized and manual). Supersedes `MSP_SET_RC_TUNING`. +* **Payload:** (Matches `MSP2_INAV_RATE_PROFILE` structure) + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `throttleMid` | `uint8_t` | 1 | Sets `currentControlRateProfile_p->throttle.rcMid8` | + | `throttleExpo` | `uint8_t` | 1 | Sets `currentControlRateProfile_p->throttle.rcExpo8` | + | `dynamicThrottlePID` | `uint8_t` | 1 | Sets `currentControlRateProfile_p->throttle.dynPID` | + | `tpaBreakpoint` | `uint16_t` | 2 | Sets `currentControlRateProfile_p->throttle.pa_breakpoint` | + | `stabRcExpo` | `uint8_t` | 1 | Sets `currentControlRateProfile_p->stabilized.rcExpo8` | + | `stabRcYawExpo` | `uint8_t` | 1 | Sets `currentControlRateProfile_p->stabilized.rcYawExpo8` | + | `stabRollRate` | `uint8_t` | 1 | Sets `currentControlRateProfile_p->stabilized.rates[FD_ROLL]` (constrained) | + | `stabPitchRate` | `uint8_t` | 1 | Sets `currentControlRateProfile_p->stabilized.rates[FD_PITCH]` (constrained) | + | `stabYawRate` | `uint8_t` | 1 | Sets `currentControlRateProfile_p->stabilized.rates[FD_YAW]` (constrained) | + | `manualRcExpo` | `uint8_t` | 1 | Sets `currentControlRateProfile_p->manual.rcExpo8` | + | `manualRcYawExpo` | `uint8_t` | 1 | Sets `currentControlRateProfile_p->manual.rcYawExpo8` | + | `manualRollRate` | `uint8_t` | 1 | Sets `currentControlRateProfile_p->manual.rates[FD_ROLL]` (constrained) | + | `manualPitchRate` | `uint8_t` | 1 | Sets `currentControlRateProfile_p->manual.rates[FD_PITCH]` (constrained) | + | `manualYawRate` | `uint8_t` | 1 | Sets `currentControlRateProfile_p->manual.rates[FD_YAW]` (constrained) | +* **Notes:** Expects 15 bytes. Constraints applied to rates based on axis. + +### `MSP2_INAV_AIR_SPEED` (0x2009 / 8201) + +* **Direction:** Out +* **Description:** Retrieves the estimated or measured airspeed. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `airspeed` | `uint32_t` | 4 | cm/s | Estimated/measured airspeed (`getAirspeedEstimate()`). 0 if `USE_PITOT` disabled or no valid data | +* **Notes:** Requires `USE_PITOT` for measured airspeed. May return GPS ground speed if pitot unavailable but GPS is present and configured. + +### `MSP2_INAV_OUTPUT_MAPPING` (0x200A / 8202) + +* **Direction:** Out +* **Description:** Retrieves the output mapping configuration (identifies which timer outputs are used for Motors/Servos). Legacy version sending only 8-bit usage flags. +* **Payload:** Repeated for each Motor/Servo timer: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `usageFlags` | `uint8_t` | 1 | Timer usage flags (truncated). `TIM_USE_MOTOR` or `TIM_USE_SERVO` | +* **Notes:** Superseded by `MSP2_INAV_OUTPUT_MAPPING_EXT2`. Only includes timers *not* used for PPM/PWM input. + +### `MSP2_INAV_MC_BRAKING` (0x200B / 8203) + +* **Direction:** Out +* **Description:** Retrieves configuration parameters for the multirotor braking mode feature. +* **Payload:** (Only if `USE_MR_BRAKING_MODE` defined) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `brakingSpeedThreshold` | `uint16_t` | 2 | cm/s | Speed above which braking engages (`navConfig()->mc.braking_speed_threshold`) | + | `brakingDisengageSpeed` | `uint16_t` | 2 | cm/s | Speed below which braking disengages (`navConfig()->mc.braking_disengage_speed`) | + | `brakingTimeout` | `uint16_t` | 2 | ms | Timeout before braking force reduces (`navConfig()->mc.braking_timeout`) | + | `brakingBoostFactor` | `uint8_t` | 1 | % | Boost factor applied during braking (`navConfig()->mc.braking_boost_factor`) | + | `brakingBoostTimeout` | `uint16_t` | 2 | ms | Timeout for the boost factor (`navConfig()->mc.braking_boost_timeout`) | + | `brakingBoostSpeedThreshold`| `uint16_t` | 2 | cm/s | Speed threshold for boost engagement (`navConfig()->mc.braking_boost_speed_threshold`) | + | `brakingBoostDisengageSpeed`| `uint16_t` | 2 | cm/s | Speed threshold for boost disengagement (`navConfig()->mc.braking_boost_disengage_speed`) | + | `brakingBankAngle` | `uint8_t` | 1 | degrees | Maximum bank angle allowed during braking (`navConfig()->mc.braking_bank_angle`) | +* **Notes:** Payload is empty if `USE_MR_BRAKING_MODE` is not defined. + +### `MSP2_INAV_SET_MC_BRAKING` (0x200C / 8204) + +* **Direction:** In +* **Description:** Sets configuration parameters for the multirotor braking mode feature. +* **Payload:** (Matches `MSP2_INAV_MC_BRAKING` structure, requires `USE_MR_BRAKING_MODE`) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `brakingSpeedThreshold` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->mc.braking_speed_threshold` | + | `brakingDisengageSpeed` | `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->mc.braking_disengage_speed` | + | `brakingTimeout` | `uint16_t` | 2 | ms | Sets `navConfigMutable()->mc.braking_timeout` | + | `brakingBoostFactor` | `uint8_t` | 1 | % | Sets `navConfigMutable()->mc.braking_boost_factor` | + | `brakingBoostTimeout` | `uint16_t` | 2 | ms | Sets `navConfigMutable()->mc.braking_boost_timeout` | + | `brakingBoostSpeedThreshold`| `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->mc.braking_boost_speed_threshold` | + | `brakingBoostDisengageSpeed`| `uint16_t` | 2 | cm/s | Sets `navConfigMutable()->mc.braking_boost_disengage_speed` | + | `brakingBankAngle` | `uint8_t` | 1 | degrees | Sets `navConfigMutable()->mc.braking_bank_angle` | +* **Notes:** Expects 14 bytes. Returns error if `USE_MR_BRAKING_MODE` is not defined. + +### `MSP2_INAV_OUTPUT_MAPPING_EXT` (0x200D / 8205) + +* **Direction:** Out +* **Description:** Retrieves extended output mapping configuration (timer ID and usage flags). Obsolete, use `MSP2_INAV_OUTPUT_MAPPING_EXT2`. +* **Payload:** Repeated for each Motor/Servo timer: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `timerId` | `uint8_t` | 1 | Hardware timer identifier (e.g., `TIM1`, `TIM2`). Value depends on target | + | `usageFlags` | `uint8_t` | 1 | Timer usage flags (truncated). `TIM_USE_MOTOR` or `TIM_USE_SERVO` | +* **Notes:** Usage flags are truncated to 8 bits. `timerId` mapping is target-specific. + +### `MSP2_INAV_TIMER_OUTPUT_MODE` (0x200E / 8206) + +* **Direction:** In/Out +* **Description:** Get or list the output mode override for hardware timers (e.g., force ONESHOT, DSHOT). +* **Request Payload (Get All):** None +* **Request Payload (Get One):** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `timerIndex` | `uint8_t` | 1 | Index of the hardware timer definition (0 to `HARDWARE_TIMER_DEFINITION_COUNT - 1`) | +* **Reply Payload (List All):** Repeated `HARDWARE_TIMER_DEFINITION_COUNT` times: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `timerIndex` | `uint8_t` | 1 | Timer index | + | `outputMode` | `uint8_t` | 1 | Output mode override (`TIMER_OUTPUT_MODE_*` enum) | +* **Reply Payload (Get One):** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `timerIndex` | `uint8_t` | 1 | Timer index requested | + | `outputMode` | `uint8_t` | 1 | Output mode override for the requested timer | +* **Notes:** Only available on non-SITL builds. `HARDWARE_TIMER_DEFINITION_COUNT` varies by target. + +### `MSP2_INAV_SET_TIMER_OUTPUT_MODE` (0x200F / 8207) + +* **Direction:** In +* **Description:** Set the output mode override for a specific hardware timer. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `timerIndex` | `uint8_t` | 1 | Index of the hardware timer definition | + | `outputMode` | `uint8_t` | 1 | Output mode override (`TIMER_OUTPUT_MODE_*` enum) to set | +* **Notes:** Only available on non-SITL builds. Expects 2 bytes. Returns error if `timerIndex` is invalid. + +### `MSP2_INAV_OUTPUT_MAPPING_EXT2` (0x210D / 8461) + +* **Direction:** Out +* **Description:** Retrieves the full extended output mapping configuration (timer ID, full 32-bit usage flags, and pin label). Supersedes `MSP2_INAV_OUTPUT_MAPPING_EXT`. +* **Payload:** Repeated for each Motor/Servo timer: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `timerId` | `uint8_t` | 1 | Hardware timer identifier (e.g., `TIM1`, `TIM2`). SITL uses index | + | `usageFlags` | `uint32_t` | 4 | Full 32-bit timer usage flags (`TIM_USE_*`) | + | `pinLabel` | `uint8_t` | 1 | Label for special pin usage (`PIN_LABEL_*` enum, e.g., `PIN_LABEL_LED`). 0 (`PIN_LABEL_NONE`) otherwise | +* **Notes:** Provides complete usage flags and helps identify pins repurposed for functions like LED strip. + +### `MSP2_INAV_MIXER` (0x2010 / 8208) + +* **Direction:** Out +* **Description:** Retrieves INAV-specific mixer configuration details. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `motorDirectionInverted` | `uint8_t` | 1 | Boolean: 1 if motor direction is reversed globally (`mixerConfig()->motorDirectionInverted`) | + | `reserved1` | `uint8_t` | 1 | Always 0 (Was yaw jump prevention limit) | + | `motorStopOnLow` | `uint8_t` | 1 | Boolean: 1 if motors stop at minimum throttle (`mixerConfig()->motorstopOnLow`) | + | `platformType` | `uint8_t` | 1 | Enum (`platformType_e`): Vehicle platform type (Multirotor, Airplane, etc.) (`mixerConfig()->platformType`) | + | `hasFlaps` | `uint8_t` | 1 | Boolean: 1 if the current mixer configuration includes flaps (`mixerConfig()->hasFlaps`) | + | `appliedMixerPreset` | `uint16_t` | 2 | Enum (`mixerPreset_e`): Mixer preset currently applied (`mixerConfig()->appliedMixerPreset`) | + | `maxMotors` | `uint8_t` | 1 | Constant: Maximum motors supported (`MAX_SUPPORTED_MOTORS`) | + | `maxServos` | `uint8_t` | 1 | Constant: Maximum servos supported (`MAX_SUPPORTED_SERVOS`) | + +### `MSP2_INAV_SET_MIXER` (0x2011 / 8209) + +* **Direction:** In +* **Description:** Sets INAV-specific mixer configuration details. +* **Payload:** (Matches `MSP2_INAV_MIXER` structure) + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `motorDirectionInverted` | `uint8_t` | 1 | Sets `mixerConfigMutable()->motorDirectionInverted` | + | `reserved1` | `uint8_t` | 1 | Ignored | + | `motorStopOnLow` | `uint8_t` | 1 | Sets `mixerConfigMutable()->motorstopOnLow` | + | `platformType` | `uint8_t` | 1 | Sets `mixerConfigMutable()->platformType` | + | `hasFlaps` | `uint8_t` | 1 | Sets `mixerConfigMutable()->hasFlaps` | + | `appliedMixerPreset` | `uint16_t` | 2 | Sets `mixerConfigMutable()->appliedMixerPreset` | + | `maxMotors` | `uint8_t` | 1 | Ignored | + | `maxServos` | `uint8_t` | 1 | Ignored | +* **Notes:** Expects 9 bytes. Calls `mixerUpdateStateFlags()`. + +### `MSP2_INAV_OSD_LAYOUTS` (0x2012 / 8210) + +* **Direction:** In/Out +* **Description:** Gets OSD layout information (counts, positions for a specific layout, or position for a specific item). +* **Request Payload (Get Counts):** None +* **Request Payload (Get Layout):** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `layoutIndex` | `uint8_t` | 1 | Index of the OSD layout (0 to `OSD_LAYOUT_COUNT - 1`) | +* **Request Payload (Get Item):** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `layoutIndex` | `uint8_t` | 1 | Index of the OSD layout | + | `itemIndex` | `uint16_t` | 2 | Index of the OSD item (`OSD_ITEM_*` enum, 0 to `OSD_ITEM_COUNT - 1`) | +* **Reply Payload (Get Counts):** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `layoutCount` | `uint8_t` | 1 | Number of OSD layouts (`OSD_LAYOUT_COUNT`) | + | `itemCount` | `uint8_t` | 1 | Number of OSD items per layout (`OSD_ITEM_COUNT`) | +* **Reply Payload (Get Layout):** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `itemPositions` | `uint16_t[OSD_ITEM_COUNT]` | `OSD_ITEM_COUNT * 2` | Packed X/Y positions for all items in the requested layout | +* **Reply Payload (Get Item):** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `itemPosition` | `uint16_t` | 2 | Packed X/Y position for the requested item in the requested layout | +* **Notes:** Requires `USE_OSD`. Returns error if indexes are invalid. + +### `MSP2_INAV_OSD_SET_LAYOUT_ITEM` (0x2013 / 8211) + +* **Direction:** In +* **Description:** Sets the position of a single OSD item within a specific layout. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `layoutIndex` | `uint8_t` | 1 | Index | Index of the OSD layout (0 to `OSD_LAYOUT_COUNT - 1`) | + | `itemIndex` | `uint8_t` | 1 | Index | Index of the OSD item (`OSD_ITEM_*` enum) | + | `itemPosition` | `uint16_t` | 2 | Coordinates | Packed X/Y position (`(Y << 8) | X`) to set | +* **Notes:** Requires `USE_OSD`. Expects 4 bytes. Returns error if indexes are invalid. If the modified layout is not the currently active one, it temporarily overrides the active layout for 10 seconds to show the change. Otherwise, triggers a full OSD redraw. + +### `MSP2_INAV_OSD_ALARMS` (0x2014 / 8212) + +* **Direction:** Out +* **Description:** Retrieves OSD alarm threshold settings. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `rssiAlarm` | `uint8_t` | 1 | % | RSSI alarm threshold (`osdConfig()->rssi_alarm`) | + | `timerAlarm` | `uint16_t` | 2 | seconds | Timer alarm threshold (`osdConfig()->time_alarm`) | + | `altAlarm` | `uint16_t` | 2 | meters | Altitude alarm threshold (`osdConfig()->alt_alarm`) | + | `distAlarm` | `uint16_t` | 2 | meters | Distance alarm threshold (`osdConfig()->dist_alarm`) | + | `negAltAlarm` | `uint16_t` | 2 | meters | Negative altitude alarm threshold (`osdConfig()->neg_alt_alarm`) | + | `gForceAlarm` | `uint16_t` | 2 | G * 1000 | G-force alarm threshold (`osdConfig()->gforce_alarm * 1000`) | + | `gForceAxisMinAlarm`| `int16_t` | 2 | G * 1000 | Min G-force per-axis alarm (`osdConfig()->gforce_axis_alarm_min * 1000`) | + | `gForceAxisMaxAlarm`| `int16_t` | 2 | G * 1000 | Max G-force per-axis alarm (`osdConfig()->gforce_axis_alarm_max * 1000`) | + | `currentAlarm` | `uint8_t` | 1 | 0.1 A ? | Current draw alarm threshold (`osdConfig()->current_alarm`). Units may need verification | + | `imuTempMinAlarm` | `uint16_t` | 2 | degrees C | Min IMU temperature alarm (`osdConfig()->imu_temp_alarm_min`) | + | `imuTempMaxAlarm` | `uint16_t` | 2 | degrees C | Max IMU temperature alarm (`osdConfig()->imu_temp_alarm_max`) | + | `baroTempMinAlarm` | `uint16_t` | 2 | degrees C | Min Baro temperature alarm (`osdConfig()->baro_temp_alarm_min`). 0 if `USE_BARO` disabled | + | `baroTempMaxAlarm` | `uint16_t` | 2 | degrees C | Max Baro temperature alarm (`osdConfig()->baro_temp_alarm_max`). 0 if `USE_BARO` disabled | + | `adsbWarnDistance`| `uint16_t` | 2 | meters | ADSB warning distance (`osdConfig()->adsb_distance_warning`). 0 if `USE_ADSB` disabled | + | `adsbAlertDistance`| `uint16_t` | 2 | meters | ADSB alert distance (`osdConfig()->adsb_distance_alert`). 0 if `USE_ADSB` disabled | +* **Notes:** Requires `USE_OSD`. + +### `MSP2_INAV_OSD_SET_ALARMS` (0x2015 / 8213) + +* **Direction:** In +* **Description:** Sets OSD alarm threshold settings. +* **Payload:** (Matches most of `MSP2_INAV_OSD_ALARMS` structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `rssiAlarm` | `uint8_t` | 1 | % | Sets `osdConfigMutable()->rssi_alarm` | + | `timerAlarm` | `uint16_t` | 2 | seconds | Sets `osdConfigMutable()->time_alarm` | + | `altAlarm` | `uint16_t` | 2 | meters | Sets `osdConfigMutable()->alt_alarm` | + | `distAlarm` | `uint16_t` | 2 | meters | Sets `osdConfigMutable()->dist_alarm` | + | `negAltAlarm` | `uint16_t` | 2 | meters | Sets `osdConfigMutable()->neg_alt_alarm` | + | `gForceAlarm` | `uint16_t` | 2 | G * 1000 | Sets `osdConfigMutable()->gforce_alarm = value / 1000.0f` | + | `gForceAxisMinAlarm`| `int16_t` | 2 | G * 1000 | Sets `osdConfigMutable()->gforce_axis_alarm_min = value / 1000.0f` | + | `gForceAxisMaxAlarm`| `int16_t` | 2 | G * 1000 | Sets `osdConfigMutable()->gforce_axis_alarm_max = value / 1000.0f` | + | `currentAlarm` | `uint8_t` | 1 | 0.1 A ? | Sets `osdConfigMutable()->current_alarm` | + | `imuTempMinAlarm` | `uint16_t` | 2 | degrees C | Sets `osdConfigMutable()->imu_temp_alarm_min` | + | `imuTempMaxAlarm` | `uint16_t` | 2 | degrees C | Sets `osdConfigMutable()->imu_temp_alarm_max` | + | `baroTempMinAlarm` | `uint16_t` | 2 | degrees C | Sets `osdConfigMutable()->baro_temp_alarm_min` (if `USE_BARO`) | + | `baroTempMaxAlarm` | `uint16_t` | 2 | degrees C | Sets `osdConfigMutable()->baro_temp_alarm_max` (if `USE_BARO`) | +* **Notes:** Requires `USE_OSD`. Expects 24 bytes. ADSB alarms are not settable via this message. + +### `MSP2_INAV_OSD_PREFERENCES` (0x2016 / 8214) + +* **Direction:** Out +* **Description:** Retrieves OSD display preferences (video system, units, styles, etc.). +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `videoSystem` | `uint8_t` | 1 | Enum: Video system (Auto/PAL/NTSC) (`osdConfig()->video_system`) | + | `mainVoltageDecimals` | `uint8_t` | 1 | Count: Decimal places for main voltage display (`osdConfig()->main_voltage_decimals`) | + | `ahiReverseRoll` | `uint8_t` | 1 | Boolean: Reverse roll direction on Artificial Horizon (`osdConfig()->ahi_reverse_roll`) | + | `crosshairsStyle` | `uint8_t` | 1 | Enum: Style of the center crosshairs (`osdConfig()->crosshairs_style`) | + | `leftSidebarScroll` | `uint8_t` | 1 | Boolean: Enable scrolling for left sidebar (`osdConfig()->left_sidebar_scroll`) | + | `rightSidebarScroll` | `uint8_t` | 1 | Boolean: Enable scrolling for right sidebar (`osdConfig()->right_sidebar_scroll`) | + | `sidebarScrollArrows` | `uint8_t` | 1 | Boolean: Show arrows for scrollable sidebars (`osdConfig()->sidebar_scroll_arrows`) | + | `units` | `uint8_t` | 1 | Enum: `osd_unit_e` Measurement units (Metric/Imperial) (`osdConfig()->units`) | + | `statsEnergyUnit` | `uint8_t` | 1 | Enum: Unit for energy display in post-flight stats (`osdConfig()->stats_energy_unit`) | +* **Notes:** Requires `USE_OSD`. + +### `MSP2_INAV_OSD_SET_PREFERENCES` (0x2017 / 8215) + +* **Direction:** In +* **Description:** Sets OSD display preferences. +* **Payload:** (Matches `MSP2_INAV_OSD_PREFERENCES` structure) + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `videoSystem` | `uint8_t` | 1 | Sets `osdConfigMutable()->video_system` | + | `mainVoltageDecimals` | `uint8_t` | 1 | Sets `osdConfigMutable()->main_voltage_decimals` | + | `ahiReverseRoll` | `uint8_t` | 1 | Sets `osdConfigMutable()->ahi_reverse_roll` | + | `crosshairsStyle` | `uint8_t` | 1 | Sets `osdConfigMutable()->crosshairs_style` | + | `leftSidebarScroll` | `uint8_t` | 1 | Sets `osdConfigMutable()->left_sidebar_scroll` | + | `rightSidebarScroll` | `uint8_t` | 1 | Sets `osdConfigMutable()->right_sidebar_scroll` | + | `sidebarScrollArrows` | `uint8_t` | 1 | Sets `osdConfigMutable()->sidebar_scroll_arrows` | + | `units` | `uint8_t` | 1 | Enum `osd_unit_e` Sets `osdConfigMutable()->units` | + | `statsEnergyUnit` | `uint8_t` | 1 | Sets `osdConfigMutable()->stats_energy_unit` | +* **Notes:** Requires `USE_OSD`. Expects 9 bytes. Triggers a full OSD redraw. + +### `MSP2_INAV_SELECT_BATTERY_PROFILE` (0x2018 / 8216) + +* **Direction:** In +* **Description:** Selects the active battery profile and saves configuration. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `batteryProfileIndex` | `uint8_t` | 1 | Index of the battery profile to activate (0-based) | +* **Notes:** Expects 1 byte. Will fail if armed. Calls `setConfigBatteryProfileAndWriteEEPROM()`. + +### `MSP2_INAV_DEBUG` (0x2019 / 8217) + +* **Direction:** Out +* **Description:** Retrieves values from the firmware's 32-bit `debug[]` array. Supersedes `MSP_DEBUG`. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `debugValues` | `uint32_t[DEBUG32_VALUE_COUNT]` | `DEBUG32_VALUE_COUNT * 4` | Values from the `debug` array (typically 8 values) | +* **Notes:** `DEBUG32_VALUE_COUNT` is usually 8. + +### `MSP2_BLACKBOX_CONFIG` (0x201A / 8218) + +* **Direction:** Out +* **Description:** Retrieves the Blackbox configuration. Supersedes `MSP_BLACKBOX_CONFIG`. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `blackboxSupported` | `uint8_t` | 1 | Boolean: 1 if Blackbox is supported (`USE_BLACKBOX`), 0 otherwise | + | `blackboxDevice` | `uint8_t` | 1 | Enum (`blackboxDevice_e`): Target device for logging (`blackboxConfig()->device`). 0 if not supported | + | `blackboxRateNum` | `uint16_t` | 2 | Numerator for logging rate divider (`blackboxConfig()->rate_num`). 0 if not supported | + | `blackboxRateDenom` | `uint16_t` | 2 | Denominator for logging rate divider (`blackboxConfig()->rate_denom`). 0 if not supported | + | `blackboxIncludeFlags`| `uint32_t`| 4 | Bitmask: Flags for fields included/excluded from logging (`blackboxConfig()->includeFlags`) | +* **Notes:** Requires `USE_BLACKBOX`. + +### `MSP2_SET_BLACKBOX_CONFIG` (0x201B / 8219) + +* **Direction:** In +* **Description:** Sets the Blackbox configuration. Supersedes `MSP_SET_BLACKBOX_CONFIG`. +* **Payload:** (Matches `MSP2_BLACKBOX_CONFIG` structure, excluding `blackboxSupported`) + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `blackboxDevice` | `uint8_t` | 1 | Sets `blackboxConfigMutable()->device` | + | `blackboxRateNum` | `uint16_t` | 2 | Sets `blackboxConfigMutable()->rate_num` | + | `blackboxRateDenom` | `uint16_t` | 2 | Sets `blackboxConfigMutable()->rate_denom` | + | `blackboxIncludeFlags`| `uint32_t`| 4 | Sets `blackboxConfigMutable()->includeFlags` | +* **Notes:** Requires `USE_BLACKBOX`. Expects 9 bytes. Returns error if Blackbox is currently logging (`!blackboxMayEditConfig()`). + +### `MSP2_INAV_TEMP_SENSOR_CONFIG` (0x201C / 8220) + +* **Direction:** Out +* **Description:** Retrieves the configuration for all onboard temperature sensors. +* **Payload:** Repeated `MAX_TEMP_SENSORS` times: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `type` | `uint8_t` | 1 | Enum (`tempSensorType_e`): Type of the temperature sensor | + | `address` | `uint64_t` | 8 | Sensor address/ID (e.g., for 1-Wire sensors) | + | `alarmMin` | `uint16_t` | 2 | Min temperature alarm threshold (degrees C) | + | `alarmMax` | `uint16_t` | 2 | Max temperature alarm threshold (degrees C) | + | `osdSymbol` | `uint8_t` | 1 | Index: OSD symbol to use for this sensor (0 to `TEMP_SENSOR_SYM_COUNT`) | + | `label` | `char[TEMPERATURE_LABEL_LEN]` | `TEMPERATURE_LABEL_LEN` | User-defined label for the sensor | +* **Notes:** Requires `USE_TEMPERATURE_SENSOR`. + +### `MSP2_INAV_SET_TEMP_SENSOR_CONFIG` (0x201D / 8221) + +* **Direction:** In +* **Description:** Sets the configuration for all onboard temperature sensors. +* **Payload:** Repeated `MAX_TEMP_SENSORS` times (matches `MSP2_INAV_TEMP_SENSOR_CONFIG` structure): + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `type` | `uint8_t` | 1 | Sets sensor type | + | `address` | `uint64_t` | 8 | Sets sensor address/ID | + | `alarmMin` | `uint16_t` | 2 | Sets min alarm threshold | + | `alarmMax` | `uint16_t` | 2 | Sets max alarm threshold | + | `osdSymbol` | `uint8_t` | 1 | Sets OSD symbol index (validated) | + | `label` | `char[TEMPERATURE_LABEL_LEN]` | `TEMPERATURE_LABEL_LEN` | Sets sensor label (converted to uppercase) | +* **Notes:** Requires `USE_TEMPERATURE_SENSOR`. Expects `MAX_TEMP_SENSORS * sizeof(tempSensorConfig_t)` bytes. + +### `MSP2_INAV_TEMPERATURES` (0x201E / 8222) + +* **Direction:** Out +* **Description:** Retrieves the current readings from all configured temperature sensors. +* **Payload:** Repeated `MAX_TEMP_SENSORS` times: + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `temperature` | `int16_t` | 2 | degrees C | Current temperature reading. -1000 if sensor is invalid or reading failed | +* **Notes:** Requires `USE_TEMPERATURE_SENSOR`. + +### `MSP_SIMULATOR` (0x201F / 8223) + +* **Direction:** In/Out +* **Description:** Handles Hardware-in-the-Loop (HITL) simulation data exchange. Receives simulated sensor data and options, sends back control outputs and debug info. +* **Request Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `simulatorVersion` | `uint8_t` | 1 | Version of the simulator protocol (`SIMULATOR_MSP_VERSION`) | + | `hitlFlags` | `uint8_t` | 1 | Bitmask: Options for HITL (`HITL_*` flags) | + | `gpsFixType` | `uint8_t` | 1 | (If `HITL_HAS_NEW_GPS_DATA`) Simulated GPS fix type | + | `gpsNumSat` | `uint8_t` | 1 | (If `HITL_HAS_NEW_GPS_DATA`) Simulated satellite count | + | `gpsLat` | `int32_t` | 4 | (If `HITL_HAS_NEW_GPS_DATA`) Simulated latitude (1e7 deg) | + | `gpsLon` | `int32_t` | 4 | (If `HITL_HAS_NEW_GPS_DATA`) Simulated longitude (1e7 deg) | + | `gpsAlt` | `int32_t` | 4 | (If `HITL_HAS_NEW_GPS_DATA`) Simulated altitude (cm) | + | `gpsSpeed` | `uint16_t` | 2 | (If `HITL_HAS_NEW_GPS_DATA`) Simulated ground speed (cm/s) | + | `gpsCourse` | `uint16_t` | 2 | (If `HITL_HAS_NEW_GPS_DATA`) Simulated ground course (deci-deg) | + | `gpsVelN` | `int16_t` | 2 | (If `HITL_HAS_NEW_GPS_DATA`) Simulated North velocity (cm/s) | + | `gpsVelE` | `int16_t` | 2 | (If `HITL_HAS_NEW_GPS_DATA`) Simulated East velocity (cm/s) | + | `gpsVelD` | `int16_t` | 2 | (If `HITL_HAS_NEW_GPS_DATA`) Simulated Down velocity (cm/s) | + | `imuRoll` | `int16_t` | 2 | (If NOT `HITL_USE_IMU`) Simulated Roll (deci-deg) | + | `imuPitch` | `int16_t` | 2 | (If NOT `HITL_USE_IMU`) Simulated Pitch (deci-deg) | + | `imuYaw` | `int16_t` | 2 | (If NOT `HITL_USE_IMU`) Simulated Yaw (deci-deg) | + | `accX` | `int16_t` | 2 | mG (G * 1000) | Simulated Accelerometer X | + | `accY` | `int16_t` | 2 | mG (G * 1000) | Simulated Accelerometer Y | + | `accZ` | `int16_t` | 2 | mG (G * 1000) | Simulated Accelerometer Z | + | `gyroX` | `int16_t` | 2 | dps * 16 | Simulated Gyroscope X rate | + | `gyroY` | `int16_t` | 2 | dps * 16 | Simulated Gyroscope Y rate | + | `gyroZ` | `int16_t` | 2 | dps * 16 | Simulated Gyroscope Z rate | + | `baroPressure` | `uint32_t` | 4 | Pa | Simulated Barometer pressure | + | `magX` | `int16_t` | 2 | Scaled | Simulated Magnetometer X (scaled by 20) | + | `magY` | `int16_t` | 2 | Scaled | Simulated Magnetometer Y (scaled by 20) | + | `magZ` | `int16_t` | 2 | Scaled | Simulated Magnetometer Z (scaled by 20) | + | `vbat` | `uint8_t` | 1 | (If `HITL_EXT_BATTERY_VOLTAGE`) Simulated battery voltage (0.1V units) | + | `airspeed` | `uint16_t` | 2 | (If `HITL_AIRSPEED`) Simulated airspeed (cm/s) | + | `extFlags` | `uint8_t` | 1 | (If `HITL_EXTENDED_FLAGS`) Additional flags (upper 8 bits) | +* **Reply Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `stabilizedRoll` | `uint16_t` | 2 | Stabilized Roll command output (-500 to 500) | + | `stabilizedPitch`| `uint16_t` | 2 | Stabilized Pitch command output (-500 to 500) | + | `stabilizedYaw` | `uint16_t` | 2 | Stabilized Yaw command output (-500 to 500) | + | `stabilizedThrottle`| `uint16_t`| 2 | Stabilized Throttle command output (-500 to 500 if armed, else -500) | + | `debugFlags` | `uint8_t` | 1 | Packed flags: Debug index (0-7), Platform type, Armed state, OSD feature status | + | `debugValue` | `uint32_t` | 4 | Current debug value (`debug[simulatorData.debugIndex]`) | + | `attitudeRoll` | `int16_t` | 2 | Current estimated Roll (deci-deg) | + | `attitudePitch`| `int16_t` | 2 | Current estimated Pitch (deci-deg) | + | `attitudeYaw` | `int16_t` | 2 | Current estimated Yaw (deci-deg) | + | `osdHeader` | `uint8_t` | 1 | OSD RLE Header (255) | + | `osdRows` | `uint8_t` | 1 | (If OSD supported) Number of OSD rows | + | `osdCols` | `uint8_t` | 1 | (If OSD supported) Number of OSD columns | + | `osdStartY` | `uint8_t` | 1 | (If OSD supported) Starting row for RLE data | + | `osdStartX` | `uint8_t` | 1 | (If OSD supported) Starting column for RLE data | + | `osdRleData` | `uint8_t[]` | Variable | (If OSD supported) Run-length encoded OSD character data. Terminated by `[0, 0]` | +* **Notes:** Requires `USE_SIMULATOR`. Complex message handling state changes for enabling/disabling HITL. Sensor data is injected directly. OSD data is sent using a custom RLE scheme. See `simulatorData` struct and associated code for details. + +### `MSP2_INAV_SERVO_MIXER` (0x2020 / 8224) + +* **Direction:** Out +* **Description:** Retrieves the custom servo mixer rules, including programming framework condition IDs, for primary and secondary mixer profiles. Supersedes `MSP_SERVO_MIX_RULES`. +* **Payload:** Repeated `MAX_SERVO_RULES` times: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `targetChannel` | `uint8_t` | 1 | Servo output channel index (0-based) | + | `inputSource` | `uint8_t` | 1 | Enum `inputSource_e` Input source | + | `rate` | `uint16_t` | 2 | Mixing rate/weight | + | `speed` | `uint8_t` | 1 | Speed/Slew rate limit (0-100) | + | `conditionId` | `uint8_t` | 1 | Logic Condition ID (0 to `MAX_LOGIC_CONDITIONS - 1`, or 255/-1 if none/disabled) | + | `targetChannel` | `uint8_t` | 1 | (Optional) Profile 2 Target channel | + | `inputSource` | `uint8_t` | 1 | (Optional) Profile 2 Enum `inputSource_e` Input source | + | `rate` | `uint16_t` | 2 | (Optional) Profile 2 Rate | + | `speed` | `uint8_t` | 1 | (Optional) Profile 2 Speed | + | `conditionId` | `uint8_t` | 1 | (Optional) Profile 2 Logic Condition ID | +* **Notes:** `conditionId` requires `USE_PROGRAMMING_FRAMEWORK`. + +### `MSP2_INAV_SET_SERVO_MIXER` (0x2021 / 8225) + +* **Direction:** In +* **Description:** Sets a single custom servo mixer rule, including programming framework condition ID. Supersedes `MSP_SET_SERVO_MIX_RULE`. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `ruleIndex` | `uint8_t` | 1 | Index of the rule to set (0 to `MAX_SERVO_RULES - 1`) | + | `targetChannel` | `uint8_t` | 1 | Servo output channel index | + | `inputSource` | `uint8_t` | 1 | Enum `inputSource_e` Input source | + | `rate` | `uint16_t` | 2 | Mixing rate/weight | + | `speed` | `uint8_t` | 1 | Speed/Slew rate limit (0-100) | + | `conditionId` | `uint8_t` | 1 | Logic Condition ID (255/-1 if none). Ignored if `USE_PROGRAMMING_FRAMEWORK` is disabled | +* **Notes:** Expects 7 bytes. Returns error if index invalid. Calls `loadCustomServoMixer()`. + +### `MSP2_INAV_LOGIC_CONDITIONS` (0x2022 / 8226) + +* **Direction:** Out +* **Description:** Retrieves the configuration of all defined Logic Conditions. +* **Payload:** Repeated `MAX_LOGIC_CONDITIONS` times: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `enabled` | `uint8_t` | 1 | Boolean: 1 if the condition is enabled | + | `activatorId` | `uint8_t` | 1 | ID of the activator condition (if any, 255 if none) | + | `operation` | `uint8_t` | 1 | Enum `logicConditionOp_e` Logical operation (AND, OR, XOR, etc.) | + | `operandAType` | `uint8_t` | 1 | Enum `logicOperandType_e` Type of the first operand (Flight Mode, GVAR, etc.) | + | `operandAValue` | `uint32_t` | 4 | Value/ID of the first operand | + | `operandBType` | `uint8_t` | 1 | Enum `logicOperandType_e`: Type of the second operand | + | `operandBValue` | `uint32_t` | 4 | Value/ID of the second operand | + | `flags` | `uint8_t` | 1 | Bitmask: Condition flags (e.g., `LC_FLAG_FIRST_TIME_TRUE`) | +* **Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. See `logicCondition_t` structure. + +### `MSP2_INAV_SET_LOGIC_CONDITIONS` (0x2023 / 8227) + +* **Direction:** In +* **Description:** Sets the configuration for a single Logic Condition by its index. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `conditionIndex` | `uint8_t` | 1 | Index of the condition to set (0 to `MAX_LOGIC_CONDITIONS - 1`) | + | `enabled` | `uint8_t` | 1 | Boolean: 1 to enable the condition | + | `activatorId` | `uint8_t` | 1 | Activator condition ID | + | `operation` | `uint8_t` | 1 | Enum `logicConditionOp_e` Logical operation | + | `operandAType` | `uint8_t` | 1 | Enum `logicOperandType_e` Type of operand A | + | `operandAValue` | `uint32_t` | 4 | Value/ID of operand A | + | `operandBType` | `uint8_t` | 1 | Enum `logicOperandType_e` Type of operand B | + | `operandBValue` | `uint32_t` | 4 | Value/ID of operand B | + | `flags` | `uint8_t` | 1 | Bitmask: Condition flags | +* **Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. Expects 15 bytes. Returns error if index is invalid. + +### `MSP2_INAV_GLOBAL_FUNCTIONS` (0x2024 / 8228) + +* **Not implemented** + +### `MSP2_INAV_SET_GLOBAL_FUNCTIONS` (0x2025 / 8229) + +* **Not implemented** + +### `MSP2_INAV_LOGIC_CONDITIONS_STATUS` (0x2026 / 8230) + +* **Direction:** Out +* **Description:** Retrieves the current evaluated status (true/false or numerical value) of all logic conditions. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `conditionValues` | `uint32_t[MAX_LOGIC_CONDITIONS]` | `MAX_LOGIC_CONDITIONS * 4` | Array of current values for each logic condition (`logicConditionGetValue(i)`). 1 for true, 0 for false, or numerical value depending on operation | +* **Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. + +### `MSP2_INAV_GVAR_STATUS` (0x2027 / 8231) + +* **Direction:** Out +* **Description:** Retrieves the current values of all Global Variables (GVARS). +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `gvarValues` | `uint32_t[MAX_GLOBAL_VARIABLES]` | `MAX_GLOBAL_VARIABLES * 4` | Array of current values for each global variable (`gvGet(i)`) | +* **Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. + +### `MSP2_INAV_PROGRAMMING_PID` (0x2028 / 8232) + +* **Direction:** Out +* **Description:** Retrieves the configuration of all Programming PIDs. +* **Payload:** Repeated `MAX_PROGRAMMING_PID_COUNT` times: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `enabled` | `uint8_t` | 1 | Boolean: 1 if the PID is enabled | + | `setpointType` | `uint8_t` | 1 | Enum (`logicOperandType_e`) Type of the setpoint source | + | `setpointValue` | `uint32_t` | 4 | Value/ID of the setpoint source | + | `measurementType` | `uint8_t` | 1 | Enum (`logicOperandType_e`) Type of the measurement source | + | `measurementValue` | `uint32_t` | 4 | Value/ID of the measurement source | + | `gainP` | `uint16_t` | 2 | Proportional gain | + | `gainI` | `uint16_t` | 2 | Integral gain | + | `gainD` | `uint16_t` | 2 | Derivative gain | + | `gainFF` | `uint16_t` | 2 | Feed-forward gain | +* **Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. See `programmingPid_t` structure. + +### `MSP2_INAV_SET_PROGRAMMING_PID` (0x2029 / 8233) + +* **Direction:** In +* **Description:** Sets the configuration for a single Programming PID by its index. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `pidIndex` | `uint8_t` | 1 | Index of the Programming PID to set (0 to `MAX_PROGRAMMING_PID_COUNT - 1`) | + | `enabled` | `uint8_t` | 1 | Boolean: 1 to enable the PID | + | `setpointType` | `uint8_t` | 1 | Enum (`logicOperandType_e`) Type of the setpoint source | + | `setpointValue` | `uint32_t` | 4 | Value/ID of the setpoint source | + | `measurementType` | `uint8_t` | 1 | Enum (`logicOperandType_e`) Type of the measurement source | + | `measurementValue` | `uint32_t` | 4 | Value/ID of the measurement source | + | `gainP` | `uint16_t` | 2 | Proportional gain | + | `gainI` | `uint16_t` | 2 | Integral gain | + | `gainD` | `uint16_t` | 2 | Derivative gain | + | `gainFF` | `uint16_t` | 2 | Feed-forward gain | +* **Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. Expects 20 bytes. Returns error if index is invalid. + +### `MSP2_INAV_PROGRAMMING_PID_STATUS` (0x202A / 8234) + +* **Direction:** Out +* **Description:** Retrieves the current output value of all Programming PIDs. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `pidOutputs` | `uint32_t[MAX_PROGRAMMING_PID_COUNT]` | `MAX_PROGRAMMING_PID_COUNT * 4` | Array of current output values for each Programming PID (`programmingPidGetOutput(i)`) | +* **Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. + +### `MSP2_PID` (0x2030 / 8240) + +* **Direction:** Out +* **Description:** Retrieves the standard PID controller gains (P, I, D, FF) for the current PID profile. +* **Payload:** Repeated `PID_ITEM_COUNT` times: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `P` | `uint8_t` | 1 | Proportional gain (`pidBank()->pid[i].P`), constrained 0-255 | + | `I` | `uint8_t` | 1 | Integral gain (`pidBank()->pid[i].I`), constrained 0-255 | + | `D` | `uint8_t` | 1 | Derivative gain (`pidBank()->pid[i].D`), constrained 0-255 | + | `FF` | `uint8_t` | 1 | Feed-forward gain (`pidBank()->pid[i].FF`), constrained 0-255 | +* **Notes:** `PID_ITEM_COUNT` defines the number of standard PID controllers (Roll, Pitch, Yaw, Alt, Vel, etc.). Updates from EZ-Tune if enabled. + +### `MSP2_SET_PID` (0x2031 / 8241) + +* **Direction:** In +* **Description:** Sets the standard PID controller gains (P, I, D, FF) for the current PID profile. +* **Payload:** Repeated `PID_ITEM_COUNT` times: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `P` | `uint8_t` | 1 | Sets Proportional gain (`pidBankMutable()->pid[i].P`) | + | `I` | `uint8_t` | 1 | Sets Integral gain (`pidBankMutable()->pid[i].I`) | + | `D` | `uint8_t` | 1 | Sets Derivative gain (`pidBankMutable()->pid[i].D`) | + | `FF` | `uint8_t` | 1 | Sets Feed-forward gain (`pidBankMutable()->pid[i].FF`) | +* **Notes:** Expects `PID_ITEM_COUNT * 4` bytes. Calls `schedulePidGainsUpdate()` and `navigationUsePIDs()`. + +### `MSP2_INAV_OPFLOW_CALIBRATION` (0x2032 / 8242) + +* **Direction:** In +* **Description:** Starts the optical flow sensor calibration procedure. +* **Payload:** None +* **Notes:** Requires `USE_OPFLOW`. Will fail if armed. Calls `opflowStartCalibration()`. + +### `MSP2_INAV_FWUPDT_PREPARE` (0x2033 / 8243) + +* **Direction:** In +* **Description:** Prepares the flight controller to receive a firmware update via MSP. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `firmwareSize` | `uint32_t` | 4 | Total size of the incoming firmware file in bytes | +* **Notes:** Requires `MSP_FIRMWARE_UPDATE`. Expects 4 bytes. Returns error if preparation fails (e.g., no storage, invalid size). Calls `firmwareUpdatePrepare()`. + +### `MSP2_INAV_FWUPDT_STORE` (0x2034 / 8244) + +* **Direction:** In +* **Description:** Stores a chunk of firmware data received via MSP. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `firmwareChunk` | `uint8_t[]` | Variable | Chunk of firmware data | +* **Notes:** Requires `MSP_FIRMWARE_UPDATE`. Returns error if storage fails (e.g., out of space, checksum error). Called repeatedly until the entire firmware is transferred. Calls `firmwareUpdateStore()`. + +### `MSP2_INAV_FWUPDT_EXEC` (0x2035 / 8245) + +* **Direction:** In +* **Description:** Executes the firmware update process (flashes the stored firmware and reboots). +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `updateType` | `uint8_t` | 1 | Type of update (e.g., full flash, specific section - currently ignored/unused) | +* **Notes:** Requires `MSP_FIRMWARE_UPDATE`. Expects 1 byte. Returns error if update cannot start (e.g., not fully received). Calls `firmwareUpdateExec()`. If successful, the device will reboot into the new firmware. + +### `MSP2_INAV_FWUPDT_ROLLBACK_PREPARE` (0x2036 / 8246) + +* **Direction:** In +* **Description:** Prepares the flight controller to perform a firmware rollback to the previously stored version. +* **Payload:** None +* **Notes:** Requires `MSP_FIRMWARE_UPDATE`. Returns error if rollback preparation fails (e.g., no rollback image available). Calls `firmwareUpdateRollbackPrepare()`. + +### `MSP2_INAV_FWUPDT_ROLLBACK_EXEC` (0x2037 / 8247) + +* **Direction:** In +* **Description:** Executes the firmware rollback process (flashes the stored backup firmware and reboots). +* **Payload:** None +* **Notes:** Requires `MSP_FIRMWARE_UPDATE`. Returns error if rollback cannot start. Calls `firmwareUpdateRollbackExec()`. If successful, the device will reboot into the backup firmware. + +### `MSP2_INAV_SAFEHOME` (0x2038 / 8248) + +* **Direction:** In/Out +* **Description:** Get or Set configuration for a specific Safe Home location. +* **Request Payload (Get):** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `safehomeIndex` | `uint8_t` | 1 | Index of the safe home location (0 to `MAX_SAFE_HOMES - 1`) | +* **Reply Payload (Get):** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `safehomeIndex` | `uint8_t` | 1 | Index requested | + | `enabled` | `uint8_t` | 1 | Boolean: 1 if this safe home is enabled | + | `latitude` | `int32_t` | 4 | Latitude (1e7 deg) | + | `longitude` | `int32_t` | 4 | Longitude (1e7 deg) | +* **Notes:** Requires `USE_SAFE_HOME`. Used by `mspFcSafeHomeOutCommand`. See `MSP2_INAV_SET_SAFEHOME` for setting. + +### `MSP2_INAV_SET_SAFEHOME` (0x2039 / 8249) + +* **Direction:** In +* **Description:** Sets the configuration for a specific Safe Home location. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `safehomeIndex` | `uint8_t` | 1 | Index of the safe home location (0 to `MAX_SAFE_HOMES - 1`) | + | `enabled` | `uint8_t` | 1 | Boolean: 1 to enable this safe home | + | `latitude` | `int32_t` | 4 | Latitude (1e7 deg) | + | `longitude` | `int32_t` | 4 | Longitude (1e7 deg) | +* **Notes:** Requires `USE_SAFE_HOME`. Expects 10 bytes. Returns error if index invalid. Resets corresponding FW autoland approach if `USE_FW_AUTOLAND` is enabled. + +### `MSP2_INAV_MISC2` (0x203A / 8250) + +* **Direction:** Out +* **Description:** Retrieves miscellaneous runtime information including timers and throttle status. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `uptimeSeconds` | `uint32_t` | 4 | Seconds | Time since boot (`micros() / 1000000`) | + | `flightTimeSeconds` | `uint32_t` | 4 | Seconds | Accumulated flight time (`getFlightTime()`) | + | `throttlePercent` | `uint8_t` | 1 | % | Current throttle output percentage (`getThrottlePercent(true)`) | + | `autoThrottleFlag` | `uint8_t` | 1 | Boolean | 1 if navigation is controlling throttle, 0 otherwise (`navigationIsControllingThrottle()`) | + +### `MSP2_INAV_LOGIC_CONDITIONS_SINGLE` (0x203B / 8251) + +* **Direction:** In/Out +* **Description:** Gets the configuration for a single Logic Condition by its index. +* **Request Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `conditionIndex` | `uint8_t` | 1 | Index of the condition to retrieve (0 to `MAX_LOGIC_CONDITIONS - 1`) | +* **Reply Payload:** (Matches structure of one entry in `MSP2_INAV_LOGIC_CONDITIONS`) + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `enabled` | `uint8_t` | 1 | Boolean: 1 if enabled | + | `activatorId` | `uint8_t` | 1 | Activator ID | + | `operation` | `uint8_t` | 1 | Enum `logicConditionOp_e` Logical operation | + | `operandAType` | `uint8_t` | 1 | Enum `logicOperandType_e` Type of operand A | + | `operandAValue` | `uint32_t` | 4 | Value/ID of operand A | + | `operandBType` | `uint8_t` | 1 | Enum `logicOperandType_e` Type of operand B | + | `operandBValue` | `uint32_t` | 4 | Value/ID of operand B | + | `flags` | `uint8_t` | 1 | Bitmask: Condition flags | +* **Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. Used by `mspFcLogicConditionCommand`. + +### `MSP2_INAV_ESC_RPM` (0x2040 / 8256) + +* **Direction:** Out +* **Description:** Retrieves the RPM reported by each ESC via telemetry. +* **Payload:** Repeated `getMotorCount()` times: + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `escRpm` | `uint32_t` | 4 | RPM | RPM reported by the ESC | +* **Notes:** Requires `USE_ESC_SENSOR`. Payload size depends on the number of detected motors with telemetry. + +### `MSP2_INAV_ESC_TELEM` (0x2041 / 8257) + +* **Direction:** Out +* **Description:** Retrieves the full telemetry data structure reported by each ESC. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `motorCount` | `uint8_t` | 1 | Number of motors reporting telemetry (`getMotorCount()`) | + | `escData` | `escSensorData_t[]` | `motorCount * sizeof(escSensorData_t)` | Array of `escSensorData_t` structures containing voltage, current, temp, RPM, errors etc. for each ESC | +* **Notes:** Requires `USE_ESC_SENSOR`. See `escSensorData_t` in `sensors/esc_sensor.h` for the exact structure fields. + +### `MSP2_INAV_LED_STRIP_CONFIG_EX` (0x2048 / 8264) + +* **Direction:** Out +* **Description:** Retrieves the full configuration for each LED on the strip using the `ledConfig_t` structure. Supersedes `MSP_LED_STRIP_CONFIG`. +* **Payload:** Repeated `LED_MAX_STRIP_LENGTH` times: + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `ledConfig` | `uint16_t` | 6 | Full configuration structure for the LED, size sizeof(ledConfig_t) | +* **Notes:** Requires `USE_LED_STRIP`. See `ledConfig_t` in `io/ledstrip.h` for structure fields (position, function, overlay, color, direction, params). + +### `MSP2_INAV_SET_LED_STRIP_CONFIG_EX` (0x2049 / 8265) + +* **Direction:** In +* **Description:** Sets the configuration for a single LED on the strip using the `ledConfig_t` structure. Supersedes `MSP_SET_LED_STRIP_CONFIG`. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `ledIndex` | `uint8_t` | 1 | Index of the LED to configure (0 to `LED_MAX_STRIP_LENGTH - 1`) | + | `ledConfig` |`uint16_t` | 6 Full configuration structure for the LED , size sizeof(ledConfig_t) | +* **Notes:** Requires `USE_LED_STRIP`. Expects `1 + sizeof(ledConfig_t)` bytes. Returns error if index invalid. Calls `reevaluateLedConfig()`. + +### `MSP2_INAV_FW_APPROACH` (0x204A / 8266) + +* **Direction:** In/Out +* **Description:** Get or Set configuration for a specific Fixed Wing Autoland approach. +* **Request Payload (Get):** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `approachIndex` | `uint8_t` | 1 | Index of the approach setting (0 to `MAX_FW_LAND_APPOACH_SETTINGS - 1`) | +* **Reply Payload (Get):** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `approachIndex` | `uint8_t` | 1 | Index | Index requested | + | `approachAlt` | `uint32_t` | 4 | cm | Altitude for the approach phase | + | `landAlt` | `uint32_t` | 4 | cm | Altitude for the final landing phase | + | `approachDirection` | `uint8_t` | 1 | Enum | Enum `fwAutolandApproachDirection_e`: Direction of approach (From WP, Specific Heading) | + | `landHeading1` | `int16_t` | 2 | degrees | Primary landing heading (if approachDirection requires it) | + | `landHeading2` | `int16_t` | 2 | degrees | Secondary landing heading (if approachDirection requires it) | + | `isSeaLevelRef` | `uint8_t` | 1 | Boolean | 1 if altitudes are relative to sea level, 0 if relative to home | +* **Notes:** Requires `USE_FW_AUTOLAND`. Used by `mspFwApproachOutCommand`. See `MSP2_INAV_SET_FW_APPROACH` for setting. + +### `MSP2_INAV_SET_FW_APPROACH` (0x204B / 8267) + +* **Direction:** In +* **Description:** Sets the configuration for a specific Fixed Wing Autoland approach. +* **Payload:** (Matches `MSP2_INAV_FW_APPROACH` reply structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `approachIndex` | `uint8_t` | 1 | Index | Index of the approach setting (0 to `MAX_FW_LAND_APPOACH_SETTINGS - 1`) | + | `approachAlt` | `uint32_t` | 4 | cm | Sets approach altitude | + | `landAlt` | `uint32_t` | 4 | cm | Sets landing altitude | + | `approachDirection` | `uint8_t` | 1 | Enum | Enum `fwAutolandApproachDirection_e` Sets approach direction | + | `landHeading1` | `int16_t` | 2 | degrees | Sets primary landing heading | + | `landHeading2` | `int16_t` | 2 | degrees | Sets secondary landing heading | + | `isSeaLevelRef` | `uint8_t` | 1 | Boolean | Sets altitude reference | +* **Notes:** Requires `USE_FW_AUTOLAND`. Expects 15 bytes. Returns error if index invalid. + +### `MSP2_INAV_GPS_UBLOX_COMMAND` (0x2050 / 8272) + +* **Direction:** In +* **Description:** Sends a raw command directly to a U-Blox GPS module connected to the FC. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `ubxCommand` | `uint8_t[]` | Variable (>= 8) | Raw U-Blox UBX protocol command frame (including header, class, ID, length, payload, checksum) | +* **Notes:** Requires GPS feature enabled (`FEATURE_GPS`) and the GPS driver to be U-Blox (`isGpsUblox()`). Payload must be at least 8 bytes (minimum UBX frame size). Use with extreme caution, incorrect commands can misconfigure the GPS module. Calls `gpsUbloxSendCommand()`. + +### `MSP2_INAV_RATE_DYNAMICS` (0x2060 / 8288) + +* **Direction:** Out +* **Description:** Retrieves Rate Dynamics configuration parameters for the current control rate profile. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `sensitivityCenter` | `uint8_t` | 1 | % | Sensitivity at stick center (`currentControlRateProfile->rateDynamics.sensitivityCenter`) | + | `sensitivityEnd` | `uint8_t` | 1 | % | Sensitivity at stick ends (`currentControlRateProfile->rateDynamics.sensitivityEnd`) | + | `correctionCenter` | `uint8_t` | 1 | % | Correction strength at stick center (`currentControlRateProfile->rateDynamics.correctionCenter`) | + | `correctionEnd` | `uint8_t` | 1 | % | Correction strength at stick ends (`currentControlRateProfile->rateDynamics.correctionEnd`) | + | `weightCenter` | `uint8_t` | 1 | % | Transition weight at stick center (`currentControlRateProfile->rateDynamics.weightCenter`) | + | `weightEnd` | `uint8_t` | 1 | % | Transition weight at stick ends (`currentControlRateProfile->rateDynamics.weightEnd`) | +* **Notes:** Requires `USE_RATE_DYNAMICS`. + +### `MSP2_INAV_SET_RATE_DYNAMICS` (0x2061 / 8289) + +* **Direction:** In +* **Description:** Sets Rate Dynamics configuration parameters for the current control rate profile. +* **Payload:** (Matches `MSP2_INAV_RATE_DYNAMICS` structure) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `sensitivityCenter` | `uint8_t` | 1 | % | Sets sensitivity at center | + | `sensitivityEnd` | `uint8_t` | 1 | % | Sets sensitivity at ends | + | `correctionCenter` | `uint8_t` | 1 | % | Sets correction at center | + | `correctionEnd` | `uint8_t` | 1 | % | Sets correction at ends | + | `weightCenter` | `uint8_t` | 1 | % | Sets weight at center | + | `weightEnd` | `uint8_t` | 1 | % | Sets weight at ends | +* **Notes:** Requires `USE_RATE_DYNAMICS`. Expects 6 bytes. + +### `MSP2_INAV_EZ_TUNE` (0x2070 / 8304) + +* **Direction:** Out +* **Description:** Retrieves the current EZ-Tune parameters. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `enabled` | `uint8_t` | 1 | Boolean: 1 if EZ-Tune is enabled (`ezTune()->enabled`) | + | `filterHz` | `uint16_t` | 2 | Filter frequency used during tuning (`ezTune()->filterHz`) | + | `axisRatio` | `uint8_t` | 1 | Roll vs Pitch axis tuning ratio (`ezTune()->axisRatio`) | + | `response` | `uint8_t` | 1 | Desired response characteristic (`ezTune()->response`) | + | `damping` | `uint8_t` | 1 | Desired damping characteristic (`ezTune()->damping`) | + | `stability` | `uint8_t` | 1 | Stability preference (`ezTune()->stability`) | + | `aggressiveness` | `uint8_t` | 1 | Aggressiveness preference (`ezTune()->aggressiveness`) | + | `rate` | `uint8_t` | 1 | Resulting rate setting (`ezTune()->rate`) | + | `expo` | `uint8_t` | 1 | Resulting expo setting (`ezTune()->expo`) | + | `snappiness` | `uint8_t` | 1 | Snappiness preference (`ezTune()->snappiness`) | +* **Notes:** Requires `USE_EZ_TUNE`. Calls `ezTuneUpdate()` before sending. + +### `MSP2_INAV_EZ_TUNE_SET` (0x2071 / 8305) + +* **Direction:** In +* **Description:** Sets the EZ-Tune parameters and triggers an update. +* **Payload:** (Matches `MSP2_INAV_EZ_TUNE` structure, potentially omitting `snappiness`) + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `enabled` | `uint8_t` | 1 | Sets enabled state | + | `filterHz` | `uint16_t` | 2 | Sets filter frequency | + | `axisRatio` | `uint8_t` | 1 | Sets axis ratio | + | `response` | `uint8_t` | 1 | Sets response characteristic | + | `damping` | `uint8_t` | 1 | Sets damping characteristic | + | `stability` | `uint8_t` | 1 | Sets stability preference | + | `aggressiveness` | `uint8_t` | 1 | Sets aggressiveness preference | + | `rate` | `uint8_t` | 1 | Sets rate setting | + | `expo` | `uint8_t` | 1 | Sets expo setting | + | `snappiness` | `uint8_t` | 1 | (Optional) Sets snappiness preference | +* **Notes:** Requires `USE_EZ_TUNE`. Expects 10 or 11 bytes. Calls `ezTuneUpdate()` after setting parameters. + +### `MSP2_INAV_SELECT_MIXER_PROFILE` (0x2080 / 8320) + +* **Direction:** In +* **Description:** Selects the active mixer profile and saves configuration. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `mixerProfileIndex` | `uint8_t` | 1 | Index of the mixer profile to activate (0-based) | +* **Notes:** Expects 1 byte. Will fail if armed. Calls `setConfigMixerProfileAndWriteEEPROM()`. Only applicable if `MAX_MIXER_PROFILE_COUNT` > 1. + +### `MSP2_ADSB_VEHICLE_LIST` (0x2090 / 8336) + +* **Direction:** Out +* **Description:** Retrieves the list of currently tracked ADSB (Automatic Dependent Surveillance–Broadcast) vehicles. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `maxVehicles` | `uint8_t` | 1 | Maximum number of vehicles tracked (`MAX_ADSB_VEHICLES`). 0 if `USE_ADSB` disabled | + | `callsignLength` | `uint8_t` | 1 | Maximum length of callsign string (`ADSB_CALL_SIGN_MAX_LENGTH`). 0 if `USE_ADSB` disabled | + | `totalVehicleMsgs` | `uint32_t` | 4 | Total vehicle messages received (`getAdsbStatus()->vehiclesMessagesTotal`). 0 if `USE_ADSB` disabled | + | `totalHeartbeatMsgs` | `uint32_t` | 4 | Total heartbeat messages received (`getAdsbStatus()->heartbeatMessagesTotal`). 0 if `USE_ADSB` disabled | + | **Vehicle Data (Repeated `maxVehicles` times):** | | | | + | `callsign` | `char[ADSB_CALL_SIGN_MAX_LENGTH]` | `ADSB_CALL_SIGN_MAX_LENGTH` | Vehicle callsign (padded with nulls) | + | `icao` | `uint32_t` | 4 | ICAO 24-bit address | + | `latitude` | `int32_t` | 4 | Latitude (1e7 deg) | + | `longitude` | `int32_t` | 4 | Longitude (1e7 deg) | + | `altitude` | `int32_t` | 4 | Altitude (cm) | + | `heading` | `int16_t` | 2 | Heading (degrees) | + | `tslc` | `uint8_t` | 1 | Time Since Last Communication (seconds) | + | `emitterType` | `uint8_t` | 1 | Enum: Type of ADSB emitter | + | `ttl` | `uint8_t` | 1 | Time-to-live counter for this entry | +* **Notes:** Requires `USE_ADSB`. + +### `MSP2_INAV_CUSTOM_OSD_ELEMENTS` (0x2100 / 8448) + +* **Direction:** Out +* **Description:** Retrieves counts related to custom OSD elements defined by the programming framework. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `maxElements` | `uint8_t` | 1 | Maximum number of custom elements (`MAX_CUSTOM_ELEMENTS`) | + | `maxTextLength` | `uint8_t` | 1 | Maximum length of the text part (`OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1`) | + | `maxParts` | `uint8_t` | 1 | Maximum number of parts per element (`CUSTOM_ELEMENTS_PARTS`) | +* **Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. + +### `MSP2_INAV_CUSTOM_OSD_ELEMENT` (0x2101 / 8449) + +* **Direction:** In/Out +* **Description:** Gets the configuration of a single custom OSD element defined by the programming framework. +* **Request Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `elementIndex` | `uint8_t` | 1 | Index of the custom element (0 to `MAX_CUSTOM_ELEMENTS - 1`) | +* **Reply Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | **Parts Data (Repeated `CUSTOM_ELEMENTS_PARTS` times):** | | | | + | `partType` | `uint8_t` | 1 | Enum (`customElementType_e`): Type of this part (Text, Variable, Symbol) | + | `partValue` | `uint16_t` | 2 | Value/ID associated with this part (GVAR index, Symbol ID, etc.) | + | **Visibility Data:** | | | | + | `visibilityType` | `uint8_t` | 1 | Enum (`logicOperandType_e`): Type of visibility condition source | + | `visibilityValue` | `uint16_t` | 2 | Value/ID of the visibility condition source (e.g., Logic Condition ID) | + | **Text Data:** | | | | + | `elementText` | `char[OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1]` | `OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1` | Static text part of the element (null padding likely) | +* **Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. See `osdCustomElement_t`. + +### `MSP2_INAV_SET_CUSTOM_OSD_ELEMENTS` (0x2102 / 8450) + +* **Direction:** In +* **Description:** Sets the configuration of a single custom OSD element defined by the programming framework. +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `elementIndex` | `uint8_t` | 1 | Index of the custom element (0 to `MAX_CUSTOM_ELEMENTS - 1`) | + | **Parts Data (Repeated `CUSTOM_ELEMENTS_PARTS` times):** | | | | + | `partType` | `uint8_t` | 1 | Enum (`customElementType_e`): Type of this part | + | `partValue` | `uint16_t` | 2 | Value/ID associated with this part | + | **Visibility Data:** | | | | + | `visibilityType` | `uint8_t` | 1 | Enum (`logicOperandType_e`): Type of visibility condition source | + | `visibilityValue` | `uint16_t` | 2 | Value/ID of the visibility condition source | + | **Text Data:** | | | | + | `elementText` | `char[OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1]` | `OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1` | Static text part of the element | +* **Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. Expects `1 + (CUSTOM_ELEMENTS_PARTS * 3) + 3 + (OSD_CUSTOM_ELEMENT_TEXT_SIZE - 1)` bytes. Returns error if index or part type is invalid. Null-terminates the text internally. + +### `MSP2_INAV_SERVO_CONFIG` (0x2200 / 8704) + +* **Direction:** Out +* **Description:** Retrieves the configuration parameters for all supported servos (min, max, middle, rate). Supersedes `MSP_SERVO_CONFIGURATIONS`. +* **Payload:** Repeated `MAX_SUPPORTED_SERVOS` times: + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `min` | `uint16_t` | 2 | PWM | Minimum servo endpoint (`servoParams(i)->min`) | + | `max` | `uint16_t` | 2 | PWM | Maximum servo endpoint (`servoParams(i)->max`) | + | `middle` | `uint16_t` | 2 | PWM | Middle/Neutral servo position (`servoParams(i)->middle`) | + | `rate` | `uint8_t` | 1 | % (-100 to 100) | Servo rate/scaling (`servoParams(i)->rate`) | + +### `MSP2_INAV_SET_SERVO_CONFIG` (0x2201 / 8705) + +* **Direction:** In +* **Description:** Sets the configuration parameters for a single servo. Supersedes `MSP_SET_SERVO_CONFIGURATION`. +* **Payload:** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `servoIndex` | `uint8_t` | 1 | Index | Index of the servo to configure (0 to `MAX_SUPPORTED_SERVOS - 1`) | + | `min` | `uint16_t` | 2 | PWM | Sets minimum servo endpoint | + | `max` | `uint16_t` | 2 | PWM | Sets maximum servo endpoint | + | `middle` | `uint16_t` | 2 | PWM | Sets middle/neutral servo position | + | `rate` | `uint8_t` | 1 | % | Sets servo rate/scaling | +* **Notes:** Expects 8 bytes. Returns error if index invalid. Calls `servoComputeScalingFactors()`. + +### `MSP2_INAV_GEOZONE` (0x2210 / 8720) + +* **Direction:** In/Out +* **Description:** Get configuration for a specific Geozone. +* **Request Payload (Get):** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `geozoneIndex` | `uint8_t` | 1 | Index of the geozone (0 to `MAX_GEOZONES_IN_CONFIG - 1`) | +* **Reply Payload (Get):** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `geozoneIndex` | `uint8_t` | 1 | Index requested | + | `type` | `uint8_t` | 1 | Define (`GEOZONE_TYPE_EXCLUSIVE/INCLUSIVE`): Zone type (Inclusion/Exclusion) | + | `shape` | `uint8_t` | 1 | Define (`GEOZONE_SHAPE_CIRCULAR/POLYGHON`): Zone shape (Polygon/Circular) | + | `minAltitude` | `uint32_t` | 4 | Minimum allowed altitude within the zone (cm) | + | `maxAltitude` | `uint32_t` | 4 | Maximum allowed altitude within the zone (cm) | + | `isSeaLevelRef` | `uint8_t` | 1 | Boolean: 1 if altitudes are relative to sea level, 0 if relative to home | + | `fenceAction` | `uint8_t` | 1 | Enum (`geozoneActionState_e`): Action to take upon boundary violation | + | `vertexCount` | `uint8_t` | 1 | Number of vertices defined for this zone | +* **Notes:** Requires `USE_GEOZONE`. Used by `mspFcGeozoneOutCommand`. + +### `MSP2_INAV_SET_GEOZONE` (0x2211 / 8721) + +* **Direction:** In +* **Description:** Sets the main configuration for a specific Geozone (type, shape, altitude, action). **This command resets (clears) all vertices associated with the zone.** +* **Payload:** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `geozoneIndex` | `uint8_t` | 1 | Index of the geozone (0 to `MAX_GEOZONES_IN_CONFIG - 1`) | + | `type` | `uint8_t` | 1 | Define (`GEOZONE_TYPE_EXCLUSIVE/INCLUSIVE`): Zone type (Inclusion/Exclusion) | + | `shape` | `uint8_t` | 1 | Define (`GEOZONE_SHAPE_CIRCULAR/POLYGHON`): Zone shape (Polygon/Circular) | + | `minAltitude` | `uint32_t` | 4 | Minimum allowed altitude (cm) | + | `maxAltitude` | `uint32_t` | 4 | Maximum allowed altitude (cm) | + | `isSeaLevelRef` | `uint8_t` | 1 | Boolean: Altitude reference | + | `fenceAction` | `uint8_t` | 1 | Enum (`geozoneActionState_e`): Action to take upon boundary violation | + | `vertexCount` | `uint8_t` | 1 | Number of vertices to be defined (used for validation later) | +* **Notes:** Requires `USE_GEOZONE`. Expects 14 bytes. Returns error if index invalid. Calls `geozoneResetVertices()`. Vertices must be set subsequently using `MSP2_INAV_SET_GEOZONE_VERTEX`. + +### `MSP2_INAV_GEOZONE_VERTEX` (0x2212 / 8722) + +* **Direction:** In/Out +* **Description:** Get a specific vertex (or center+radius for circular zones) of a Geozone. +* **Request Payload (Get):** + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `geozoneIndex` | `uint8_t` | 1 | Index of the geozone | + | `vertexId` | `uint8_t` | 1 | Index of the vertex within the zone (0-based). For circles, 0 = center | +* **Reply Payload (Get - Polygon):** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `geozoneIndex` | `uint8_t` | 1 | Index | Geozone index requested | + | `vertexId` | `uint8_t` | 1 | Index | Vertex index requested | + | `latitude` | `int32_t` | 4 | deg * 1e7 | Vertex latitude | + | `longitude` | `int32_t` | 4 | deg * 1e7 | Vertex longitude | +* **Reply Payload (Get - Circular):** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `geozoneIndex` | `uint8_t` | 1 | Index | Geozone index requested | + | `vertexId` | `uint8_t` | 1 | Index | Vertex index requested (always 0 for center) | + | `centerLatitude` | `int32_t` | 4 | deg * 1e7 | Center latitude | + | `centerLongitude` | `int32_t` | 4 | deg * 1e7 | Center longitude | + | `radius` | `uint32_t` | 4 | cm | Radius of the circular zone | +* **Notes:** Requires `USE_GEOZONE`. Returns error if indexes are invalid or vertex doesn't exist. For circular zones, the radius is stored internally as the 'latitude' of the vertex with index 1. + +### `MSP2_INAV_SET_GEOZONE_VERTEX` (0x2213 / 8723) + +* **Direction:** In +* **Description:** Sets a specific vertex (or center+radius for circular zones) for a Geozone. +* **Payload (Polygon):** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `geozoneIndex` | `uint8_t` | 1 | Index | Geozone index | + | `vertexId` | `uint8_t` | 1 | Index | Vertex index (0-based) | + | `latitude` | `int32_t` | 4 | deg * 1e7 | Vertex latitude | + | `longitude` | `int32_t` | 4 | deg * 1e7 | Vertex longitude | +* **Payload (Circular):** + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `geozoneIndex` | `uint8_t` | 1 | Index | Geozone index | + | `vertexId` | `uint8_t` | 1 | Index | Vertex index (must be 0 for center) | + | `centerLatitude` | `int32_t` | 4 | deg * 1e7 | Center latitude | + | `centerLongitude` | `int32_t` | 4 | deg * 1e7 | Center longitude | + | `radius` | `uint32_t` | 4 | cm | Radius of the circular zone | +* **Notes:** Requires `USE_GEOZONE`. Expects 10 bytes (Polygon) or 14 bytes (Circular). Returns error if indexes invalid or if trying to set vertex beyond `vertexCount` defined in `MSP2_INAV_SET_GEOZONE`. Calls `geozoneSetVertex()`. For circular zones, sets center (vertex 0) and radius (vertex 1's latitude). + +--- + +## MSPv2 Betaflight Commands (0x3000 Range) + +### `MSP2_BETAFLIGHT_BIND` (0x3000 / 12288) + +* **Direction:** In +* **Description:** Initiates the receiver binding procedure for supported serial protocols (CRSF, SRXL2). +* **Payload:** None +* **Notes:** Requires `rxConfig()->receiverType == RX_TYPE_SERIAL`. Requires `USE_SERIALRX_CRSF` or `USE_SERIALRX_SRXL2`. Calls `crsfBind()` or `srxl2Bind()` respectively. Returns error if receiver type or provider is not supported for binding. + +--- + +## MSPv2 Sensor Input Commands (0x1F00 Range) + +These commands are typically sent *to* the FC from external sensor modules connected via a serial port configured for MSP sensor input. They usually expect **no reply** (`MSP_RESULT_NO_REPLY`). + +### `MSP2_SENSOR_RANGEFINDER` (0x1F01 / 7937) + +* **Direction:** In +* **Description:** Provides rangefinder data (distance, quality) from an external MSP-based sensor. +* **Payload:** (`mspSensorRangefinderDataMessage_t`) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `quality` | `uint8_t` | 1 | 0-255 | Quality of the measurement | + | `distanceMm` | `int32_t` | 4 | mm | Measured distance. Negative value indicates out of range | +* **Notes:** Requires `USE_RANGEFINDER_MSP`. Calls `mspRangefinderReceiveNewData()`. + +### `MSP2_SENSOR_OPTIC_FLOW` (0x1F02 / 7938) + +* **Direction:** In +* **Description:** Provides optical flow data (motion, quality) from an external MSP-based sensor. +* **Payload:** (`mspSensorOpflowDataMessage_t`) + | Field | C Type | Size (Bytes) | Description | + |---|---|---|---| + | `quality` | `uint8_t` | 1 | Quality of the measurement (0-255) | + | `motionX` | `int32_t` | 4 | Raw integrated flow value X | + | `motionY` | `int32_t` | 4 | Raw integrated flow value Y | +* **Notes:** Requires `USE_OPFLOW_MSP`. Calls `mspOpflowReceiveNewData()`. + +### `MSP2_SENSOR_GPS` (0x1F03 / 7939) + +* **Direction:** In +* **Description:** Provides detailed GPS data from an external MSP-based GPS module. +* **Payload:** (`mspSensorGpsDataMessage_t`) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `instance` | `uint8_t` | 1 | - | Sensor instance number (for multi-GPS) | + | `gpsWeek` | `uint16_t` | 2 | - | GPS week number (0xFFFF if unavailable) | + | `msTOW` | `uint32_t` | 4 | ms | Milliseconds Time of Week | + | `fixType` | `uint8_t` | 1 | Enum | Enum `gpsFixType_e` Type of GPS fix | + | `satellitesInView`| `uint8_t` | 1 | Count | Number of satellites used in solution | + | `hPosAccuracy` | `uint16_t` | 2 | cm | Horizontal position accuracy estimate | + | `vPosAccuracy` | `uint16_t` | 2 | cm | Vertical position accuracy estimate | + | `hVelAccuracy` | `uint16_t` | 2 | cm/s | Horizontal velocity accuracy estimate | + | `hdop` | `uint16_t` | 2 | HDOP * 100 | Horizontal Dilution of Precision | + | `longitude` | `int32_t` | 4 | deg * 1e7 | Longitude | + | `latitude` | `int32_t` | 4 | deg * 1e7 | Latitude | + | `mslAltitude` | `int32_t` | 4 | cm | Altitude above Mean Sea Level | + | `nedVelNorth` | `int32_t` | 4 | cm/s | North velocity (NED frame) | + | `nedVelEast` | `int32_t` | 4 | cm/s | East velocity (NED frame) | + | `nedVelDown` | `int32_t` | 4 | cm/s | Down velocity (NED frame) | + | `groundCourse` | `uint16_t` | 2 | deg * 100 | Ground course (0-36000) | + | `trueYaw` | `uint16_t` | 2 | deg * 100 | True heading/yaw (0-36000, 65535 if unavailable) | + | `year` | `uint16_t` | 2 | - | Year (e.g., 2023) | + | `month` | `uint8_t` | 1 | - | Month (1-12) | + | `day` | `uint8_t` | 1 | - | Day of month (1-31) | + | `hour` | `uint8_t` | 1 | - | Hour (0-23) | + | `min` | `uint8_t` | 1 | - | Minute (0-59) | + | `sec` | `uint8_t` | 1 | - | Second (0-59) | +* **Notes:** Requires `USE_GPS_PROTO_MSP`. Calls `mspGPSReceiveNewData()`. + +### `MSP2_SENSOR_COMPASS` (0x1F04 / 7940) + +* **Direction:** In +* **Description:** Provides magnetometer data from an external MSP-based compass module. +* **Payload:** (`mspSensorCompassDataMessage_t`) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `instance` | `uint8_t` | 1 | - | Sensor instance number | + | `timeMs` | `uint32_t` | 4 | ms | Timestamp from the sensor | + | `magX` | `int16_t` | 2 | mGauss | Front component reading | + | `magY` | `int16_t` | 2 | mGauss | Right component reading | + | `magZ` | `int16_t` | 2 | mGauss | Down component reading | +* **Notes:** Requires `USE_MAG_MSP`. Calls `mspMagReceiveNewData()`. + +### `MSP2_SENSOR_BAROMETER` (0x1F05 / 7941) + +* **Direction:** In +* **Description:** Provides barometer data from an external MSP-based barometer module. +* **Payload:** (`mspSensorBaroDataMessage_t`) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `instance` | `uint8_t` | 1 | - | Sensor instance number | + | `timeMs` | `uint32_t` | 4 | ms | Timestamp from the sensor | + | `pressurePa` | `float` | 4 | Pa | Absolute pressure | + | `temp` | `int16_t` | 2 | 0.01 deg C | Temperature | +* **Notes:** Requires `USE_BARO_MSP`. Calls `mspBaroReceiveNewData()`. + +### `MSP2_SENSOR_AIRSPEED` (0x1F06 / 7942) + +* **Direction:** In +* **Description:** Provides airspeed data from an external MSP-based pitot sensor module. +* **Payload:** (`mspSensorAirspeedDataMessage_t`) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `instance` | `uint8_t` | 1 | - | Sensor instance number | + | `timeMs` | `uint32_t` | 4 | ms | Timestamp from the sensor | + | `diffPressurePa`| `float` | 4 | Pa | Differential pressure | + | `temp` | `int16_t` | 2 | 0.01 deg C | Temperature | +* **Notes:** Requires `USE_PITOT_MSP`. Calls `mspPitotmeterReceiveNewData()`. + +### `MSP2_SENSOR_HEADTRACKER` (0x1F07 / 7943) + +* **Direction:** In +* **Description:** Provides head tracker orientation data. +* **Payload:** (Structure not defined in provided headers, but likely Roll, Pitch, Yaw angles) + | Field | C Type | Size (Bytes) | Units | Description | + |---|---|---|---|---| + | `...` | Varies | Variable | Head tracker angles (e.g., int16 Roll, Pitch, Yaw in deci-degrees) | +* **Notes:** Requires `USE_HEADTRACKER` and `USE_HEADTRACKER_MSP`. Calls `mspHeadTrackerReceiverNewData()`. Payload structure needs verification from `mspHeadTrackerReceiverNewData` implementation. diff --git a/docs/development/msp/rev b/docs/development/msp/rev new file mode 100644 index 00000000000..00750edc07d --- /dev/null +++ b/docs/development/msp/rev @@ -0,0 +1 @@ +3 diff --git a/docs/development/release-create.md b/docs/development/release-create.md new file mode 100644 index 00000000000..ec362da0911 --- /dev/null +++ b/docs/development/release-create.md @@ -0,0 +1,345 @@ +# Creating INAV Releases + +This document describes the process for creating INAV firmware and configurator releases. + +> **Note:** This document is designed to be used with coding assistants (such as Claude Code) that can execute the commands and automate parts of the release process. Update this document with lessons learned after each release. + +## Overview + +INAV releases include both firmware (for flight controllers) and the configurator application (for configuration). Both repositories must be tagged with matching version numbers. + +**Repositories:** +- Firmware: https://github.com/iNavFlight/inav +- Configurator: https://github.com/iNavFlight/inav-configurator + +## Version Numbering + +INAV uses semantic versioning: `MAJOR.MINOR.PATCH` + +- **MAJOR:** Breaking changes, major new features +- **MINOR:** New features, significant improvements +- **PATCH:** Bug fixes, minor improvements + +Version numbers are set in: +- Firmware: in `CMakeLists.txt` via `project(INAV VERSION X.Y.Z)` + Verify/update: + - View: `grep -E 'project\\(INAV VERSION' CMakeLists.txt` + - Update: edit `CMakeLists.txt` to set the desired version +- Configurator: in `package.json` field `"version"` + Verify/update: + - View: `jq -r .version package.json` (or `node -p "require('./package.json').version"`) + - Update: `npm version --no-git-tag-version` + +## Pre-Release Checklist + +### Code Readiness + +- [ ] All planned PRs merged +- [ ] CI passing on master branch +- [ ] No critical open issues blocking release +- [ ] Version numbers updated in both repositories +- [ ] SITL binaries updated in configurator + +### Documentation + +- [ ] Release notes drafted +- [ ] Breaking changes documented +- [ ] New features documented + +## Release Workflow + +``` +1. Verify release readiness + ├── All PRs merged + ├── CI passing + └── Version numbers updated + +2. Update SITL binaries in Configurator + ├── Download from nightly or build for each platform + └── Commit updated binaries to configurator repo + +3. Create tags + ├── inav: git tag + └── inav-configurator: git tag + +4. Generate changelog + ├── List PRs since last tag + ├── Categorize changes + └── Format release notes + +5. Download/build artifacts + ├── Firmware: from nightly builds + └── Configurator: from CI artifacts + +6. Create draft releases + ├── Upload firmware artifacts + ├── Upload configurator artifacts + └── Add release notes + +7. Review and publish + ├── Maintainer review + └── Publish releases +``` + +## Updating SITL Binaries + +SITL binaries must be updated before tagging the configurator. They are stored in: +``` +inav-configurator/resources/public/sitl/ +├── linux/ +│ ├── inav_SITL +│ └── arm64/inav_SITL +├── macos/ +│ └── inav_SITL +└── windows/ + ├── inav_SITL.exe + └── cygwin1.dll +``` + +### Download from Nightly + +```bash +# Find matching nightly release +gh release list --repo iNavFlight/inav-nightly --limit 5 + +# Download SITL resources +curl -L -o /tmp/sitl-resources.zip \ + "https://github.com/iNavFlight/inav-nightly/releases/download//sitl-resources.zip" +unzip /tmp/sitl-resources.zip -d /tmp/sitl-extract + +# Copy to configurator +cd inav-configurator +cp /tmp/sitl-extract/resources/sitl/linux/inav_SITL resources/public/sitl/linux/ +cp /tmp/sitl-extract/resources/sitl/linux/arm64/inav_SITL resources/public/sitl/linux/arm64/ +cp /tmp/sitl-extract/resources/sitl/macos/inav_SITL resources/public/sitl/macos/ +cp /tmp/sitl-extract/resources/sitl/windows/inav_SITL.exe resources/public/sitl/windows/ + +# Commit +git add resources/public/sitl/ +git commit -m "Update SITL binaries for " +``` + +## Tagging + +### Check Latest Tags + +```bash +# Firmware +cd inav +git fetch --tags +git tag --sort=-v:refname | head -10 + +# Configurator +cd inav-configurator +git fetch --tags +git tag --sort=-v:refname | head -10 +``` + +### Create New Tags + +```bash +# Firmware +cd inav +git checkout master && git pull +git tag -a -m "INAV " +git push origin + +# Configurator +cd inav-configurator +git checkout master && git pull +git tag -a -m "INAV Configurator " +git push origin +``` + +## Changelog Generation + +### List PRs Since Last Tag + +```bash +cd inav +LAST_TAG=$(git describe --tags --abbrev=0) +gh pr list --state merged --search "merged:>=$(git log -1 --format=%ai $LAST_TAG | cut -d' ' -f1)" --limit 100 +``` + +### Using git log + +```bash +LAST_TAG=$(git describe --tags --abbrev=0) +git log $LAST_TAG..HEAD --oneline --merges +``` + +### Changelog Format + +```markdown +## INAV Release Notes + +### Firmware Changes + +#### New Features +- PR #1234: Description (@contributor) + +#### Bug Fixes +- PR #1236: Description (@contributor) + +#### Improvements +- PR #1237: Description (@contributor) + +### Configurator Changes + +#### New Features +- PR #100: Description (@contributor) + +### Full Changelog +**Firmware:** https://github.com/iNavFlight/inav/compare/... +**Configurator:** https://github.com/iNavFlight/inav-configurator/compare/... +``` + +## Downloading Release Artifacts + +### Firmware Hex Files + +Firmware is available from the nightly build system: + +```bash +# List recent nightlies +gh release list --repo iNavFlight/inav-nightly --limit 5 + +# Download hex files +gh release download --repo iNavFlight/inav-nightly --pattern "*.hex" +``` + +#### Renaming Firmware Files + +Remove CI suffix and add RC number for RC releases: + +```bash +RC_NUM="RC2" # Empty for final releases + +# Check if any .hex files exist to avoid errors with the glob +if compgen -G "*.hex" > /dev/null; then + for f in *.hex; do + target=$(echo "$f" | sed -E 's/inav_[0-9]+\.[0-9]+\.[0-9]+_(.*)_ci-.*/\1/') + version=$(echo "$f" | sed -E 's/inav_([0-9]+\.[0-9]+\.[0-9]+)_.*/\1/') + if [ -n "$RC_NUM" ]; then + mv "$f" "inav_${version}_${RC_NUM}_${target}.hex" + else + mv "$f" "inav_${version}_${target}.hex" + fi + done +else + echo "No .hex files found to rename." +fi +``` + +### Configurator Builds + +Download from GitHub Actions CI: + +```bash +# List recent workflow runs +gh run list --repo iNavFlight/inav-configurator --limit 10 + +# Download artifacts +gh run download --repo iNavFlight/inav-configurator + +# Flatten directory structure +find . -mindepth 2 -type f -exec mv -t . {} + +# Remove the now-empty subdirectories +find . -mindepth 1 -type d -empty -delete +``` + +## Creating GitHub Releases + +### Create Draft Release + +```bash +# Firmware +cd inav +gh release create --draft --title "INAV " --notes-file release-notes.md +gh release upload *.hex + +# Configurator +cd inav-configurator +gh release create --draft --title "INAV Configurator " --notes-file release-notes.md +gh release upload *.zip *.dmg *.exe *.AppImage *.deb *.rpm *.msi +``` + +### Managing Release Assets + +#### Rename Assets via API + +```bash +# Get release and asset IDs +gh api repos/iNavFlight/inav/releases --jq '.[] | select(.draft == true) | {id: .id, name: .name}' +gh api repos/iNavFlight/inav/releases/RELEASE_ID/assets --paginate --jq '.[] | "\(.id) \(.name)"' + +# Rename an asset +gh api -X PATCH "repos/iNavFlight/inav/releases/assets/ASSET_ID" -f name="new-filename.hex" +``` + +#### Delete Outdated Assets from Draft Release + +If a draft release has outdated assets that need to be replaced (e.g., from a previous upload attempt), delete them before uploading new ones: + +```bash +gh api -X DELETE "repos/iNavFlight/inav/releases/assets/ASSET_ID" +``` + +### Publish Release + +```bash +gh release edit --draft=false +``` + +## Asset Naming Conventions + +**Firmware (RC releases):** `inav__RC_.hex` +**Firmware (final):** `inav__.hex` + +**Configurator (RC releases):** `INAV-Configurator___RC.` +**Configurator (final):** `INAV-Configurator__.` + +## Maintenance Branches + +When releasing a new major version, create maintenance branches: + +- **maintenance-X.x** - For bugfixes to version X +- **maintenance-(X+1).x** - For breaking changes targeting the next major version + +### Creating Maintenance Branches + +```bash +COMMIT_SHA="" + +# inav +gh api repos/iNavFlight/inav/git/refs -f ref="refs/heads/maintenance-9.x" -f sha="$COMMIT_SHA" + +# inav-configurator +gh api repos/iNavFlight/inav-configurator/git/refs -f ref="refs/heads/maintenance-9.x" -f sha="$COMMIT_SHA" +``` + +### Branch Usage + +- **X.x bugfixes** → PR to maintenance-X.x +- **Breaking changes** → PR to maintenance-(X+1).x +- **Non-breaking features** → PR to master + +Lower version branches are periodically merged into higher version branches (e.g., maintenance-9.x → maintenance-10.x → master). + +## Hotfix Releases + +For critical bugs discovered after release: + +1. Create hotfix branch from release tag +2. Cherry-pick or create fix +3. Tag as `X.Y.Z+1` (patch increment) +4. Build and release following normal process +5. Document as hotfix in release notes + +## Post-Release Tasks + +- [ ] Announce release (Discord, forums, etc.) +- [ ] Update any pinned issues +- [ ] Monitor for critical bug reports +- [ ] Prepare hotfix if needed +- [ ] Update this document with any lessons learned diff --git a/docs/development/serial_printf_debugging.md b/docs/development/serial_printf_debugging.md index 53711fe4e34..e700e2cf1b4 100644 --- a/docs/development/serial_printf_debugging.md +++ b/docs/development/serial_printf_debugging.md @@ -147,4 +147,6 @@ log) by defining USE_BOOTLOG: Then `make clean` and `make`. Then in the CLI you can run `bootlog` to see the buffered log. -Note dmesg also requires that a serial port be defined for serial debugging. + +Note bootlog also requires that a serial port be defined for serial debugging. + diff --git a/docs/javascript_programming/GENERATE_CONSTANTS_README.md b/docs/javascript_programming/GENERATE_CONSTANTS_README.md new file mode 100644 index 00000000000..dfea3be7979 --- /dev/null +++ b/docs/javascript_programming/GENERATE_CONSTANTS_README.md @@ -0,0 +1,169 @@ +# INAV Constants Generator + +## Purpose + +Generates `inav_constants.js` from the INAV firmware header file (`logic_condition.h`) to ensure the transpiler/decompiler constants exactly match the actual firmware. + +## Single Source of Truth Architecture + +**Firmware Header** (`logic_condition.h`) + ↓ (parse) +**Constants File** (`inav_constants.js`) - AUTO-GENERATED + ↓ (import) +**API Definitions** (`flight.js`, etc.) - Reference constants + ↓ (use) +**Transpiler & Decompiler** - Use constants + +## Usage + +### Generate Constants + +```bash +node scripts/generate-constants.js [output-path] +``` + +### Example + +```bash +# From INAV configurator root +node scripts/generate-constants.js \ + ../inav/src/main/programming/logic_condition.h \ + js/transpiler/transpiler/inav_constants.js +``` + +### Default Output + +If output path is not specified, defaults to: +``` +js/transpiler/transpiler/inav_constants.js +``` + +## What It Parses + +The parser extracts these enums from `logic_condition.h`: + +1. **logicOperation_e** → `OPERATION` + - TRUE, EQUAL, GREATER_THAN, etc. + +2. **logicOperandType_s** → `OPERAND_TYPE` + - VALUE, RC_CHANNEL, FLIGHT, etc. + +3. **logicFlightOperands_e** → `FLIGHT_PARAM` + - ARM_TIMER, HOME_DISTANCE, RSSI, YAW, etc. + +4. **logicFlightModeOperands_e** → `FLIGHT_MODE` + - FAILSAFE, MANUAL, RTH, etc. + +5. **logicWaypointOperands_e** → `WAYPOINT_PARAM` + - IS_WP, WAYPOINT_INDEX, etc. + +## Generated File Format + +```javascript +/** + * AUTO-GENERATED from firmware header files + * DO NOT EDIT MANUALLY + */ + +const OPERAND_TYPE = { + VALUE: 0, + RC_CHANNEL: 1, + FLIGHT: 2, + // ... +}; + +const OPERATION = { + TRUE: 0, + EQUAL: 1, + // ... +}; + +const FLIGHT_PARAM = { + ARM_TIMER: 0, + HOME_DISTANCE: 1, + // ... + YAW: 40, // ← Correct value from firmware! + // ... +}; + +// ... exports + +``` + +## Build Integration + +Add to package.json scripts: + +```json +{ + "scripts": { + "generate-constants": "node scripts/generate-constants.js ../inav/src/main/programming/logic_condition.h", + "prebuild": "npm run generate-constants" + } +} +``` + +This ensures constants are regenerated before each build. + +## Next Steps: Update API Definitions + +Currently, API definitions have hardcoded values: + +```javascript +// inav.flight.js - WRONG (hardcoded) +yaw: { + inavOperand: { type: 2, value: 17 } // Wrong value! +} + +``` + +Change to reference constants: + +```javascript +// inav.flight.js - CORRECT (references constants) +const { OPERAND_TYPE, FLIGHT_PARAM } = require('../../transpiler/inav_constants.js'); + +yaw: { + inavOperand: { type: OPERAND_TYPE.FLIGHT, value: FLIGHT_PARAM.ATTITUDE_YAW } +} + +``` + +Benefits: +- ✅ Single source of truth (firmware) +- ✅ Type-safe references +- ✅ Compile-time errors if constants missing +- ✅ Auto-update when firmware changes + +## Verification + +After generating, verify key values: + +```bash +grep "ATTITUDE_YAW" js/transpiler/transpiler/inav_constants.js +# Should show: ATTITUDE_YAW: 40, + +grep "IS_ARMED" js/transpiler/transpiler/inav_constants.js +# Should show: IS_ARMED: 17, +``` + +## Error Handling + +The parser handles: +- ✅ Both `typedef enum { } name_e;` and `typedef enum name { }` formats +- ✅ Explicit values (`= 40`) +- ✅ Auto-incrementing values +- ✅ Hex values (`= 0x10`) +- ✅ C-style comments (`//` and `/* */`) +- ✅ Missing enums (warns but continues) + +## Maintenance + +**When INAV firmware updates:** +1. Get new `logic_condition.h` from firmware repo +2. Run `npm run generate-constants` +3. Review diff in `inav_constants.js` +4. Test transpiler/decompiler +5. Commit updated constants file + +**DO NOT manually edit `inav_constants.js`** - all changes will be overwritten! diff --git a/docs/javascript_programming/JAVASCRIPT_PROGRAMMING_GUIDE.md b/docs/javascript_programming/JAVASCRIPT_PROGRAMMING_GUIDE.md new file mode 100644 index 00000000000..fedda8bb7ac --- /dev/null +++ b/docs/javascript_programming/JAVASCRIPT_PROGRAMMING_GUIDE.md @@ -0,0 +1,321 @@ +# INAV JavaScript Programming - Quick Reference + +## Relationship to Logic Conditions + +This JavaScript programming interface is built on top of INAV's traditional +[Logic Conditions](../../Programming%20Framework.md) system. The JavaScript code you +write is transpiled (converted) into logic conditions that run on the flight controller. + +If you're familiar with the traditional logic conditions interface, you can think of +JavaScript programming as a more user-friendly syntax that generates the same logic +conditions behind the scenes. + +**See also:** +- [Programming Framework documentation](../../Programming%20Framework.md) - Details about the underlying logic conditions system +- [Operations Reference](OPERATIONS_REFERENCE.md) - Complete reference for all supported operations + +--- + +## Pattern Guide + +### Continuous Conditions (if statements) +Use `if` statements for conditions that should check and execute **every cycle**: + +```javascript + +// Checks every cycle - adjusts VTX power continuously +if (inav.flight.homeDistance > 100) { + inav.override.vtx.power = 3; +} + +``` + +**Use when:** You want the action to happen continuously while the condition is true. + +--- + +### One-Time Execution (edge) +Use `edge()` for actions that should execute **only once** when a condition becomes true: + +```javascript + +// Executes ONCE when armTimer reaches 1000ms +inav.events.edge(() => inav.flight.armTimer > 1000, { duration: 0 }, () => { + inav.gvar[0] = inav.flight.yaw; // Save initial heading + inav.gvar[1] = 0; // Initialize counter +}); + +``` + +**Parameters:** +- **condition**: Function returning boolean +- **duration**: Minimum duration in ms (0 = instant, >0 = debounce) +- **action**: Function to execute once + +**Use when:** +- Initializing on arm +- Detecting events (first time RSSI drops) +- Counting discrete occurrences +- Debouncing noisy signals + +--- + +### Latching/Sticky Conditions +Use `sticky()` for conditions that latch ON and stay ON until reset: + +```javascript + +// Latches ON when RSSI < 30, stays ON until RSSI > 70 +inav.events.sticky( + () => inav.flight.rssi < 30, // ON condition + () => inav.flight.rssi > 70, // OFF condition + () => { + inav.override.vtx.power = 4; // Executes while latched + } +); + +``` + +**Parameters:** +- **onCondition**: When to latch ON +- **offCondition**: When to latch OFF +- **action**: What to do while latched + +**Use when:** +- Warning states that need manual reset +- Hysteresis/deadband behavior +- Failsafe conditions + +--- + +### Delayed Execution (delay) +Use `delay()` to execute after a condition has been true for a duration: + +```javascript + +// Executes only if RSSI < 30 for 2 seconds continuously +inav.events.delay(() => inav.flight.rssi < 30, { duration: 2000 }, () => { + inav.gvar[0] = 1; // Set failsafe flag +}); + +``` + +**Parameters:** +- **condition**: Condition that must remain true +- **duration**: How long condition must be true (ms) +- **action**: Action to execute after delay + +**Use when:** +- Avoiding false triggers +- Requiring sustained conditions +- Timeouts and delays + +--- + +## Common Patterns + +### Initialize on Arm +```javascript + +inav.events.edge(() => inav.flight.armTimer > 1000, { duration: 0 }, () => { + inav.gvar[0] = 0; // Reset counter + inav.gvar[1] = inav.flight.yaw; // Save heading + inav.gvar[2] = inav.flight.altitude; // Save starting altitude +}); + +``` + +### Count Events +```javascript + +// Initialize +inav.events.edge(() => inav.flight.armTimer > 1000, { duration: 0 }, () => { + inav.gvar[0] = 0; +}); + +// Count each time RSSI drops below 30 (counts transitions, not duration) +inav.events.edge(() => inav.flight.rssi < 30, { duration: 100 }, () => { + inav.gvar[0] = inav.gvar[0] + 1; +}); + +``` + +### Debounce Noisy Signals +```javascript + +// Only trigger if RSSI < 30 for at least 500ms +inav.events.edge(() => inav.flight.rssi < 30, { duration: 500 }, () => { + inav.override.vtx.power = 4; +}); + +``` + +### Multi-Stage Logic +```javascript + +// Stage 1: Far away +if (inav.flight.homeDistance > 500) { + inav.override.vtx.power = 4; +} + +// Stage 2: Medium distance +if (inav.flight.homeDistance > 200 && inav.flight.homeDistance <= 500) { + inav.override.vtx.power = 3; +} + +// Stage 3: Close to home +if (inav.flight.homeDistance <= 200) { + inav.override.vtx.power = 2; +} + +``` + +### Hysteresis/Deadband +```javascript + +// Turn ON at low voltage, turn OFF when recovered +inav.events.sticky( + () => inav.flight.cellVoltage < 330, // Warning threshold + () => inav.flight.cellVoltage > 350, // Recovery threshold + () => { + inav.override.throttleScale = 50; // Reduce throttle while in warning + inav.gvar[0] = 1; // Warning flag + } +); + +``` + +--- + +## Key Differences + +| Pattern | Executes | Reset | Use Case | +|---------|----------|-------|----------| +| `if` | Every cycle while true | N/A | Continuous control | +| `edge()` | Once per transition | When condition becomes false | Events, initialization | +| `sticky()` | Continuous while latched | When OFF condition met | Warnings, hysteresis | +| `delay()` | Once after duration | When condition becomes false | Timeouts, debouncing | + +--- + +## Variables + +### Let/Const Variables + +Use `let` or `const` to define reusable expressions that are compiled into the logic: + +```javascript +// Define reusable calculations +let distanceThreshold = 500; +let altitudeLimit = 100; +let combinedCondition = inav.flight.homeDistance > distanceThreshold && inav.flight.altitude > altitudeLimit; + +// Use in conditions +if (combinedCondition) { + inav.override.vtx.power = 4; +} +``` + +**Benefits:** +- Makes code more readable with named values +- Compiler automatically optimizes duplicate expressions +- Variables preserve their custom names through compile/decompile cycles + +**Important:** `let`/`const` variables are **compile-time substituted**, not runtime variables. For runtime state, use `inav.gvar[]`. + +### Ternary Operator + +Use ternary expressions for conditional values: + +```javascript +// Assign based on condition +let throttleLimit = inav.flight.cellVoltage < 330 ? 25 : 50; + +if (inav.flight.cellVoltage < 350) { + inav.override.throttleScale = throttleLimit; +} + +// Inline in expressions +inav.override.vtx.power = inav.flight.homeDistance > 500 ? 4 : 2; +``` + +**Use when:** You need conditional value assignment in a single expression. + +--- + +## Available Objects + +The `inav` namespace provides access to all flight controller data and control functions: + +- `inav.flight` - Flight telemetry (including `flight.mode.*`) +- `inav.override` - Override flight parameters +- `inav.rc` - RC channels +- `inav.gvar` - Global variables (0-7) +- `inav.pid` - Programming PID outputs (`pid[0-3].output`) +- `inav.waypoint` - Waypoint navigation +- `inav.events.edge` - Edge detection +- `inav.events.sticky` - Latching conditions +- `inav.events.delay` - Delayed execution + +### Flight Mode Detection + +Check which flight modes are currently active via `inav.flight.mode.*`: + +```javascript +if (inav.flight.mode.poshold === 1) { + inav.gvar[0] = 1; // Flag: in position hold +} + +if (inav.flight.mode.rth === 1) { + inav.override.vtx.power = 4; // Max power during RTH +} +``` + +**Available modes:** `failsafe`, `manual`, `rth`, `poshold`, `cruise`, `althold`, `angle`, `horizon`, `air`, `acro`, `courseHold`, `waypointMission`, `user1` through `user4` + +### PID Controller Outputs + +Read output values from the 4 programming PID controllers (configured in Programming PID tab): + +```javascript +if (inav.pid[0].output > 500) { + inav.override.throttle = 1600; +} + +inav.gvar[0] = inav.pid[0].output; // Store for OSD display +``` + +**Available:** `inav.pid[0].output` through `inav.pid[3].output` + +--- + +## Tips + +1. **Initialize variables on arm** using `inav.events.edge()` with `inav.flight.armTimer > 1000` +2. **Use inav.gvar for state** - they persist between logic condition evaluations +3. **edge() duration = 0** means instant trigger on condition becoming true +4. **edge() duration > 0** adds debounce time +5. **if statements are continuous** - they execute every cycle +6. **sticky() provides hysteresis** - prevents rapid ON/OFF switching +7. **Use Math functions** - `Math.abs()`, `Math.min()`, `Math.max()` are available + +--- + +## Debugging + +Use global variables to track state: +```javascript + +// Debug counter +inav.events.edge(() => inav.flight.armTimer > 1000, { duration: 0 }, () => { + inav.gvar[7] = 0; // Use inav.gvar[7] as debug counter +}); + +// Increment on each event +inav.events.edge(() => inav.flight.rssi < 30, { duration: 0 }, () => { + inav.gvar[7] = inav.gvar[7] + 1; +}); + +// Check inav.gvar[7] value in OSD or Configurator to see event count + +``` diff --git a/docs/javascript_programming/OPERATIONS_REFERENCE.md b/docs/javascript_programming/OPERATIONS_REFERENCE.md new file mode 100644 index 00000000000..eb967ae31bb --- /dev/null +++ b/docs/javascript_programming/OPERATIONS_REFERENCE.md @@ -0,0 +1,126 @@ +# INAV Logic Condition Operations Reference + +This document provides a complete reference for all INAV logic condition operations and their JavaScript transpiler implementations. + +## All INAV Operations Now Implemented! 🎉 + +All INAV logic condition operations supported by the firmware are now fully implemented in the JavaScript transpiler. + +## Already Implemented + +✅ **Arithmetic**: ADD, SUB, MUL, DIV, MODULUS (via +, -, *, /, %) +✅ **Comparisons**: EQUAL, GREATER_THAN, LOWER_THAN, APPROX_EQUAL (via ===, >, <, approxEqual()) +✅ **Logical**: AND, OR, NOT, XOR, NAND, NOR (via &&, ||, !, xor(), nand(), nor()) +✅ **Math**: MIN, MAX, SIN, COS, TAN, ABS (via Math.min/max/sin/cos/tan/abs) +✅ **Scaling**: MAP_INPUT, MAP_OUTPUT (via mapInput(), mapOutput()) +✅ **Flow control**: STICKY, EDGE, DELAY, TIMER, DELTA (via on.* and helper functions) +✅ **Variables**: GVAR_SET, GVAR_INC, GVAR_DEC (via assignments, ++, --) +✅ **Overrides**: VTX, throttle, arming, etc. (via override.* properties) +✅ **RC Channel States**: LOW, MID, HIGH (via rc[n].low, rc[n].mid, rc[n].high) + +## Usage Examples + +### Logical Operations + +```javascript +// XOR - true when exactly one condition is true +if (xor(inav.flight.armed, inav.flight.mode.failsafe)) { + // One or the other, but not both +} + +// NAND - false only when both are true +if (nand(inav.flight.armed, inav.gvar[0] > 100)) { + // Not both conditions true +} + +// NOR - true only when both are false +if (nor(inav.flight.mode.failsafe, inav.flight.mode.rth)) { + // Neither failsafe nor RTH active +} + +// Approximate equality with tolerance +if (approxEqual(inav.flight.altitude, 1000, 50)) { + // Altitude is 1000 ± 50 +} + +``` + +### Scaling/Mapping Operations + +```javascript +// mapInput: Scale from [0:maxValue] to [0:1000] +// Example: RC value (1000-2000) to normalized (0-1000) +const normalizedThrottle = mapInput(inav.rc[3].value - 1000, 1000); + +// mapOutput: Scale from [0:1000] to [0:maxValue] +// Example: normalized (0-1000) to servo angle (0-180) +const servoAngle = mapOutput(normalizedThrottle, 180); + +// Chaining for full range mapping +// Map altitude (0-5000m) to percentage (0-100) +const altitudePercent = mapOutput(mapInput(inav.flight.altitude, 5000), 100); + +``` + +### RC Channel State Detection + +```javascript +// LOW state - RC value < 1333us +if (inav.rc[0].low) { + // Roll stick is in low position + inav.gvar[0] = 1; +} + +// MID state - RC value between 1333-1666us +if (inav.rc[1].mid) { + // Pitch stick is centered + inav.gvar[1] = 1; +} + +// HIGH state - RC value > 1666us +if (inav.rc[2].high) { + // Throttle stick is in high position + inav.override.vtx.power = 4; +} + +// Access raw RC value +if (inav.rc[3].value > 1700) { + // Custom threshold on yaw channel + inav.gvar[2] = 1; +} + +// Combined with logical operations +if (inav.rc[0].low && inav.rc[1].mid && inav.rc[2].high) { + // Specific stick combination detected + inav.override.armSafety = 1; +} + +``` + +## Notes + +- RC channels: `rc[0]` through `rc[17]` (18 channels) +- RC channel properties: `.value` (1000-2000us), `.low` (<1333us), `.mid` (1333-1666us), `.high` (>1666us) +- All trig functions (sin/cos/tan) take degrees, not radians +- MODULUS operator bug fixed: `%` now correctly generates MODULUS operation +- MAP_INPUT normalizes to [0:1000], MAP_OUTPUT scales from [0:1000] + +## Recent Changes + +**2025-11-25 (Session 3)**: +- ✅ Implemented RC channel LOW/MID/HIGH state detection (rc[n].low, rc[n].mid, rc[n].high) +- ✅ Fixed analyzer to validate RC channel array access patterns +- ✅ Updated codegen to generate LOW/MID/HIGH operations (operations 4, 5, 6) +- 🎉 ALL INAV logic condition operations now fully implemented and tested! + +**2025-11-25 (Session 2)**: +- ✅ Implemented XOR, NAND, NOR logical operations (xor(), nand(), nor()) +- ✅ Implemented APPROX_EQUAL comparison (approxEqual()) +- ✅ Implemented MAP_INPUT and MAP_OUTPUT scaling (mapInput(), mapOutput()) + +**2025-11-25 (Session 1)**: +- ✅ Implemented Math.min(), Math.max(), Math.sin(), Math.cos(), Math.tan() +- ✅ Fixed MODULUS operator (was using wrong constant name) +- 📝 Documented unimplemented operations for future work + +**Last Updated**: 2025-11-25 diff --git a/docs/javascript_programming/TESTING_GUIDE.md b/docs/javascript_programming/TESTING_GUIDE.md new file mode 100644 index 00000000000..c44f4ad9202 --- /dev/null +++ b/docs/javascript_programming/TESTING_GUIDE.md @@ -0,0 +1,267 @@ +# Transpiler Testing Guide + +This guide explains how to test changes to the INAV JavaScript transpiler to ensure all components work together correctly. + +## Overview + +The transpiler has 4 main components that must stay in sync: + +1. **Parser** (`transpiler/parser.js`) - Parses JavaScript code into AST +2. **Analyzer** (`transpiler/analyzer.js`) - Validates syntax and checks for errors +3. **Codegen** (`transpiler/codegen.js`) - Generates INAV CLI commands from AST +4. **Decompiler** (`transpiler/decompiler.js`) - Converts CLI commands back to JavaScript + +## When Adding a New Feature + +When implementing a new operation, function, or syntax feature, you must update: + +### 1. Analyzer (transpiler/analyzer.js) +- Add validation for new syntax patterns +- Update property access validation if needed +- Add warnings for unsupported variations + +Example: For RC channel states, added validation for `rc[0-17]` with `.low/.mid/.high` properties + +### 2. Codegen (transpiler/codegen.js) +- Add code generation for the new feature +- Handle the new AST node types +- Generate correct INAV operation codes + +Example: For RC channel states, added handler in `generateCondition()` to detect `rc[n].low` and generate LOW operation (4) + +### 3. Decompiler (transpiler/decompiler.js) +- Add reverse mapping from operation codes to JavaScript +- Ensure generated code matches expected syntax + +Example: For RC channel states, map operations 4/5/6 to `.low/.mid/.high` properties + +### 4. Diagnostics (optional, editor/diagnostics.js) +- Add helpful warnings for common mistakes +- Suggest correct syntax alternatives + +## Round-Trip Testing + +The most reliable way to test transpiler changes is round-trip testing: +JavaScript → CLI commands → JavaScript + +### Basic Test Template + +```javascript +import { Transpiler } from './transpiler/index.js'; +import { Decompiler } from './transpiler/decompiler.js'; + +const testCode = ` + +// Your test code here +if (inav.flight.altitude > 100) { + inav.gvar[0] = 1; +} +`; + +console.log('=== Original JavaScript ===\n'); +console.log(testCode); + +// Transpile +const transpiler = new Transpiler(); +const transpileResult = transpiler.transpile(testCode); + +console.log('\n=== Generated CLI Commands ===\n'); +transpileResult.commands.forEach(cmd => console.log(cmd)); + +// Parse commands to LC format for decompiler +const logicConditions = transpileResult.commands.map((cmd) => { + const parts = cmd.split(/\s+/); + return { + index: parseInt(parts[1]), + enabled: parseInt(parts[2]), + activatorId: parseInt(parts[3]), + operation: parseInt(parts[4]), + operandAType: parseInt(parts[5]), + operandAValue: parseInt(parts[6]), + operandBType: parseInt(parts[7]), + operandBValue: parseInt(parts[8]), + flags: parseInt(parts[9]) + }; +}); + +// Decompile +const decompiler = new Decompiler(); +const decompileResult = decompiler.decompile(logicConditions); + +console.log('\n=== Decompiled JavaScript ===\n'); +console.log(decompileResult.code); + +if (decompileResult.success) { + console.log('\n✅ Round-trip successful!'); +} else { + console.error('\n❌ Decompilation failed:', decompileResult.error); +} + +``` + +### Running Tests + +```bash +cd /path/to/inav-configurator/js/transpiler + +# Create your test file +nano test_feature.js + +# Run the test +node test_feature.js + +# Clean up after testing +rm test_feature.js +``` + +## Test Cases to Verify + +When making changes, test these scenarios: + +### 1. Basic Functionality +```javascript + +if (inav.flight.altitude > 100) { + inav.gvar[0] = 1; +} + +``` +Expected: 2 commands (condition + action) + +### 2. Error Handling +```javascript +// Invalid syntax - should produce helpful error +if (inav.flight.invalidProperty > 100) { + inav.gvar[0] = 1; +} + +``` +Expected: Error with suggestion for correct property + +### 3. Edge Cases +```javascript +// Test boundary values +if (inav.rc[0].low) { // Channel 0 (first) + inav.gvar[0] = 1; +} + +if (inav.rc[17].high) { // Channel 17 (last valid) + inav.gvar[1] = 1; +} + +``` +Expected: Valid generation for both + +### 4. Complex Combinations +```javascript +// Test multiple operations together +if (xor(inav.rc[0].low, inav.flight.armed)) { + inav.gvar[0] = Math.max(100, inav.flight.altitude); +} + +``` +Expected: Proper nesting of operations + +## Common Issues + +### Analyzer Rejects Valid Syntax +**Problem**: Analyzer validation is too strict +**Solution**: Update `checkPropertyAccess()` or relevant validation method +**Example**: RC channels needed special handling for array syntax `rc[n]` + +### Codegen Produces Wrong Operation Code +**Problem**: Operation constant name incorrect or not imported +**Solution**: Check `OPERATION.*` constants match `inav_constants.js` +**Example**: MODULUS was incorrectly `OPERATION.MOD` instead of `OPERATION.MODULUS` + +### Decompiler Output Doesn't Match Input +**Problem**: Decompiler reverse mapping incomplete +**Solution**: Add case for new operation in decompiler +**Example**: LOW/MID/HIGH operations needed to map to `.low/.mid/.high` properties + +### Line Numbers Off in Errors +**Problem**: Auto-import adds lines before parsing +**Solution**: Track lineOffset and adjust all error/warning line numbers +**Fixed**: Session 1 of 2025-11-25 + +## Verification Checklist + +Before committing changes: + +- [ ] Analyzer validates new syntax without false positives +- [ ] Codegen generates correct operation codes +- [ ] Decompiler correctly reverses the operation +- [ ] Round-trip test passes (JS → CLI → JS) +- [ ] Error messages are helpful and accurate +- [ ] Edge cases are handled (boundary values, empty inputs) +- [ ] Documentation updated (OPERATIONS_REFERENCE.md, API definitions) + +## Example: Adding a New Operation + +Let's say you want to add support for a new INAV operation `FOOBAR (operation 99)`: + +**1. Check if operation exists in firmware** +```javascript +// Look in transpiler/inav_constants.js +const OPERATION = { + // ... + FOOBAR: 99, // Check this exists + // ... +}; + +``` + +**2. Update Codegen** +```javascript +// In transpiler/codegen.js +case 'CallExpression': { + const funcName = condition.callee?.name; + + if (funcName === 'foobar') { + // Generate FOOBAR operation + const resultIndex = this.lcIndex; + this.commands.push( + `logic ${this.lcIndex} 1 ${activatorId} ${OPERATION.FOOBAR} ...` + ); + this.lcIndex++; + return resultIndex; + } +} + +``` + +**3. Update Decompiler** +```javascript +// In transpiler/decompiler.js +case OPERATION.FOOBAR: + return 'foobar()'; + +``` + +**4. Update Analyzer (if needed)** +```javascript +// In transpiler/analyzer.js +// Add validation for foobar() usage + +``` + +**5. Test Round-Trip** +```javascript +const testCode = ` + +if (foobar()) { + inav.gvar[0] = 1; +} +`; +// Run round-trip test... + +``` + +**6. Update Documentation** +- Add to OPERATIONS_REFERENCE.md +- Add usage examples +- Update API definitions if needed + +## Last Updated + +2025-11-25 - Created after implementing RC channel state detection and completing all INAV operations diff --git a/docs/javascript_programming/TIMER_WHENCHANGED_EXAMPLES.md b/docs/javascript_programming/TIMER_WHENCHANGED_EXAMPLES.md new file mode 100644 index 00000000000..35b89d30f8c --- /dev/null +++ b/docs/javascript_programming/TIMER_WHENCHANGED_EXAMPLES.md @@ -0,0 +1,297 @@ +# timer() and whenChanged() Examples + +## timer() Examples + +### Example 1: Periodic VTX Power Boost +```javascript + +// Boost VTX power to maximum for 1 second every 5 seconds +inav.events.timer(1000, 5000, () => { + inav.override.vtx.power = 4; +}); + +``` + +### Example 2: Flashing OSD Layout +```javascript + +// Alternate between OSD layout 0 and 1 every second +inav.events.timer(1000, 1000, () => { + inav.override.osdLayout = 1; +}); + +``` + +### Example 3: Periodic Status Check +```javascript + +// Record battery voltage every 10 seconds for 100ms +inav.events.timer(100, 10000, () => { + inav.gvar[0] = inav.flight.vbat; + inav.gvar[1] = inav.flight.current; +}); + +``` + +### Example 4: Warning Beep Pattern +```javascript + +// Beep pattern: 200ms on, 200ms off, 200ms on, 5s off +// (Would need multiple timers or different approach for complex patterns) +inav.events.timer(200, 200, () => { + inav.override.rcChannel(8, 2000); // Beeper channel +}); + +``` + +## whenChanged() Examples + +### Example 1: Altitude Change Logger +```javascript + +// Log altitude whenever it changes by 50cm or more +inav.events.whenChanged(inav.flight.altitude, 50, () => { + inav.gvar[0] = inav.flight.altitude; +}); + +``` + +### Example 2: RSSI Drop Detection +```javascript + +// Boost VTX power when RSSI drops by 10 or more +inav.events.whenChanged(inav.flight.rssi, 10, () => { + if (inav.flight.rssi < 50) { + inav.override.vtx.power = 4; + } +}); + +``` + +### Example 3: Speed Change Tracker +```javascript + +// Track ground speed changes of 100cm/s or more +inav.events.whenChanged(inav.flight.groundSpeed, 100, () => { + inav.gvar[1] = inav.flight.groundSpeed; +}); + +``` + +### Example 4: Battery Voltage Monitor +```javascript + +// Record voltage whenever it changes by 1 unit (0.1V) +inav.events.whenChanged(inav.flight.vbat, 1, () => { + inav.gvar[2] = inav.flight.vbat; + inav.gvar[3] = inav.flight.mahDrawn; +}); + +``` + +### Example 5: Climb Rate Detection +```javascript + +// Detect rapid climbs (>50cm/s change in vertical speed) +inav.events.whenChanged(inav.flight.verticalSpeed, 50, () => { + if (inav.flight.verticalSpeed > 200) { + // Climbing fast - reduce throttle scale + inav.override.throttleScale = 80; + } +}); + +``` + +## Combined Examples + +### Example 1: Timer + WhenChanged +```javascript + +// Periodic VTX boost +inav.events.timer(1000, 5000, () => { + inav.override.vtx.power = 4; +}); + +// Log altitude changes +inav.events.whenChanged(inav.flight.altitude, 100, () => { + inav.gvar[0] = inav.flight.altitude; +}); + +``` + +### Example 2: Conditional Timer +```javascript + +// Only run timer when armed +if (inav.flight.isArmed) { + inav.events.timer(500, 500, () => { + inav.override.osdLayout = 2; + }); +} + +``` + +### Example 3: Emergency Response System +```javascript + +// Monitor RSSI drops +inav.events.whenChanged(inav.flight.rssi, 5, () => { + if (inav.flight.rssi < 30) { + // RSSI critical - max VTX power + inav.override.vtx.power = 4; + inav.gvar[7] = 1; // Set emergency flag + } +}); + +// Monitor altitude changes +inav.events.whenChanged(inav.flight.altitude, 200, () => { + if (inav.flight.altitude > 10000) { + // Too high - warn + inav.gvar[6] = inav.flight.altitude; + } +}); + +``` + +### Example 4: Data Logging System +```javascript + +// Periodic logging every 5 seconds +inav.events.timer(100, 5000, () => { + inav.gvar[0] = inav.flight.vbat; + inav.gvar[1] = inav.flight.current; + inav.gvar[2] = inav.flight.altitude; +}); + +// Event-based logging on significant changes +inav.events.whenChanged(inav.flight.altitude, 500, () => { + inav.gvar[3] = inav.flight.altitude; + inav.gvar[4] = inav.flight.verticalSpeed; +}); + +inav.events.whenChanged(inav.flight.groundSpeed, 200, () => { + inav.gvar[5] = inav.flight.groundSpeed; +}); + +``` + +## Logic Condition Output Examples + +### timer() Output +```javascript +inav.events.timer(1000, 2000, () => { + inav.gvar[0] = 1; +}); + +``` +Generates: +``` +logic 0 1 -1 49 0 1000 0 2000 0 # TIMER: 1000ms ON, 2000ms OFF +logic 1 1 0 18 0 0 0 0 1 0 # gvar[0] = 1 +``` + +### whenChanged() Output +```javascript +inav.events.whenChanged(inav.flight.altitude, 50, () => { + inav.gvar[0] = inav.flight.altitude; +}); + +``` +Generates: +``` +logic 0 1 -1 50 2 12 0 50 0 # DELTA: altitude, threshold 50 +logic 1 1 0 18 0 0 2 12 0 # gvar[0] = altitude +``` + +## Use Cases + +### timer() Use Cases +1. **Periodic Tasks** - Execute actions at regular intervals +2. **Flashing Indicators** - Toggle states on/off +3. **Sampling** - Collect data periodically +4. **Timeouts** - Implement time-based state machines +5. **Warning Systems** - Beep or flash warnings + +### whenChanged() Use Cases +1. **Event Detection** - React to significant changes +2. **Data Logging** - Record values when they change +3. **Thresholds** - Trigger actions on large changes +4. **Rate Limiting** - Avoid excessive updates (100ms window) +5. **Monitoring** - Track important parameter changes + +## Best Practices + +### timer() +- ✅ Use for periodic, time-based actions +- ✅ Keep durations reasonable (>100ms) +- ✅ Avoid very short ON times (may miss cycles) +- ❌ Don't use for one-time delays (use delay() instead) +- ❌ Don't nest timers (creates complex behavior) + +### whenChanged() +- ✅ Use for event-driven responses +- ✅ Set thresholds appropriate to your data +- ✅ Monitor slowly-changing values +- ❌ Don't use for rapidly-changing values (may trigger constantly) +- ❌ Don't set threshold too low (noise will trigger it) +- ⚠️ Remember: 100ms detection window (built into DELTA operation) + +## Common Patterns + +### Pattern 1: Status Monitor +```javascript +// Log key parameters when they change significantly +inav.events.whenChanged(inav.flight.vbat, 2, () => { inav.gvar[0] = inav.flight.vbat; }); +inav.events.whenChanged(inav.flight.rssi, 10, () => { inav.gvar[1] = inav.flight.rssi; }); +inav.events.whenChanged(inav.flight.altitude, 100, () => { inav.gvar[2] = inav.flight.altitude; }); + +``` + +### Pattern 2: Periodic Beacon +```javascript +// Boost VTX power briefly every 10 seconds +inav.events.timer(500, 10000, () => { + inav.override.vtx.power = 4; +}); + +``` + +### Pattern 3: Adaptive Response +```javascript +// React to rapid altitude changes +inav.events.whenChanged(inav.flight.altitude, 200, () => { + if (inav.flight.altitude < 1000) { + inav.override.throttleScale = 120; // Boost + } else { + inav.override.throttleScale = 80; // Reduce + } +}); + +``` + +## Troubleshooting + +### timer() Issues +**Problem:** Timer doesn't seem to trigger +- Check that ON duration > 0 +- Check that OFF duration > 0 +- Verify actions are valid + +**Problem:** Timer seems irregular +- INAV runs at different rates depending on mode +- Timer precision is limited by LC execution rate + +### whenChanged() Issues +**Problem:** Never triggers +- Check threshold isn't too high +- Verify value actually changes +- Remember: 100ms detection window + +**Problem:** Triggers too often +- Increase threshold +- Value may be noisy +- Consider using timer() for periodic sampling instead + +**Problem:** Doesn't track fast changes +- Built-in 100ms window +- Use timer() for faster sampling if needed diff --git a/docs/javascript_programming/TIMER_WHENCHANGED_IMPLEMENTATION.md b/docs/javascript_programming/TIMER_WHENCHANGED_IMPLEMENTATION.md new file mode 100644 index 00000000000..8befa685ff2 --- /dev/null +++ b/docs/javascript_programming/TIMER_WHENCHANGED_IMPLEMENTATION.md @@ -0,0 +1,156 @@ +# timer() and whenChanged() Functions + +## Overview + +The `timer()` and `whenChanged()` functions provide advanced timing and change-detection capabilities for INAV logic programming. + +## timer() Function + +### Syntax +```javascript +inav.events.timer(onMs, offMs, () => { + // actions +}); + +``` + +### Description +Execute actions on a periodic timer with on/off cycling. The action executes during the "on" period and pauses during the "off" period. + +### Parameters +- `onMs`: Duration in milliseconds to run the action +- `offMs`: Duration in milliseconds to wait between executions +- `action`: Arrow function containing actions to execute during on-time + +### Example +```javascript + +// Flash VTX power: ON for 1 second, OFF for 2 seconds, repeat +inav.events.timer(1000, 2000, () => { + inav.override.vtx.power = 4; +}); + +``` + +**Generated Logic Conditions:** +``` +logic 0 1 -1 49 0 1000 0 2000 0 # TIMER: ON 1000ms, OFF 2000ms +logic 1 1 0 25 0 0 0 0 4 0 # Set VTX power = 4 +``` + +### Validation Rules +- Requires exactly 3 arguments +- `onMs` must be numeric literal > 0 +- `offMs` must be numeric literal > 0 +- Third argument must be arrow function with actions + +## whenChanged() Function + +### Syntax +```javascript +inav.events.whenChanged(value, threshold, () => { + // actions +}); + +``` + +### Description +Execute actions when a monitored value changes by more than the specified threshold. Uses a built-in 100ms detection window. + +### Parameters +- `value`: Flight parameter or global variable to monitor +- `threshold`: Minimum change required to trigger (numeric literal) +- `action`: Arrow function containing actions to execute on change + +### Example +```javascript + +// Log altitude whenever it changes by 50cm or more +inav.events.whenChanged(inav.flight.altitude, 50, () => { + inav.gvar[0] = inav.flight.altitude; +}); + +``` + +**Generated Logic Conditions:** +``` +logic 0 1 -1 50 2 12 0 50 0 # DELTA: altitude, threshold 50 +logic 1 1 0 18 0 0 2 12 0 # gvar[0] = altitude +``` + +### Validation Rules +- Requires exactly 3 arguments +- `value` must be a valid flight parameter or gvar +- `threshold` must be numeric literal > 0 +- Third argument must be arrow function with actions + +## Round-Trip Support + +Both functions support perfect round-trip transpilation/decompilation: + +### timer() Round-Trip +```javascript +// Original JavaScript +inav.events.timer(1000, 2000, () => { inav.gvar[0] = 1; }); + +// Transpiled to logic conditions +logic 0 1 -1 49 0 1000 0 2000 0 +logic 1 1 0 18 0 0 0 0 1 0 + +// Decompiled back to JavaScript +inav.events.timer(1000, 2000, () => { + inav.gvar[0] = 1; +}); + +``` + +### whenChanged() Round-Trip +```javascript +// Original JavaScript +inav.events.whenChanged(inav.flight.altitude, 50, () => { inav.gvar[0] = inav.flight.altitude; }); + +// Transpiled to logic conditions +logic 0 1 -1 50 2 12 0 50 0 +logic 1 1 0 18 0 0 2 12 0 + +// Decompiled back to JavaScript +inav.events.whenChanged(inav.flight.altitude, 50, () => { + inav.gvar[0] = inav.flight.altitude; +}); + +``` + +## API Definitions + +Both functions are defined in `api/definitions/events.js`: + +```javascript +timer: { + type: 'function', + desc: 'Execute action on a periodic timer (on/off cycling)', + params: { + onMs: { type: 'number', unit: 'ms', desc: 'Duration to run action' }, + offMs: { type: 'number', unit: 'ms', desc: 'Duration to wait between executions' }, + action: { type: 'function', desc: 'Action to execute during on-time' } + }, + example: 'inav.events.timer(1000, 5000, () => { inav.override.vtx.power = 4; })' +}, + +whenChanged: { + type: 'function', + desc: 'Execute when value changes by more than threshold', + params: { + value: { type: 'number', desc: 'Value to monitor' }, + threshold: { type: 'number', desc: 'Change threshold' }, + action: { type: 'function', desc: 'Action to execute on change' } + }, + example: 'inav.events.whenChanged(inav.flight.altitude, 100, () => { inav.gvar[0] = inav.flight.altitude; })' +} + +``` + +## See Also + +- **TIMER_WHENCHANGED_EXAMPLES.md** - More usage examples and patterns +- **JAVASCRIPT_PROGRAMMING_GUIDE.md** - Complete programming guide +- **events.js** - Full API definitions diff --git a/docs/javascript_programming/api_definitions_summary.md b/docs/javascript_programming/api_definitions_summary.md new file mode 100644 index 00000000000..a7e74bd3bcb --- /dev/null +++ b/docs/javascript_programming/api_definitions_summary.md @@ -0,0 +1,284 @@ +# INAV API Definitions - Complete Implementation + +## Overview + +The INAV JavaScript API definitions are implemented in `js/transpiler/api/definitions/`. These files define the complete JavaScript API surface that maps to INAV firmware logic conditions. + +## API Definition Files + +### 1. ✅ **flight.js** - Flight Telemetry (READ-ONLY) +**Source**: `src/main/programming/logic_condition.c` (OPERAND_FLIGHT) + +Contains ~40 flight parameters including: +- **Timing**: armTimer, flightTime +- **Distance**: homeDistance, tripDistance, homeDirection +- **Communication**: rssi +- **Battery**: vbat, cellVoltage, current, mahDrawn, mwhDrawn, batteryPercentage +- **GPS**: gpsSats, gpsValid +- **Speed/Altitude**: groundSpeed, altitude, verticalSpeed +- **Attitude**: roll, pitch, yaw, heading, throttlePos +- **State**: isArmed, isAutoLaunch, isFailsafe +- **Profile**: mixerProfile +- **Navigation**: activeWpNumber, activeWpAction, courseToHome, gpsCourseOverGround +- **Modes** (nested): failsafe, manual, rth, poshold, althold, wp, gcs_nav, airmode, angle, horizon, cruise + +### 2. ✅ **override.js** - Flight Control Overrides (WRITABLE) +**Source**: `src/main/programming/logic_condition.c` (OPERATION_OVERRIDE_*) + +Contains override operations: +- **Throttle**: throttleScale, throttle +- **VTX** (nested): power, band, channel +- **Attitude** (nested): roll.angle, roll.rate, pitch.angle, pitch.rate, yaw.angle, yaw.rate +- **Heading**: heading override +- **RC Channels**: rcChannel[] array +- **Arming**: armSafety +- **OSD**: osdElement + +### 3. ✅ **rc.js** - RC Receiver Channels (READ-ONLY) +**Source**: RC channel handling in firmware + +18 RC channels (rc[0] through rc[17]), each with: +- `value`: Raw channel value (1000-2000µs) +- `low`: Channel < 1333µs +- `mid`: Channel 1333-1666µs +- `high`: Channel > 1666µs + +### 4. ✅ **gvar.js** - Global Variables (READ/WRITE) +**Source**: `src/main/programming/global_variables.c` + +8 global variables (gvar[0] through gvar[7]): +- Range: -1,000,000 to 1,000,000 +- Used for storing/sharing data between logic conditions + +### 5. ✅ **waypoint.js** - Waypoint Navigation (READ-ONLY) +**Source**: `src/main/navigation/navigation_pos_estimator.c` + +Waypoint mission data: +- **Current WP**: number, action +- **Position**: latitude, longitude, altitude +- **Navigation**: distance, bearing +- **Status**: missionReached, missionValid + +### 6. ✅ **pid.js** - Programming PID Controllers +**Source**: `src/main/programming/pid.c` + +4 PID controllers (pid[0] through pid[3]), each with: +- `configure()` method: setpoint, measurement, p, i, d, ff +- `output` property: Controller output +- `enabled` property: Controller state + +### 7. ✅ **helpers.js** - Math & Utility Functions +**Source**: `src/main/programming/logic_condition.c` (OPERATION_*) + +Math functions: +- **Basic**: min, max, abs +- **Trig**: sin, cos, tan (degrees) +- **Mapping**: mapInput, mapOutput +- **Arithmetic**: add, sub, mul, div, mod (operators) + +### 8. ✅ **events.js** - Event Handler Functions +**Source**: Logic condition framework + +Event handlers: +- **on.arm**: Execute after arming with delay +- **on.always**: Execute every cycle +- **when**: Execute when condition true +- **sticky**: Execute between on/off conditions +- **edge**: Execute on rising edge +- **delay**: Execute after condition true for duration +- **timer**: Execute on periodic timer +- **whenChanged**: Execute when value changes + +### 9. ✅ **index.js** - Main Export +Combines all definitions into single object. + +## Implementation Status + +| File | Status | Lines | Operands | Notes | +|------|--------|-------|----------|-------| +| flight.js | ✅ Ready | ~250 | FLIGHT(0-44) | All flight parameters | +| override.js | ✅ Ready | ~150 | OPS(23-46) | All override operations | +| rc.js | ✅ Ready | ~80 | RC(0-17) | 18 RC channels | +| gvar.js | ✅ Ready | ~30 | GVAR(0-7) | 8 global variables | +| waypoint.js | ✅ Ready | ~80 | WAYPOINT(0-8) | Waypoint nav data | +| pid.js | ✅ Ready | ~70 | PID(0-3) | 4 PID controllers | +| helpers.js | ✅ Ready | ~80 | OPS(14-39) | Math functions | +| events.js | ✅ Ready | ~120 | - | Event handlers | +| index.js | ✅ Ready | ~20 | - | Main export | + +## Operand Type Mapping + +From INAV firmware (`logic_condition.h`): + +```c +typedef enum logicOperandType_s { + LOGIC_CONDITION_OPERAND_TYPE_VALUE = 0, // Literal number + LOGIC_CONDITION_OPERAND_TYPE_RC_CHANNEL = 1, // RC channel value + LOGIC_CONDITION_OPERAND_TYPE_FLIGHT = 2, // Flight parameter (flight.js) + LOGIC_CONDITION_OPERAND_TYPE_FLIGHT_MODE = 3, // Flight mode + LOGIC_CONDITION_OPERAND_TYPE_LC = 4, // Logic condition result + LOGIC_CONDITION_OPERAND_TYPE_GVAR = 5, // Global variable (gvar.js) + LOGIC_CONDITION_OPERAND_TYPE_PID = 6, // Programming PID (pid.js) + LOGIC_CONDITION_OPERAND_TYPE_WAYPOINTS = 7 // Waypoint (waypoint.js) +} logicOperandType_e; +``` + +## Operation Mapping + +Key operations from firmware: + +```c +// Conditionals +OPERATION_TRUE = 0 +OPERATION_EQUAL = 1 +OPERATION_GREATER_THAN = 2 +OPERATION_LOWER_THAN = 3 +OPERATION_LOW = 4 +OPERATION_MID = 5 +OPERATION_HIGH = 6 + +// Logical +OPERATION_AND = 7 +OPERATION_OR = 8 +OPERATION_NOT = 12 +OPERATION_STICKY = 13 + +// Arithmetic +OPERATION_ADD = 14 +OPERATION_SUB = 15 +OPERATION_MUL = 16 +OPERATION_DIV = 17 +OPERATION_MOD = 18 + +// Global Variables +OPERATION_GVAR_SET = 19 +OPERATION_INC_GVAR = 20 +OPERATION_DEC_GVAR = 21 + +// Overrides +OPERATION_OVERRIDE_ARM_SAFETY = 23 +OPERATION_OVERRIDE_ARMING_DISABLED = 24 +OPERATION_OVERRIDE_THROTTLE_SCALE = 25 +OPERATION_OVERRIDE_THROTTLE = 26 +OPERATION_OVERRIDE_VTX_POWER = 27 +OPERATION_OVERRIDE_VTX_BAND = 28 +OPERATION_OVERRIDE_VTX_CHANNEL = 29 + +// Math +OPERATION_MIN = 30 +OPERATION_MAX = 31 +OPERATION_ABS = 32 +OPERATION_SIN = 35 +OPERATION_COS = 36 +OPERATION_TAN = 37 +OPERATION_MAP_INPUT = 38 +OPERATION_MAP_OUTPUT = 39 +``` + +## Flight Parameter Values + +From firmware (`logic_condition.c`): + +```c +LOGIC_CONDITION_OPERAND_FLIGHT_ARM_TIMER = 0 +LOGIC_CONDITION_OPERAND_FLIGHT_HOME_DISTANCE = 1 +LOGIC_CONDITION_OPERAND_FLIGHT_TRIP_DISTANCE = 2 +LOGIC_CONDITION_OPERAND_FLIGHT_RSSI = 3 +LOGIC_CONDITION_OPERAND_FLIGHT_VBAT = 4 +LOGIC_CONDITION_OPERAND_FLIGHT_CELL_VOLTAGE = 5 +LOGIC_CONDITION_OPERAND_FLIGHT_CURRENT = 6 +LOGIC_CONDITION_OPERAND_FLIGHT_MAH_DRAWN = 7 +LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS = 9 +LOGIC_CONDITION_OPERAND_FLIGHT_GROUND_SPEED = 11 +LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE = 12 +// ... and more +``` + +## Usage After Implementation + +Once these files are created: + +### 1. Analyzer Auto-Updates +```javascript +// analyzer.js automatically picks up new properties +const apiDefinitions = require('./../api/definitions/index.js'); +this.inavAPI = this.buildAPIStructure(apiDefinitions); +// No code changes needed! + +``` + +### 2. Decompiler Auto-Updates +```javascript +// decompiler.js automatically maps operands +this.operandToProperty = this.buildOperandMapping(apiDefinitions); +// Decompiles FLIGHT(40) → inav.flight.compassHeading automatically + +``` + +### 3. TypeScript Auto-Generation +```javascript +// types.js generates Monaco definitions +const dts = generateTypeDefinitions(apiDefinitions); +// IntelliSense shows all properties automatically + +``` + +### 4. Adding New Properties +To add `flight.newSensor`: + +1. Edit `flight.js`: +```javascript +newSensor: { + type: 'number', + desc: 'New sensor value', + inavOperand: { type: 2, value: 50 } +} + +``` + +2. Done! Everything updates automatically: + - ✅ Analyzer validates `flight.newSensor` + - ✅ Decompiler recognizes operand 50 + - ✅ TypeScript shows in autocomplete + - ✅ Code generator uses correct operand + +## Verification Checklist + +After creating these files: + +- [ ] All files exist in `js/transpiler/api/definitions/` +- [ ] `index.js` exports all definitions +- [ ] Each property has `inavOperand` or `inavOperation` +- [ ] Operand values match INAV firmware +- [ ] Range values are correct +- [ ] Readonly flags are correct +- [ ] Nested objects are properly structured +- [ ] `analyzer.js` imports and uses definitions +- [ ] `decompiler.js` imports and uses definitions +- [ ] `types.js` generates TypeScript correctly +- [ ] Test transpilation works +- [ ] Test decompilation works +- [ ] Test Monaco autocomplete works + +## References + +- **INAV Source**: https://github.com/iNavFlight/inav + - `src/main/programming/logic_condition.c` + - `src/main/programming/logic_condition.h` + - `src/main/programming/global_variables.c` + - `src/main/programming/pid.c` +- **Documentation**: Programming Framework.md +- **Configurator**: programming.js, programming.html + +## Benefits + +✅ **Single Source of Truth**: One place to edit +✅ **Automatic Updates**: Add property → everything works +✅ **Type Safety**: Proper TypeScript generation +✅ **Maintainability**: Easy to keep in sync with INAV +✅ **Documentation**: Self-documenting API +✅ **Validation**: Comprehensive range/type checking + +## Maintenance + +When adding new properties or updating existing ones, see `api_maintenance_guide.md` for the complete workflow and best practices. diff --git a/docs/javascript_programming/api_maintenance_guide.md b/docs/javascript_programming/api_maintenance_guide.md new file mode 100644 index 00000000000..85804c1e8d3 --- /dev/null +++ b/docs/javascript_programming/api_maintenance_guide.md @@ -0,0 +1,409 @@ +# API Definition Maintenance Guide + +## Single Source of Truth + +All INAV JavaScript API definitions are centralized in: +``` +js/transpiler/api/definitions/ +``` + +**When adding new INAV features, you only need to edit files in this directory.** + +## Directory Structure + +``` +js/transpiler/api/definitions/ +├── index.js # Exports all definitions +├── events.js # Event handlers (timer, whenChanged, etc.) +├── flight.js # Flight parameters (read-only) +├── gvar.js # Global variables (read/write) +├── helpers.js # Math & utility functions +├── override.js # Override settings (writable) +├── pid.js # Programming PID controllers +├── rc.js # RC channels (read/write) +└── waypoint.js # Waypoint navigation +``` + +## Definition Format + +Each definition file exports objects following this structure: + +```javascript +module.exports = { + propertyName: { + type: 'number' | 'boolean' | 'string' | 'object' | 'function', + desc: 'Human-readable description', + unit: 'Unit of measurement (optional)', + readonly: true | false, + range: [min, max], // Optional: valid value range + inavOperand: { + type: 2, // OPERAND_TYPE constant + value: 1 // Operand value for INAV + }, + inavOperation: 27, // Optional: OPERATION constant + }, + + // Nested objects + nestedObject: { + type: 'object', + desc: 'Description', + properties: { + subProperty: { + type: 'number', + desc: 'Sub-property description', + // ... same structure as above + } + } + } +}; + +``` + +## What Uses These Definitions + +### 1. **Semantic Analyzer** (`analyzer.js`) +- Validates property access +- Checks writable properties +- Validates value ranges +- **Auto-updates when definitions change** + +### 2. **Type Definitions** (`types.js`) +- Generates TypeScript definitions for Monaco Editor +- Provides IntelliSense autocomplete +- **Auto-generates from definitions** + +### 3. **Code Generator** (`codegen.js`) +- Maps JavaScript to INAV operands +- Uses `inavOperand` and `inavOperation` fields +- **Requires manual update for new operations** + +### 4. **Decompiler** (`decompiler.js`) +- Reverse maps INAV to JavaScript +- Uses operand mappings from API definitions +- **Auto-updates when definitions change** + +## Adding a New Property + +### Example: Adding `flight.compassHeading` + +**1. Edit `js/transpiler/api/definitions/flight.js`:** + +```javascript +module.exports = { + // ... existing properties ... + + compassHeading: { + type: 'number', + unit: '°', + desc: 'Compass heading in degrees (0-359)', + readonly: true, + range: [0, 359], + inavOperand: { + type: 2, // OPERAND_TYPE.FLIGHT + value: 40 // FLIGHT_PARAM.COMPASS_HEADING + } + } +}; + +``` + +**2. Update `inav_constants.js` (if needed):** + +Only if adding a completely new INAV firmware feature: + +```javascript +const FLIGHT_PARAM = { + // ... existing params ... + COMPASS_HEADING: 40 +}; + +const FLIGHT_PARAM_NAMES = { + // ... existing names ... + [FLIGHT_PARAM.COMPASS_HEADING]: 'compassHeading' +}; + +``` + +**3. That's it!** + +The following automatically update: +- ✅ Semantic analyzer validates `flight.compassHeading` +- ✅ TypeScript definitions show in autocomplete +- ✅ Range checking works automatically +- ✅ Decompiler recognizes the property + +## Adding a New Writable Property + +### Example: Adding `override.vtx.frequency` + +**1. Edit `js/transpiler/api/definitions/override.js`:** + +```javascript +module.exports = { + // ... existing properties ... + + vtx: { + type: 'object', + desc: 'VTX control', + properties: { + power: { /* ... */ }, + band: { /* ... */ }, + channel: { /* ... */ }, + + // NEW PROPERTY + frequency: { + type: 'number', + unit: 'MHz', + desc: 'VTX frequency in MHz', + readonly: false, // Writable! + range: [5000, 6000], + inavOperation: 50 // New operation code + } + } + } +}; + +``` + +**2. Update `inav_constants.js`:** + +```javascript +const OPERATION = { + // ... existing operations ... + OVERRIDE_VTX_FREQUENCY: 50 +}; + +``` + +**3. Update `codegen.js` (manual):** + +Add code generation logic: + +```javascript +// In generateAction() method +if (stmt.target === 'inav.override.vtx.frequency') { + return this.pushLogicCommand( + OPERATION.OVERRIDE_VTX_FREQUENCY, + { type: OPERAND_TYPE.VALUE, value: 0 }, + this.valueOperand(stmt.value), + activatorId + ); +} + +``` + +## Adding a New Top-Level API Object + +### Example: Adding `inav.sensors` + +**1. Create `js/transpiler/api/definitions/sensors.js`:** + +```javascript +'use strict'; + +module.exports = { + acc: { + type: 'boolean', + desc: 'Accelerometer sensor detected', + readonly: true, + inavOperand: { + type: 2, // FLIGHT + value: 50 // New param ID + } + }, + + mag: { + type: 'boolean', + desc: 'Magnetometer sensor detected', + readonly: true, + inavOperand: { + type: 2, + value: 51 + } + } + + // ... more sensors +}; + +``` + +**2. Update `js/transpiler/api/definitions/index.js`:** + +```javascript +'use strict'; + +module.exports = { + flight: require('./inav.flight.js'), + override: require('./inav.override.js'), + rc: require('./rc.js'), + gvar: require('./gvar.js'), + waypoint: require('./inav.waypoint.js'), + pid: require('./pid.js'), + helpers: require('./helpers.js'), + events: require('./events.js'), + sensors: require('./sensors.js') // ADD THIS +}; + +``` + +**3. Update TypeScript types in `types.js` generation:** + +The type generator should automatically pick it up, but verify: + +```javascript +// In generateTypeDefinitions() +dts += generateInterfaceFromDefinition('sensors', apiDefinitions.sensors); + +``` + +## Validation Checklist + +When adding/modifying API definitions: + +- [ ] Property has correct `type` +- [ ] Has descriptive `desc` +- [ ] Has `unit` if applicable +- [ ] `readonly` flag is correct +- [ ] `range` is specified for numeric values +- [ ] `inavOperand` maps to correct INAV constant +- [ ] `inavOperation` specified for writable properties +- [ ] Updated `index.js` if new file +- [ ] Updated `inav_constants.js` if new INAV feature +- [ ] Updated `codegen.js` for new writable properties +- [ ] Tested with sample code +- [ ] TypeScript definitions generate correctly + +## Testing Changes + +After modifying definitions: + +```javascript +// 1. Test semantic analysis +const code = ` +if (sensors.acc) { + // ... +} +`; + +const transpiler = new Transpiler(); +const result = transpiler.transpile(code); +// Should not have errors + +// 2. Test type generation +const { generateTypeDefinitions } = require('./api/types.js'); +const dts = generateTypeDefinitions(apiDefinitions); +// Should include new properties + +// 3. Test in Monaco Editor +// Open configurator, verify autocomplete shows new properties + +``` + +## Common Mistakes + +### ❌ Wrong: Editing analyzer.js directly +```javascript +// DON'T DO THIS in analyzer.js: +this.inavAPI = { + 'flight': { + properties: ['homeDistance', 'newProperty'] // Hard-coded! + } +}; + +``` + +### ✅ Right: Edit definition file +```javascript +// DO THIS in inav.flight.js: +module.exports = { + newProperty: { + type: 'number', + desc: 'New property', + // ... + } +}; + +``` + +### ❌ Wrong: Duplicating definitions +```javascript +// DON'T duplicate in multiple files +// decompiler.js - NO! +const FLIGHT_PARAMS = { + 1: 'homeDistance' +}; + +// analyzer.js - NO! +properties: ['homeDistance'] + +``` + +### ✅ Right: Use centralized definitions +```javascript +// DO THIS - import from definitions +const apiDefinitions = require('./../api/definitions/index.js'); +const flightDef = apiDefinitions.flight; + +``` + +## File Dependencies + +``` +js/transpiler/api/definitions/ + ├── index.js + ├── events.js + ├── flight.js + ├── gvar.js + ├── helpers.js + ├── override.js + ├── pid.js + ├── rc.js + └── waypoint.js + ↓ + Used by: + ├── analyzer.js (validation) + ├── types.js (TypeScript generation) + ├── codegen.js (code generation) + └── decompiler.js (via inav_constants.js) +``` + +## Migration from Hardcoded Values + +If you find hardcoded API definitions elsewhere in the code: + +1. **Identify the hardcoded values** +2. **Check if they exist in `api/definitions/`** +3. **If not, add them to appropriate definition file** +4. **Replace hardcoded values with imports** +5. **Test thoroughly** +6. **Remove old hardcoded definitions** + +Example: + +```javascript +// Before (hardcoded in analyzer.js) +this.inavAPI = { + 'flight': { + properties: ['homeDistance', 'altitude'] + } +}; + +// After (using definitions) +const apiDefinitions = require('./../api/definitions/index.js'); +this.inavAPI = this.buildAPIStructure(apiDefinitions); + +``` + +## Summary + +**One Rule: Edit only `js/transpiler/api/definitions/*.js`** + +Everything else updates automatically (except `codegen.js` which requires manual updates for new operations). + +This ensures: +- ✅ Single source of truth +- ✅ No duplication +- ✅ Easy maintenance +- ✅ Fewer bugs +- ✅ Automatic validation +- ✅ Automatic type generation diff --git a/docs/javascript_programming/example_override_rc.png b/docs/javascript_programming/example_override_rc.png new file mode 100644 index 00000000000..be01a598c63 Binary files /dev/null and b/docs/javascript_programming/example_override_rc.png differ diff --git a/docs/javascript_programming/example_vtx_power.png b/docs/javascript_programming/example_vtx_power.png new file mode 100644 index 00000000000..3dc72c394f5 Binary files /dev/null and b/docs/javascript_programming/example_vtx_power.png differ diff --git a/docs/javascript_programming/implementation_summary.md b/docs/javascript_programming/implementation_summary.md new file mode 100644 index 00000000000..2f6e532ce8b --- /dev/null +++ b/docs/javascript_programming/implementation_summary.md @@ -0,0 +1,217 @@ +# INAV JavaScript Transpiler - Technical Overview + +## Overview + +The INAV JavaScript Transpiler is a bidirectional JavaScript ↔ INAV Logic Conditions system that allows users to write flight controller logic in JavaScript instead of raw logic condition commands. + +**System Components:** +- **Transpiler**: JavaScript → INAV Logic Conditions +- **Decompiler**: INAV Logic Conditions → JavaScript +- **Semantic Analysis**: Full validation and error checking +- **Parser**: Production-grade using Acorn +- **Code Generation**: Optimized INAV CLI commands +- **Integration**: Monaco Editor with IntelliSense + +## Architecture + +``` +┌─────────────────────────────────────────────────────────────┐ +│ User Interface │ +│ (Monaco Editor + Event Handlers) │ +└───────────────┬───────────────────────┬─────────────────────┘ + │ │ + │ Transpile │ Load from FC + ▼ ▼ +┌───────────────────────────┐ ┌──────────────────────────────┐ +│ TRANSPILER │ │ DECOMPILER │ +│ (JavaScript → INAV) │ │ (INAV → JavaScript) │ +└───────────────────────────┘ └──────────────────────────────┘ + │ │ + ▼ ▼ +┌──────────────────┐ ┌──────────────────┐ +│ Parser (Acorn) │ │ Analyze & Group │ +└────────┬─────────┘ └────────┬─────────┘ + │ │ + ▼ ▼ +┌──────────────────┐ ┌──────────────────┐ +│ Semantic Analyzer│ │ Generate Code │ +└────────┬─────────┘ └────────┬─────────┘ + │ │ + ▼ │ +┌──────────────────┐ │ +│ Optimizer │ │ +└────────┬─────────┘ │ + │ │ + ▼ │ +┌──────────────────┐ │ +│ Code Generator │ │ +└────────┬─────────┘ │ + │ │ + ▼ ▼ +┌─────────────────────────────────────────────────────────────┐ +│ INAV Logic Conditions │ +│ (Flight Controller MSP) │ +└─────────────────────────────────────────────────────────────┘ +``` + +## Key Features + +### Transpiler (JavaScript → INAV) + +✅ **Robust Parsing** +- Uses Acorn for production-grade JavaScript parsing +- Handles all edge cases correctly +- Proper error messages with line/column numbers + +✅ **Comprehensive Validation** +- Variable scope checking +- Property access validation +- Range checking (gvar indices, heading values, etc.) +- Dead code detection +- Conflict detection +- Uninitialized variable detection + +✅ **Smart Code Generation** +- Optimized logic condition output +- Efficient operand usage +- Proper activator chaining + +✅ **Developer Experience** +- Monaco Editor integration +- Real-time syntax highlighting +- IntelliSense autocomplete +- Lint mode for fast feedback +- Detailed error messages with code context + +### Decompiler (INAV → JavaScript) + +✅ **Intelligent Reconstruction** +- Pattern recognition for handler types +- Smart grouping of related conditions +- Preserves logical structure + +✅ **Comprehensive Coverage** +- All INAV operations supported +- Flight parameters +- Global variables +- Override operations +- Arithmetic operations + +✅ **Warning System** +- Alerts about lossy conversions +- Flags unsupported features +- Suggests manual review where needed + +✅ **Documentation** +- Inline comments in generated code +- Warning annotations +- Original logic condition references + +## Usage Examples + +### Example 1: Transpilation + +**Input JavaScript:** +```javascript + +if (inav.flight.homeDistance > 100) { + inav.override.vtx.power = 3; +} + +``` + +**Output INAV Commands:** +``` +logic 0 1 -1 2 2 1 0 100 0 +logic 1 1 0 27 0 0 0 3 0 +``` + +### Example 2: Decompilation + +**Input INAV Commands:** +``` +logic 0 1 -1 2 2 5 0 350 0 +logic 1 1 0 25 0 0 0 50 0 +``` + +**Output JavaScript:** +```javascript + +if (inav.flight.cellVoltage < 350) { + inav.override.throttleScale = 50; +} + +``` + +### Example 3: Full Round-Trip + +**Original Code:** +```javascript +on.arm({ delay: 1 }, () => { + inav.gvar[0] = inav.flight.yaw; +}); + +if (inav.flight.homeDistance > 500) { + inav.override.vtx.power = 4; + inav.override.throttleScale = 75; +} + +``` + +**Transpiled → Saved to FC → Loaded from FC:** +```javascript +// INAV Logic Conditions - Decompiled to JavaScript +// Note: Comments, variable names, and some structure may be lost + + +on.arm({ delay: 1 }, () => { + inav.gvar[0] = inav.flight.yaw; +}); + +if (inav.flight.homeDistance > 500) { + inav.override.vtx.power = 4; + inav.override.throttleScale = 75; +} + +``` + +## Known Limitations + +### Transpiler + +1. **Subset of JavaScript**: Only supports INAV-specific syntax +2. **No complex expressions**: Nested function calls not supported +3. **Limited control flow**: Only if/else supported, no loops or complex functions + +### Decompiler + +1. **Lossy conversion**: Comments and variable names lost +2. **Structure changes**: Optimizations may alter original code +3. **Complex conditions**: May not perfectly reconstruct nested logic +4. **LC references**: References between logic conditions flagged for review + +## Testing + +### Running Tests + +```bash +npm test parser.test.js +npm test analyzer.test.js +npm test decompiler.test.js +npm test integration.test.js +``` + +### Test Coverage + +- Parser: Empty input, syntax errors, edge cases +- Analyzer: Validation, dead code, conflicts, ranges +- Transpiler: Full pipeline, error handling +- Decompiler: All operations, grouping, warnings +- Integration: Monaco editor, UI events, MSP communication + +## Further Documentation + +- **User Guide**: See `JAVASCRIPT_PROGRAMMING_GUIDE.md` for usage patterns +- **API Reference**: See `api_definitions_summary.md` for complete API +- **Maintenance**: See `api_maintenance_guide.md` for adding new features +- **Timer/WhenChanged**: See `TIMER_WHENCHANGED_EXAMPLES.md` for advanced patterns diff --git a/docs/javascript_programming/index.md b/docs/javascript_programming/index.md new file mode 100644 index 00000000000..ce8ad26dd27 --- /dev/null +++ b/docs/javascript_programming/index.md @@ -0,0 +1,289 @@ +# INAV JavaScript Programming Documentation + +Complete documentation for the INAV JavaScript transpiler that allows programming flight controller logic conditions in JavaScript. + +## 📚 Table of Contents + +- [Quick Start](#quick-start) +- [User Guides](#user-guides) +- [Developer Documentation](#developer-documentation) +- [Features](#features) + +--- + +## Quick Start + +The INAV JavaScript transpiler converts JavaScript code into INAV logic conditions, enabling you to program flight controller behavior using familiar JavaScript syntax instead of raw logic condition commands. + +### Features at a Glance + +**✨ Modern Development Experience** + +![IntelliSense Autocomplete](intellisense.png) +*Real-time autocomplete with type information and documentation* + +![Helpful Warnings](warnings.png) +*Clear error messages with line numbers and suggestions* + +### Basic Example + +```javascript + +// Increase VTX power when far from home +if (inav.flight.homeDistance > 500) { + inav.override.vtx.power = 4; +} + +``` + +![VTX Power Control Example](example_vtx_power.png) +*Complete example showing VTX power control based on distance* + +### RC Channel Override Example + +![RC Override Example](example_override_rc.png) +*Using RC channels to control behavior* + +--- + +## User Guides + +### 📖 [JavaScript Programming Guide](JAVASCRIPT_PROGRAMMING_GUIDE.md) +**Start here if you're new to INAV JavaScript programming** + +Quick reference covering: +- Pattern guide: `if`, `edge()`, `sticky()`, `delay()`, `timer()`, `whenChanged()` +- Common patterns (initialization, event counting, debouncing) +- Key differences between patterns +- Available objects and APIs +- Debugging techniques + +### 🔧 [Operations Reference](OPERATIONS_REFERENCE.md) +**Complete reference for all supported operations** + +Comprehensive guide to all INAV logic condition operations: +- ✅ Arithmetic: `+`, `-`, `*`, `/`, `%` +- ✅ Comparisons: `===`, `>`, `<`, `approxEqual()` +- ✅ Logical: `&&`, `||`, `!`, `xor()`, `nand()`, `nor()` +- ✅ Math: `Math.min()`, `Math.max()`, `Math.sin()`, `Math.cos()`, `Math.tan()`, `Math.abs()` +- ✅ Scaling: `mapInput()`, `mapOutput()` +- ✅ Flow control: `edge()`, `sticky()`, `delay()`, `timer()`, `whenChanged()` +- ✅ Variables: `gvar[0-7]`, `let`, `var` +- ✅ Overrides: `override.vtx.*`, `override.throttle`, `override.armSafety`, etc. +- ✅ RC channel states: `rc[n].low`, `rc[n].mid`, `rc[n].high`, `rc[n].value` + +Includes usage examples and notes for each operation. + +### ⏱️ [Timer and WhenChanged Examples](TIMER_WHENCHANGED_EXAMPLES.md) +**Practical examples for time-based and change-detection patterns** + +Examples covering: +- Timer patterns (blinking, cycling, periodic actions) +- Change detection (RSSI monitoring, altitude tracking) +- Combined patterns with multiple conditions +- Real-world use cases + +--- + +## Developer Documentation + +### 🏗️ [Technical Implementation Overview](implementation_summary.md) +**Architecture and design of the transpiler system** + +Covers: +- System architecture and component interaction +- Transpiler pipeline (Parser → Analyzer → Optimizer → Codegen) +- Decompiler pattern recognition +- Key features and capabilities +- Integration with Monaco Editor + +### 🧪 [Testing Guide](TESTING_GUIDE.md) +**How to test changes to the transpiler** + +Essential reading before making changes: +- Which components need updates (parser, analyzer, codegen, decompiler) +- Round-trip testing template (JavaScript → CLI → JavaScript) +- Common issues and solutions +- Verification checklist +- Step-by-step example of adding a new operation + +**Always use round-trip testing when modifying the transpiler!** + +### 🔌 [API Definitions Summary](api_definitions_summary.md) +**Structure of the INAV API definitions** + +Documents the API definition system: +- Available API objects (`flight`, `override`, `rc`, `gvar`, `waypoint`, etc.) +- Property types and metadata +- INAV operand mappings +- How definitions drive IntelliSense + +### 🛠️ [API Maintenance Guide](api_maintenance_guide.md) +**How to add, modify, or maintain API definitions** + +Step-by-step instructions for: +- Adding new properties to existing APIs +- Creating new API objects +- Updating operand mappings +- Testing API changes +- Common pitfalls + +### ⏲️ [Timer and WhenChanged Implementation](TIMER_WHENCHANGED_IMPLEMENTATION.md) +**Technical details of TIMER and DELTA operations** + +Implementation guide for: +- How TIMER operation works (operations 49) +- How DELTA operation works (operation 50) +- AST representation +- Code generation strategy +- Decompiler pattern recognition + +### ⚙️ [Generate Constants README](GENERATE_CONSTANTS_README.md) +**How to regenerate INAV constants from firmware** + +Instructions for: +- Extracting operation codes from firmware +- Regenerating `inav_constants.js` +- Keeping constants in sync with firmware +- What to do when firmware adds new operations + +--- + +## Features + +### Bidirectional Translation + +- **Transpiler**: JavaScript → INAV Logic Conditions +- **Decompiler**: INAV Logic Conditions → JavaScript +- **Round-trip capable**: Code survives transpile/decompile cycles + +### Development Experience + +- **Monaco Editor Integration**: Full-featured code editor +- **IntelliSense**: Context-aware autocomplete with documentation +- **Real-time Validation**: Immediate feedback on syntax errors +- **Syntax Highlighting**: JavaScript syntax with INAV-specific extensions +- **Error Messages**: Clear, actionable error messages with line numbers + +### Language Support + +**All INAV Operations Supported:** +- Arithmetic operations +- Comparison and logical operations +- Math functions (trigonometry, min/max) +- Flow control (edge detection, sticky conditions, delays, timers) +- Variable management (global variables, let/var) +- RC channel access and state detection +- Flight parameter overrides +- Waypoint navigation + +**JavaScript Features:** +- Namespaced API access: `inav.flight.*`, `inav.override.*`, `inav.events.*` +- `let`/`const` variables: compile-time constant substitution +- `var` variables: allocated to global variables +- Ternary operator: `condition ? value1 : value2` +- Arrow functions: `() => condition` +- Object property access: `inav.flight.altitude`, `inav.rc[0].value` +- Binary expressions: `+`, `-`, `*`, `/`, `%` +- Comparison operators: `>`, `<`, `===` +- Logical operators: `&&`, `||`, `!` +- Math methods: `Math.min()`, `Math.max()`, `Math.sin()`, etc. +- Flight mode detection: `inav.flight.mode.poshold`, `inav.flight.mode.rth`, etc. +- PID controller outputs: `inav.pid[0-3].output` + +### Validation + +- Property access validation against API definitions +- Range checking (gvar indices, heading values, etc.) +- Type checking for function arguments +- Dead code detection +- Uninitialized variable detection +- Helpful suggestions for common mistakes + +### Optimization + +- Common Subexpression Elimination (CSE) +- Efficient operand usage +- Optimized GVAR_INC/GVAR_DEC generation +- Minimal logic condition count + +--- + +## File Organization + +``` +js/transpiler/ +├── docs/ # Documentation (this directory) +│ ├── index.md # This file - documentation index +│ ├── JAVASCRIPT_PROGRAMMING_GUIDE.md # User guide +│ ├── OPERATIONS_REFERENCE.md # All operations +│ ├── TIMER_WHENCHANGED_EXAMPLES.md # Timer/change examples +│ ├── implementation_summary.md # Technical overview +│ ├── TESTING_GUIDE.md # Testing workflow +│ ├── api_definitions_summary.md # API structure +│ ├── api_maintenance_guide.md # API maintenance +│ ├── GENERATE_CONSTANTS_README.md # Constants generation +│ ├── TIMER_WHENCHANGED_IMPLEMENTATION.md # Implementation details +│ ├── intellisense.png # Screenshot: autocomplete +│ ├── warnings.png # Screenshot: errors +│ ├── example_vtx_power.png # Screenshot: VTX example +│ └── example_override_rc.png # Screenshot: RC example +├── api/ # API definitions +│ └── definitions/ # INAV API object definitions +├── editor/ # Monaco editor integration +│ ├── monaco_setup.js # Editor configuration +│ ├── intellisense.js # Autocomplete provider +│ └── diagnostics.js # Real-time validation +├── transpiler/ # Core transpiler +│ ├── index.js # Main transpiler entry point +│ ├── parser.js # JavaScript parser (Acorn wrapper) +│ ├── analyzer.js # Semantic analysis +│ ├── optimizer.js # Code optimization (CSE) +│ ├── codegen.js # INAV command generation +│ ├── decompiler.js # INAV → JavaScript +│ ├── inav_constants.js # Operation codes and types +│ ├── variable_handler.js # let/var variable management +│ ├── arrow_function_helper.js # Arrow function utilities +│ └── error_handler.js # Error collection +└── tools/ # Build tools + └── generate-constants.js # Extract constants from firmware +``` + +--- + +## Getting Help + +### Common Questions + +**Q: Which pattern should I use?** +- Use `if` for continuous control (checking every cycle) +- Use `edge()` for one-time actions when condition becomes true +- Use `sticky()` for latching conditions with hysteresis +- Use `delay()` for actions that need sustained conditions +- Use `timer()` for periodic toggling +- Use `whenChanged()` for detecting value changes + +See the [JavaScript Programming Guide](JAVASCRIPT_PROGRAMMING_GUIDE.md) for detailed examples. + +**Q: How do I debug my code?** +Use global variables to track state and view them in the OSD or configurator. See the debugging section in the [JavaScript Programming Guide](JAVASCRIPT_PROGRAMMING_GUIDE.md). + +**Q: What operations are supported?** +All INAV logic condition operations! See [Operations Reference](OPERATIONS_REFERENCE.md) for the complete list. + +**Q: How do I contribute?** +Read the [Testing Guide](TESTING_GUIDE.md) to understand the testing workflow, then make your changes ensuring all 4 components (parser, analyzer, codegen, decompiler) are updated and round-trip tested. + +--- + +## Version History + +**2025-11-25**: Complete implementation +- All INAV logic condition operations now supported +- RC channel state detection (LOW/MID/HIGH) +- XOR/NAND/NOR logical operations +- APPROX_EQUAL comparison +- MAP_INPUT/MAP_OUTPUT scaling +- Comprehensive documentation + +**Last Updated**: 2025-11-25 diff --git a/docs/javascript_programming/intellisense.png b/docs/javascript_programming/intellisense.png new file mode 100644 index 00000000000..ce3b6854380 Binary files /dev/null and b/docs/javascript_programming/intellisense.png differ diff --git a/docs/javascript_programming/warnings.png b/docs/javascript_programming/warnings.png new file mode 100644 index 00000000000..27400d82d1b Binary files /dev/null and b/docs/javascript_programming/warnings.png differ diff --git a/docs/policies/NEW_HARDWARE_POLICY.md b/docs/policies/NEW_HARDWARE_POLICY.md index f11601b8380..bd4bddedf05 100644 --- a/docs/policies/NEW_HARDWARE_POLICY.md +++ b/docs/policies/NEW_HARDWARE_POLICY.md @@ -68,3 +68,8 @@ If one of the core developers has the hardware in possession they may opt in and 1. Requester is advised to open a feature request to add support for certain hardware to INAV by following [this link](https://github.com/iNavFlight/inav/issues/new/choose) 2. After opening a feature request, Requester is advised to contact the core development team by [email](mailto:coredev@inavflight.com) mentioning the open feature request and communicate with developer team via email to arrange hardware and specifications delivery. + + +## See also +[Hardware Design Guidelines](https://github.com/iNavFlight/inav/wiki/Hardware-Design-Guidelines) + diff --git a/lib/main/MAVLink/checksum.h b/lib/main/MAVLink/checksum.h index 0a899a60771..d0d011d1ea1 100755 --- a/lib/main/MAVLink/checksum.h +++ b/lib/main/MAVLink/checksum.h @@ -45,7 +45,7 @@ static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) /** - * @brief Initiliaze the buffer for the MCRF4XX CRC16 + * @brief Initialize the buffer for the MCRF4XX CRC16 * * @param crcAccum the 16 bit MCRF4XX CRC16 */ diff --git a/lib/main/MAVLink/common/common.h b/lib/main/MAVLink/common/common.h index ec5fc7218d2..4e47afb03ee 100755 --- a/lib/main/MAVLink/common/common.h +++ b/lib/main/MAVLink/common/common.h @@ -10,8 +10,7 @@ #error Wrong include order: MAVLINK_COMMON.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call. #endif -#undef MAVLINK_THIS_XML_IDX -#define MAVLINK_THIS_XML_IDX 0 +#define MAVLINK_COMMON_XML_HASH 1337309235272790416 #ifdef __cplusplus extern "C" { @@ -24,7 +23,7 @@ extern "C" { #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {{0, 50, 9, 9, 0, 0, 0}, {1, 124, 31, 31, 0, 0, 0}, {2, 137, 12, 12, 0, 0, 0}, {4, 237, 14, 14, 3, 12, 13}, {5, 217, 28, 28, 1, 0, 0}, {6, 104, 3, 3, 0, 0, 0}, {7, 119, 32, 32, 0, 0, 0}, {8, 117, 36, 36, 0, 0, 0}, {11, 89, 6, 6, 1, 4, 0}, {19, 137, 24, 24, 3, 4, 5}, {20, 214, 20, 20, 3, 2, 3}, {21, 159, 2, 2, 3, 0, 1}, {22, 220, 25, 25, 0, 0, 0}, {23, 168, 23, 23, 3, 4, 5}, {24, 24, 30, 52, 0, 0, 0}, {25, 23, 101, 101, 0, 0, 0}, {26, 170, 22, 24, 0, 0, 0}, {27, 144, 26, 29, 0, 0, 0}, {28, 67, 16, 16, 0, 0, 0}, {29, 115, 14, 16, 0, 0, 0}, {30, 39, 28, 28, 0, 0, 0}, {31, 246, 32, 48, 0, 0, 0}, {32, 185, 28, 28, 0, 0, 0}, {33, 104, 28, 28, 0, 0, 0}, {34, 237, 22, 22, 0, 0, 0}, {35, 244, 22, 22, 0, 0, 0}, {36, 222, 21, 37, 0, 0, 0}, {37, 212, 6, 7, 3, 4, 5}, {38, 9, 6, 7, 3, 4, 5}, {39, 254, 37, 38, 3, 32, 33}, {40, 230, 4, 5, 3, 2, 3}, {41, 28, 4, 4, 3, 2, 3}, {42, 28, 2, 2, 0, 0, 0}, {43, 132, 2, 3, 3, 0, 1}, {44, 221, 4, 5, 3, 2, 3}, {45, 232, 2, 3, 3, 0, 1}, {46, 11, 2, 2, 0, 0, 0}, {47, 153, 3, 4, 3, 0, 1}, {48, 41, 13, 21, 1, 12, 0}, {49, 39, 12, 20, 0, 0, 0}, {50, 78, 37, 37, 3, 18, 19}, {51, 196, 4, 5, 3, 2, 3}, {52, 132, 7, 7, 0, 0, 0}, {54, 15, 27, 27, 3, 24, 25}, {55, 3, 25, 25, 0, 0, 0}, {61, 167, 72, 72, 0, 0, 0}, {62, 183, 26, 26, 0, 0, 0}, {63, 119, 181, 181, 0, 0, 0}, {64, 191, 225, 225, 0, 0, 0}, {65, 118, 42, 42, 0, 0, 0}, {66, 148, 6, 6, 3, 2, 3}, {67, 21, 4, 4, 0, 0, 0}, {69, 243, 11, 11, 1, 10, 0}, {70, 124, 18, 38, 3, 16, 17}, {73, 38, 37, 38, 3, 32, 33}, {74, 20, 20, 20, 0, 0, 0}, {75, 158, 35, 35, 3, 30, 31}, {76, 152, 33, 33, 3, 30, 31}, {77, 143, 3, 10, 3, 8, 9}, {80, 14, 4, 4, 3, 2, 3}, {81, 106, 22, 22, 0, 0, 0}, {82, 49, 39, 39, 3, 36, 37}, {83, 22, 37, 37, 0, 0, 0}, {84, 143, 53, 53, 3, 50, 51}, {85, 140, 51, 51, 0, 0, 0}, {86, 5, 53, 53, 3, 50, 51}, {87, 150, 51, 51, 0, 0, 0}, {89, 231, 28, 28, 0, 0, 0}, {90, 183, 56, 56, 0, 0, 0}, {91, 63, 42, 42, 0, 0, 0}, {92, 54, 33, 33, 0, 0, 0}, {93, 47, 81, 81, 0, 0, 0}, {100, 175, 26, 34, 0, 0, 0}, {101, 102, 32, 117, 0, 0, 0}, {102, 158, 32, 117, 0, 0, 0}, {103, 208, 20, 57, 0, 0, 0}, {104, 56, 32, 116, 0, 0, 0}, {105, 93, 62, 63, 0, 0, 0}, {106, 138, 44, 44, 0, 0, 0}, {107, 108, 64, 65, 0, 0, 0}, {108, 32, 84, 84, 0, 0, 0}, {109, 185, 9, 9, 0, 0, 0}, {110, 84, 254, 254, 3, 1, 2}, {111, 34, 16, 16, 0, 0, 0}, {112, 174, 12, 12, 0, 0, 0}, {113, 124, 36, 39, 0, 0, 0}, {114, 237, 44, 44, 0, 0, 0}, {115, 4, 64, 64, 0, 0, 0}, {116, 76, 22, 24, 0, 0, 0}, {117, 128, 6, 6, 3, 4, 5}, {118, 56, 14, 14, 0, 0, 0}, {119, 116, 12, 12, 3, 10, 11}, {120, 134, 97, 97, 0, 0, 0}, {121, 237, 2, 2, 3, 0, 1}, {122, 203, 2, 2, 3, 0, 1}, {123, 250, 113, 113, 3, 0, 1}, {124, 87, 35, 37, 0, 0, 0}, {125, 203, 6, 6, 0, 0, 0}, {126, 220, 79, 79, 0, 0, 0}, {127, 25, 35, 35, 0, 0, 0}, {128, 226, 35, 35, 0, 0, 0}, {129, 46, 22, 24, 0, 0, 0}, {130, 29, 13, 13, 0, 0, 0}, {131, 223, 255, 255, 0, 0, 0}, {132, 85, 14, 39, 0, 0, 0}, {133, 6, 18, 18, 0, 0, 0}, {134, 229, 43, 43, 0, 0, 0}, {135, 203, 8, 8, 0, 0, 0}, {136, 1, 22, 22, 0, 0, 0}, {137, 195, 14, 16, 0, 0, 0}, {138, 109, 36, 120, 0, 0, 0}, {139, 168, 43, 43, 3, 41, 42}, {140, 181, 41, 41, 0, 0, 0}, {141, 47, 32, 32, 0, 0, 0}, {142, 72, 243, 243, 0, 0, 0}, {143, 131, 14, 16, 0, 0, 0}, {144, 127, 93, 93, 0, 0, 0}, {146, 103, 100, 100, 0, 0, 0}, {147, 154, 36, 54, 0, 0, 0}, {148, 178, 60, 78, 0, 0, 0}, {149, 200, 30, 60, 0, 0, 0}, {162, 189, 8, 9, 0, 0, 0}, {192, 36, 44, 54, 0, 0, 0}, {225, 208, 65, 65, 0, 0, 0}, {230, 163, 42, 42, 0, 0, 0}, {231, 105, 40, 40, 0, 0, 0}, {232, 151, 63, 65, 0, 0, 0}, {233, 35, 182, 182, 0, 0, 0}, {234, 150, 40, 40, 0, 0, 0}, {235, 179, 42, 42, 0, 0, 0}, {241, 90, 32, 32, 0, 0, 0}, {242, 104, 52, 60, 0, 0, 0}, {243, 85, 53, 61, 1, 52, 0}, {244, 95, 6, 6, 0, 0, 0}, {245, 130, 2, 2, 0, 0, 0}, {246, 184, 38, 38, 0, 0, 0}, {247, 81, 19, 19, 0, 0, 0}, {248, 8, 254, 254, 3, 3, 4}, {249, 204, 36, 36, 0, 0, 0}, {250, 49, 30, 30, 0, 0, 0}, {251, 170, 18, 18, 0, 0, 0}, {252, 44, 18, 18, 0, 0, 0}, {253, 83, 51, 54, 0, 0, 0}, {254, 46, 9, 9, 0, 0, 0}, {256, 71, 42, 42, 3, 8, 9}, {257, 131, 9, 9, 0, 0, 0}, {258, 187, 32, 232, 3, 0, 1}, {259, 92, 235, 235, 0, 0, 0}, {260, 146, 5, 13, 0, 0, 0}, {261, 179, 27, 60, 0, 0, 0}, {262, 12, 18, 22, 0, 0, 0}, {263, 133, 255, 255, 0, 0, 0}, {264, 49, 28, 28, 0, 0, 0}, {265, 26, 16, 20, 0, 0, 0}, {266, 193, 255, 255, 3, 2, 3}, {267, 35, 255, 255, 3, 2, 3}, {268, 14, 4, 4, 3, 2, 3}, {269, 109, 213, 213, 0, 0, 0}, {270, 59, 19, 19, 0, 0, 0}, {271, 22, 52, 52, 0, 0, 0}, {275, 126, 31, 31, 0, 0, 0}, {276, 18, 49, 49, 0, 0, 0}, {280, 70, 33, 33, 0, 0, 0}, {281, 48, 13, 13, 0, 0, 0}, {282, 123, 35, 35, 3, 32, 33}, {283, 74, 144, 144, 0, 0, 0}, {284, 99, 32, 32, 3, 30, 31}, {285, 137, 40, 40, 3, 38, 39}, {286, 210, 53, 53, 3, 50, 51}, {287, 1, 23, 23, 3, 20, 21}, {288, 20, 23, 23, 3, 20, 21}, {290, 221, 42, 42, 0, 0, 0}, {291, 10, 57, 57, 0, 0, 0}, {299, 19, 96, 98, 0, 0, 0}, {300, 217, 22, 22, 0, 0, 0}, {301, 243, 58, 58, 0, 0, 0}, {310, 28, 17, 17, 0, 0, 0}, {311, 95, 116, 116, 0, 0, 0}, {320, 243, 20, 20, 3, 2, 3}, {321, 88, 2, 2, 3, 0, 1}, {322, 243, 149, 149, 0, 0, 0}, {323, 78, 147, 147, 3, 0, 1}, {324, 132, 146, 146, 0, 0, 0}, {330, 23, 158, 167, 0, 0, 0}, {331, 91, 230, 232, 0, 0, 0}, {332, 236, 239, 239, 0, 0, 0}, {333, 231, 109, 109, 0, 0, 0}, {334, 72, 10, 10, 0, 0, 0}, {335, 225, 24, 24, 0, 0, 0}, {336, 245, 84, 84, 0, 0, 0}, {339, 199, 5, 5, 0, 0, 0}, {340, 99, 70, 70, 0, 0, 0}, {350, 232, 20, 252, 0, 0, 0}, {360, 11, 25, 25, 0, 0, 0}, {370, 75, 87, 87, 0, 0, 0}, {373, 117, 42, 42, 0, 0, 0}, {375, 251, 140, 140, 0, 0, 0}, {380, 232, 20, 20, 0, 0, 0}, {385, 147, 133, 133, 3, 2, 3}, {390, 156, 238, 238, 0, 0, 0}, {395, 163, 156, 156, 0, 0, 0}, {400, 110, 254, 254, 3, 4, 5}, {401, 183, 6, 6, 3, 4, 5}, {9000, 113, 137, 137, 0, 0, 0}, {9005, 117, 34, 34, 0, 0, 0}, {12900, 114, 44, 44, 3, 0, 1}, {12901, 254, 59, 59, 3, 30, 31}, {12902, 49, 53, 53, 3, 4, 5}, {12903, 249, 46, 46, 3, 0, 1}, {12904, 203, 46, 46, 3, 20, 21}, {12905, 49, 43, 43, 3, 0, 1}, {12915, 62, 254, 254, 3, 0, 1}} +#define MAVLINK_MESSAGE_CRCS {{0, 50, 9, 9, 0, 0, 0}, {1, 124, 31, 43, 0, 0, 0}, {2, 137, 12, 12, 0, 0, 0}, {4, 237, 14, 14, 3, 12, 13}, {5, 217, 28, 28, 1, 0, 0}, {6, 104, 3, 3, 0, 0, 0}, {7, 119, 32, 32, 0, 0, 0}, {8, 117, 36, 36, 0, 0, 0}, {11, 89, 6, 6, 1, 4, 0}, {20, 214, 20, 20, 3, 2, 3}, {21, 159, 2, 2, 3, 0, 1}, {22, 220, 25, 25, 0, 0, 0}, {23, 168, 23, 23, 3, 4, 5}, {24, 24, 30, 52, 0, 0, 0}, {25, 23, 101, 101, 0, 0, 0}, {26, 170, 22, 24, 0, 0, 0}, {27, 144, 26, 29, 0, 0, 0}, {28, 67, 16, 16, 0, 0, 0}, {29, 115, 14, 16, 0, 0, 0}, {30, 39, 28, 28, 0, 0, 0}, {31, 246, 32, 48, 0, 0, 0}, {32, 185, 28, 28, 0, 0, 0}, {33, 104, 28, 28, 0, 0, 0}, {34, 237, 22, 22, 0, 0, 0}, {35, 244, 22, 22, 0, 0, 0}, {36, 222, 21, 37, 0, 0, 0}, {37, 212, 6, 7, 3, 4, 5}, {38, 9, 6, 7, 3, 4, 5}, {39, 254, 37, 38, 3, 32, 33}, {40, 230, 4, 5, 3, 2, 3}, {41, 28, 4, 4, 3, 2, 3}, {42, 28, 2, 18, 0, 0, 0}, {43, 132, 2, 3, 3, 0, 1}, {44, 221, 4, 9, 3, 2, 3}, {45, 232, 2, 3, 3, 0, 1}, {46, 11, 2, 2, 0, 0, 0}, {47, 153, 3, 8, 3, 0, 1}, {48, 41, 13, 21, 1, 12, 0}, {49, 39, 12, 20, 0, 0, 0}, {50, 78, 37, 37, 3, 18, 19}, {51, 196, 4, 5, 3, 2, 3}, {54, 15, 27, 27, 3, 24, 25}, {55, 3, 25, 25, 0, 0, 0}, {61, 167, 72, 72, 0, 0, 0}, {62, 183, 26, 26, 0, 0, 0}, {63, 119, 181, 181, 0, 0, 0}, {64, 191, 225, 225, 0, 0, 0}, {65, 118, 42, 42, 0, 0, 0}, {66, 148, 6, 6, 3, 2, 3}, {67, 21, 4, 4, 0, 0, 0}, {69, 243, 11, 30, 1, 10, 0}, {70, 124, 18, 38, 3, 16, 17}, {73, 38, 37, 38, 3, 32, 33}, {74, 20, 20, 20, 0, 0, 0}, {75, 158, 35, 35, 3, 30, 31}, {76, 152, 33, 33, 3, 30, 31}, {77, 143, 3, 10, 3, 8, 9}, {80, 14, 4, 4, 3, 2, 3}, {81, 106, 22, 22, 0, 0, 0}, {82, 49, 39, 51, 3, 36, 37}, {83, 22, 37, 37, 0, 0, 0}, {84, 143, 53, 53, 3, 50, 51}, {85, 140, 51, 51, 0, 0, 0}, {86, 5, 53, 53, 3, 50, 51}, {87, 150, 51, 51, 0, 0, 0}, {89, 231, 28, 28, 0, 0, 0}, {90, 183, 56, 56, 0, 0, 0}, {91, 63, 42, 42, 0, 0, 0}, {92, 54, 33, 33, 0, 0, 0}, {93, 47, 81, 81, 0, 0, 0}, {100, 175, 26, 34, 0, 0, 0}, {101, 102, 32, 117, 0, 0, 0}, {102, 158, 32, 117, 0, 0, 0}, {103, 208, 20, 57, 0, 0, 0}, {104, 56, 32, 116, 0, 0, 0}, {105, 93, 62, 63, 0, 0, 0}, {106, 138, 44, 44, 0, 0, 0}, {107, 108, 64, 65, 0, 0, 0}, {108, 32, 84, 92, 0, 0, 0}, {109, 185, 9, 9, 0, 0, 0}, {110, 84, 254, 254, 3, 1, 2}, {111, 34, 16, 18, 3, 16, 17}, {112, 174, 12, 12, 0, 0, 0}, {113, 124, 36, 39, 0, 0, 0}, {114, 237, 44, 44, 0, 0, 0}, {115, 4, 64, 64, 0, 0, 0}, {116, 76, 22, 24, 0, 0, 0}, {117, 128, 6, 6, 3, 4, 5}, {118, 56, 14, 14, 0, 0, 0}, {119, 116, 12, 12, 3, 10, 11}, {120, 134, 97, 97, 0, 0, 0}, {121, 237, 2, 2, 3, 0, 1}, {122, 203, 2, 2, 3, 0, 1}, {123, 250, 113, 113, 3, 0, 1}, {124, 87, 35, 57, 0, 0, 0}, {125, 203, 6, 6, 0, 0, 0}, {126, 220, 79, 81, 3, 79, 80}, {127, 25, 35, 35, 0, 0, 0}, {128, 226, 35, 35, 0, 0, 0}, {129, 46, 22, 24, 0, 0, 0}, {130, 29, 13, 13, 0, 0, 0}, {131, 223, 255, 255, 0, 0, 0}, {132, 85, 14, 39, 0, 0, 0}, {133, 6, 18, 18, 0, 0, 0}, {134, 229, 43, 43, 0, 0, 0}, {135, 203, 8, 8, 0, 0, 0}, {136, 1, 22, 22, 0, 0, 0}, {137, 195, 14, 16, 0, 0, 0}, {138, 109, 36, 120, 0, 0, 0}, {139, 168, 43, 43, 3, 41, 42}, {140, 181, 41, 41, 0, 0, 0}, {141, 47, 32, 32, 0, 0, 0}, {142, 72, 243, 243, 0, 0, 0}, {143, 131, 14, 16, 0, 0, 0}, {144, 127, 93, 93, 0, 0, 0}, {146, 103, 100, 100, 0, 0, 0}, {147, 154, 36, 54, 0, 0, 0}, {148, 178, 60, 78, 0, 0, 0}, {149, 200, 30, 60, 0, 0, 0}, {162, 189, 8, 9, 0, 0, 0}, {192, 36, 44, 54, 0, 0, 0}, {225, 208, 65, 73, 0, 0, 0}, {230, 163, 42, 42, 0, 0, 0}, {231, 105, 40, 40, 0, 0, 0}, {232, 151, 63, 65, 0, 0, 0}, {233, 35, 182, 182, 0, 0, 0}, {234, 150, 40, 40, 0, 0, 0}, {235, 179, 42, 42, 0, 0, 0}, {241, 90, 32, 32, 0, 0, 0}, {242, 104, 52, 60, 0, 0, 0}, {243, 85, 53, 61, 1, 52, 0}, {244, 95, 6, 6, 0, 0, 0}, {245, 130, 2, 2, 0, 0, 0}, {246, 184, 38, 38, 0, 0, 0}, {247, 81, 19, 19, 0, 0, 0}, {248, 8, 254, 254, 3, 3, 4}, {249, 204, 36, 36, 0, 0, 0}, {250, 49, 30, 30, 0, 0, 0}, {251, 170, 18, 18, 0, 0, 0}, {252, 44, 18, 18, 0, 0, 0}, {253, 83, 51, 54, 0, 0, 0}, {254, 46, 9, 9, 0, 0, 0}, {256, 71, 42, 42, 3, 8, 9}, {257, 131, 9, 9, 0, 0, 0}, {258, 187, 32, 232, 3, 0, 1}, {259, 92, 235, 237, 0, 0, 0}, {260, 146, 5, 14, 0, 0, 0}, {261, 179, 27, 61, 0, 0, 0}, {262, 12, 18, 23, 0, 0, 0}, {263, 133, 255, 255, 0, 0, 0}, {264, 49, 28, 32, 0, 0, 0}, {265, 26, 16, 20, 0, 0, 0}, {266, 193, 255, 255, 3, 2, 3}, {267, 35, 255, 255, 3, 2, 3}, {268, 14, 4, 4, 3, 2, 3}, {269, 109, 213, 215, 0, 0, 0}, {270, 59, 19, 20, 0, 0, 0}, {271, 22, 52, 53, 0, 0, 0}, {275, 126, 31, 32, 0, 0, 0}, {276, 18, 49, 50, 0, 0, 0}, {277, 62, 30, 30, 0, 0, 0}, {280, 70, 33, 33, 0, 0, 0}, {281, 48, 13, 13, 0, 0, 0}, {282, 123, 35, 35, 3, 32, 33}, {283, 74, 144, 145, 0, 0, 0}, {284, 99, 32, 32, 3, 30, 31}, {285, 137, 40, 49, 3, 38, 39}, {286, 210, 53, 57, 3, 50, 51}, {287, 1, 23, 23, 3, 20, 21}, {288, 20, 23, 23, 3, 20, 21}, {290, 251, 46, 46, 0, 0, 0}, {291, 10, 57, 57, 0, 0, 0}, {299, 19, 96, 98, 0, 0, 0}, {300, 217, 22, 22, 0, 0, 0}, {301, 243, 58, 58, 0, 0, 0}, {310, 28, 17, 17, 0, 0, 0}, {311, 95, 116, 116, 0, 0, 0}, {320, 243, 20, 20, 3, 2, 3}, {321, 88, 2, 2, 3, 0, 1}, {322, 243, 149, 149, 0, 0, 0}, {323, 78, 147, 147, 3, 0, 1}, {324, 132, 146, 146, 0, 0, 0}, {330, 23, 158, 167, 0, 0, 0}, {331, 91, 230, 233, 0, 0, 0}, {332, 236, 239, 239, 0, 0, 0}, {333, 231, 109, 109, 0, 0, 0}, {334, 72, 10, 10, 0, 0, 0}, {335, 225, 24, 24, 0, 0, 0}, {336, 245, 84, 84, 0, 0, 0}, {339, 199, 5, 5, 0, 0, 0}, {340, 99, 70, 70, 0, 0, 0}, {350, 232, 20, 252, 0, 0, 0}, {360, 11, 25, 25, 0, 0, 0}, {370, 75, 87, 109, 0, 0, 0}, {371, 10, 26, 26, 0, 0, 0}, {372, 26, 140, 140, 0, 0, 0}, {373, 117, 42, 42, 0, 0, 0}, {375, 251, 140, 140, 0, 0, 0}, {380, 232, 20, 20, 0, 0, 0}, {385, 147, 133, 133, 3, 2, 3}, {386, 132, 16, 16, 3, 4, 5}, {387, 4, 72, 72, 3, 4, 5}, {388, 8, 37, 37, 3, 32, 33}, {390, 156, 238, 238, 0, 0, 0}, {395, 0, 212, 212, 0, 0, 0}, {396, 50, 160, 160, 0, 0, 0}, {397, 182, 108, 108, 0, 0, 0}, {400, 110, 254, 254, 3, 4, 5}, {401, 183, 6, 6, 3, 4, 5}, {410, 160, 53, 53, 0, 0, 0}, {411, 106, 3, 3, 0, 0, 0}, {412, 33, 6, 6, 3, 4, 5}, {413, 77, 7, 7, 3, 4, 5}, {435, 134, 46, 46, 0, 0, 0}, {436, 193, 9, 9, 0, 0, 0}, {437, 30, 1, 1, 0, 0, 0}, {440, 66, 35, 35, 0, 0, 0}, {9000, 113, 137, 137, 0, 0, 0}, {9005, 117, 34, 34, 0, 0, 0}, {12900, 114, 44, 44, 3, 0, 1}, {12901, 254, 59, 59, 3, 30, 31}, {12902, 140, 53, 53, 3, 4, 5}, {12903, 249, 46, 46, 3, 0, 1}, {12904, 77, 54, 54, 3, 28, 29}, {12905, 49, 43, 43, 3, 0, 1}, {12915, 94, 249, 249, 3, 0, 1}, {12918, 139, 51, 51, 0, 0, 0}, {12919, 7, 18, 18, 3, 16, 17}, {12920, 20, 5, 5, 0, 0, 0}} #endif #include "../protocol.h" @@ -34,20 +33,6 @@ extern "C" { // ENUM DEFINITIONS -/** @brief These values define the type of firmware release. These values indicate the first version or release of this type. For example the first alpha release would be 64, the second would be 65. */ -#ifndef HAVE_ENUM_FIRMWARE_VERSION_TYPE -#define HAVE_ENUM_FIRMWARE_VERSION_TYPE -typedef enum FIRMWARE_VERSION_TYPE -{ - FIRMWARE_VERSION_TYPE_DEV=0, /* development release | */ - FIRMWARE_VERSION_TYPE_ALPHA=64, /* alpha release | */ - FIRMWARE_VERSION_TYPE_BETA=128, /* beta release | */ - FIRMWARE_VERSION_TYPE_RC=192, /* release candidate | */ - FIRMWARE_VERSION_TYPE_OFFICIAL=255, /* official stable release | */ - FIRMWARE_VERSION_TYPE_ENUM_END=256, /* | */ -} FIRMWARE_VERSION_TYPE; -#endif - /** @brief Flags to report failure cases over the high latency telemetry. */ #ifndef HAVE_ENUM_HL_FAILURE_FLAG #define HAVE_ENUM_HL_FAILURE_FLAG @@ -61,7 +46,7 @@ typedef enum HL_FAILURE_FLAG HL_FAILURE_FLAG_3D_MAG=32, /* Magnetometer sensor failure. | */ HL_FAILURE_FLAG_TERRAIN=64, /* Terrain subsystem failure. | */ HL_FAILURE_FLAG_BATTERY=128, /* Battery failure/critical low battery. | */ - HL_FAILURE_FLAG_RC_RECEIVER=256, /* RC receiver failure/no rc connection. | */ + HL_FAILURE_FLAG_RC_RECEIVER=256, /* RC receiver failure/no RC connection. | */ HL_FAILURE_FLAG_OFFBOARD_LINK=512, /* Offboard link failure. | */ HL_FAILURE_FLAG_ENGINE=1024, /* Engine failure. | */ HL_FAILURE_FLAG_GEOFENCE=2048, /* Geofence violation. | */ @@ -84,23 +69,22 @@ typedef enum MAV_GOTO } MAV_GOTO; #endif -/** @brief These defines are predefined OR-combined mode flags. There is no need to use values from this enum, but it - simplifies the use of the mode flags. Note that manual input is enabled in all modes as a safety override. */ +/** @brief Predefined OR-combined MAV_MODE_FLAG values. These can simplify using the flags when setting modes. Note that manual input is enabled in all modes as a safety override. */ #ifndef HAVE_ENUM_MAV_MODE #define HAVE_ENUM_MAV_MODE typedef enum MAV_MODE { MAV_MODE_PREFLIGHT=0, /* System is not ready to fly, booting, calibrating, etc. No flag is set. | */ - MAV_MODE_MANUAL_DISARMED=64, /* System is allowed to be active, under manual (RC) control, no stabilization | */ - MAV_MODE_TEST_DISARMED=66, /* UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. | */ - MAV_MODE_STABILIZE_DISARMED=80, /* System is allowed to be active, under assisted RC control. | */ - MAV_MODE_GUIDED_DISARMED=88, /* System is allowed to be active, under autonomous control, manual setpoint | */ - MAV_MODE_AUTO_DISARMED=92, /* System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by waypoints) | */ - MAV_MODE_MANUAL_ARMED=192, /* System is allowed to be active, under manual (RC) control, no stabilization | */ - MAV_MODE_TEST_ARMED=194, /* UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. | */ - MAV_MODE_STABILIZE_ARMED=208, /* System is allowed to be active, under assisted RC control. | */ - MAV_MODE_GUIDED_ARMED=216, /* System is allowed to be active, under autonomous control, manual setpoint | */ - MAV_MODE_AUTO_ARMED=220, /* System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by waypoints) | */ + MAV_MODE_MANUAL_DISARMED=64, /* System is allowed to be active, under manual (RC) control, no stabilization (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) | */ + MAV_MODE_TEST_DISARMED=66, /* UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. (MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, MAV_MODE_FLAG_TEST_ENABLED). | */ + MAV_MODE_STABILIZE_DISARMED=80, /* System is allowed to be active, under assisted RC control (MAV_MODE_FLAG_SAFETY_ARMED, MAV_MODE_FLAG_STABILIZE_ENABLED) | */ + MAV_MODE_GUIDED_DISARMED=88, /* System is allowed to be active, under autonomous control, manual setpoint (MAV_MODE_FLAG_SAFETY_ARMED, MAV_MODE_FLAG_STABILIZE_ENABLED, MAV_MODE_FLAG_GUIDED_ENABLED) | */ + MAV_MODE_AUTO_DISARMED=92, /* System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by waypoints). (MAV_MODE_FLAG_SAFETY_ARMED, MAV_MODE_FLAG_STABILIZE_ENABLED, MAV_MODE_FLAG_GUIDED_ENABLED, MAV_MODE_FLAG_AUTO_ENABLED). | */ + MAV_MODE_MANUAL_ARMED=192, /* System is allowed to be active, under manual (RC) control, no stabilization (MAV_MODE_FLAG_SAFETY_ARMED, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) | */ + MAV_MODE_TEST_ARMED=194, /* UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only (MAV_MODE_FLAG_SAFETY_ARMED, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, MAV_MODE_FLAG_TEST_ENABLED) | */ + MAV_MODE_STABILIZE_ARMED=208, /* System is allowed to be active, under assisted RC control (MAV_MODE_FLAG_SAFETY_ARMED, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, MAV_MODE_FLAG_STABILIZE_ENABLED) | */ + MAV_MODE_GUIDED_ARMED=216, /* System is allowed to be active, under autonomous control, manual setpoint (MAV_MODE_FLAG_SAFETY_ARMED, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, MAV_MODE_FLAG_STABILIZE_ENABLED, MAV_MODE_FLAG_GUIDED_ENABLED) | */ + MAV_MODE_AUTO_ARMED=220, /* System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by waypoints). (MAV_MODE_FLAG_SAFETY_ARMED, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, MAV_MODE_FLAG_STABILIZE_ENABLED, MAV_MODE_FLAG_GUIDED_ENABLED,MAV_MODE_FLAG_AUTO_ENABLED). | */ MAV_MODE_ENUM_END=221, /* | */ } MAV_MODE; #endif @@ -126,7 +110,7 @@ typedef enum MAV_SYS_STATUS_SENSOR MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL=8192, /* 0x2000 z/altitude control | */ MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL=16384, /* 0x4000 x/y position control | */ MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS=32768, /* 0x8000 motor outputs / control | */ - MAV_SYS_STATUS_SENSOR_RC_RECEIVER=65536, /* 0x10000 rc receiver | */ + MAV_SYS_STATUS_SENSOR_RC_RECEIVER=65536, /* 0x10000 RC receiver | */ MAV_SYS_STATUS_SENSOR_3D_GYRO2=131072, /* 0x20000 2nd 3D gyro | */ MAV_SYS_STATUS_SENSOR_3D_ACCEL2=262144, /* 0x40000 2nd 3D accelerometer | */ MAV_SYS_STATUS_SENSOR_3D_MAG2=524288, /* 0x80000 2nd 3D magnetometer | */ @@ -141,28 +125,56 @@ typedef enum MAV_SYS_STATUS_SENSOR MAV_SYS_STATUS_PREARM_CHECK=268435456, /* 0x10000000 pre-arm check status. Always healthy when armed | */ MAV_SYS_STATUS_OBSTACLE_AVOIDANCE=536870912, /* 0x20000000 Avoidance/collision prevention | */ MAV_SYS_STATUS_SENSOR_PROPULSION=1073741824, /* 0x40000000 propulsion (actuator, esc, motor or propellor) | */ - MAV_SYS_STATUS_SENSOR_ENUM_END=1073741825, /* | */ + MAV_SYS_STATUS_EXTENSION_USED=2147483648, /* 0x80000000 Extended bit-field are used for further sensor status bits (needs to be set in onboard_control_sensors_present only) | */ + MAV_SYS_STATUS_SENSOR_ENUM_END=2147483649, /* | */ } MAV_SYS_STATUS_SENSOR; #endif -/** @brief */ +/** @brief These encode the sensors whose status is sent as part of the SYS_STATUS message in the extended fields. */ +#ifndef HAVE_ENUM_MAV_SYS_STATUS_SENSOR_EXTENDED +#define HAVE_ENUM_MAV_SYS_STATUS_SENSOR_EXTENDED +typedef enum MAV_SYS_STATUS_SENSOR_EXTENDED +{ + MAV_SYS_STATUS_RECOVERY_SYSTEM=1, /* 0x01 Recovery system (parachute, balloon, retracts etc) | */ + MAV_SYS_STATUS_SENSOR_EXTENDED_ENUM_END=2, /* | */ +} MAV_SYS_STATUS_SENSOR_EXTENDED; +#endif + +/** @brief Coordinate frames used by MAVLink. Not all frames are supported by all commands, messages, or vehicles. + + Global frames use the following naming conventions: + - "GLOBAL": Global coordinate frame with WGS84 latitude/longitude and altitude positive over mean sea level (MSL) by default. + The following modifiers may be used with "GLOBAL": + - "RELATIVE_ALT": Altitude is relative to the vehicle home position rather than MSL. + - "TERRAIN_ALT": Altitude is relative to ground level rather than MSL. + - "INT": Latitude/longitude (in degrees) are scaled by multiplying by 1E7. + + Local frames use the following naming conventions: + - "LOCAL": Origin of local frame is fixed relative to earth. Unless otherwise specified this origin is the origin of the vehicle position-estimator ("EKF"). + - "BODY": Origin of local frame travels with the vehicle. NOTE, "BODY" does NOT indicate alignment of frame axis with vehicle attitude. + - "OFFSET": Deprecated synonym for "BODY" (origin travels with the vehicle). Not to be used for new frames. + + Some deprecated frames do not follow these conventions (e.g. MAV_FRAME_BODY_NED and MAV_FRAME_BODY_OFFSET_NED). + */ #ifndef HAVE_ENUM_MAV_FRAME #define HAVE_ENUM_MAV_FRAME typedef enum MAV_FRAME { - MAV_FRAME_GLOBAL=0, /* Global (WGS84) coordinate frame + MSL altitude. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL). | */ - MAV_FRAME_LOCAL_NED=1, /* Local coordinate frame, Z-down (x: North, y: East, z: Down). | */ + MAV_FRAME_GLOBAL=0, /* Global (WGS84) coordinate frame + altitude relative to mean sea level (MSL). | */ + MAV_FRAME_LOCAL_NED=1, /* NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth. | */ MAV_FRAME_MISSION=2, /* NOT a coordinate frame, indicates a mission command. | */ - MAV_FRAME_GLOBAL_RELATIVE_ALT=3, /* Global (WGS84) coordinate frame + altitude relative to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. | */ - MAV_FRAME_LOCAL_ENU=4, /* Local coordinate frame, Z-up (x: East, y: North, z: Up). | */ - MAV_FRAME_GLOBAL_INT=5, /* Global (WGS84) coordinate frame (scaled) + MSL altitude. First value / x: latitude in degrees*1.0e-7, second value / y: longitude in degrees*1.0e-7, third value / z: positive altitude over mean sea level (MSL). | */ - MAV_FRAME_GLOBAL_RELATIVE_ALT_INT=6, /* Global (WGS84) coordinate frame (scaled) + altitude relative to the home position. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude with 0 being at the altitude of the home location. | */ - MAV_FRAME_LOCAL_OFFSET_NED=7, /* Offset to the current local frame. Anything expressed in this frame should be added to the current local frame position. | */ - MAV_FRAME_BODY_NED=8, /* Setpoint in body NED frame. This makes sense if all position control is externalized - e.g. useful to command 2 m/s^2 acceleration to the right. | */ - MAV_FRAME_BODY_OFFSET_NED=9, /* Offset in body NED frame. This makes sense if adding setpoints to the current flight path, to avoid an obstacle - e.g. useful to command 2 m/s^2 acceleration to the east. | */ - MAV_FRAME_GLOBAL_TERRAIN_ALT=10, /* Global (WGS84) coordinate frame with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees, second value / y: longitude in degrees, third value / z: positive altitude in meters with 0 being at ground level in terrain model. | */ - MAV_FRAME_GLOBAL_TERRAIN_ALT_INT=11, /* Global (WGS84) coordinate frame (scaled) with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude in meters with 0 being at ground level in terrain model. | */ - MAV_FRAME_BODY_FRD=12, /* Body fixed frame of reference, Z-down (x: Forward, y: Right, z: Down). | */ + MAV_FRAME_GLOBAL_RELATIVE_ALT=3, /* + Global (WGS84) coordinate frame + altitude relative to the home position. + | */ + MAV_FRAME_LOCAL_ENU=4, /* ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth. | */ + MAV_FRAME_GLOBAL_INT=5, /* Global (WGS84) coordinate frame (scaled) + altitude relative to mean sea level (MSL). | */ + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT=6, /* Global (WGS84) coordinate frame (scaled) + altitude relative to the home position. | */ + MAV_FRAME_LOCAL_OFFSET_NED=7, /* NED local tangent frame (x: North, y: East, z: Down) with origin that travels with the vehicle. | */ + MAV_FRAME_BODY_NED=8, /* Same as MAV_FRAME_LOCAL_NED when used to represent position values. Same as MAV_FRAME_BODY_FRD when used with velocity/acceleration values. | */ + MAV_FRAME_BODY_OFFSET_NED=9, /* This is the same as MAV_FRAME_BODY_FRD. | */ + MAV_FRAME_GLOBAL_TERRAIN_ALT=10, /* Global (WGS84) coordinate frame with AGL altitude (altitude at ground level). | */ + MAV_FRAME_GLOBAL_TERRAIN_ALT_INT=11, /* Global (WGS84) coordinate frame (scaled) with AGL altitude (altitude at ground level). | */ + MAV_FRAME_BODY_FRD=12, /* FRD local frame aligned to the vehicle's attitude (x: Forward, y: Right, z: Down) with an origin that travels with vehicle. | */ MAV_FRAME_RESERVED_13=13, /* MAV_FRAME_BODY_FLU - Body fixed frame of reference, Z-up (x: Forward, y: Left, z: Up). | */ MAV_FRAME_RESERVED_14=14, /* MAV_FRAME_MOCAP_NED - Odometry local coordinate frame of data given by a motion capture system, Z-down (x: North, y: East, z: Down). | */ MAV_FRAME_RESERVED_15=15, /* MAV_FRAME_MOCAP_ENU - Odometry local coordinate frame of data given by a motion capture system, Z-up (x: East, y: North, z: Up). | */ @@ -170,8 +182,8 @@ typedef enum MAV_FRAME MAV_FRAME_RESERVED_17=17, /* MAV_FRAME_VISION_ENU - Odometry local coordinate frame of data given by a vision estimation system, Z-up (x: East, y: North, z: Up). | */ MAV_FRAME_RESERVED_18=18, /* MAV_FRAME_ESTIM_NED - Odometry local coordinate frame of data given by an estimator running onboard the vehicle, Z-down (x: North, y: East, z: Down). | */ MAV_FRAME_RESERVED_19=19, /* MAV_FRAME_ESTIM_ENU - Odometry local coordinate frame of data given by an estimator running onboard the vehicle, Z-up (x: East, y: North, z: Up). | */ - MAV_FRAME_LOCAL_FRD=20, /* Forward, Right, Down coordinate frame. This is a local frame with Z-down and arbitrary F/R alignment (i.e. not aligned with NED/earth frame). | */ - MAV_FRAME_LOCAL_FLU=21, /* Forward, Left, Up coordinate frame. This is a local frame with Z-up and arbitrary F/L alignment (i.e. not aligned with ENU/earth frame). | */ + MAV_FRAME_LOCAL_FRD=20, /* FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth. The forward axis is aligned to the front of the vehicle in the horizontal plane. | */ + MAV_FRAME_LOCAL_FLU=21, /* FLU local tangent frame (x: Forward, y: Left, z: Up) with origin fixed relative to earth. The forward axis is aligned to the front of the vehicle in the horizontal plane. | */ MAV_FRAME_ENUM_END=22, /* | */ } MAV_FRAME; #endif @@ -191,20 +203,6 @@ typedef enum MAVLINK_DATA_STREAM_TYPE } MAVLINK_DATA_STREAM_TYPE; #endif -/** @brief */ -#ifndef HAVE_ENUM_FENCE_ACTION -#define HAVE_ENUM_FENCE_ACTION -typedef enum FENCE_ACTION -{ - FENCE_ACTION_NONE=0, /* Disable fenced mode | */ - FENCE_ACTION_GUIDED=1, /* Switched to guided mode to return point (fence point 0) | */ - FENCE_ACTION_REPORT=2, /* Report fence breach, but don't take action | */ - FENCE_ACTION_GUIDED_THR_PASS=3, /* Switched to guided mode to return point (fence point 0) with manual throttle control | */ - FENCE_ACTION_RTL=4, /* Switch to RTL (return to launch) mode and head for the return point. | */ - FENCE_ACTION_ENUM_END=5, /* | */ -} FENCE_ACTION; -#endif - /** @brief */ #ifndef HAVE_ENUM_FENCE_BREACH #define HAVE_ENUM_FENCE_BREACH @@ -230,44 +228,62 @@ typedef enum FENCE_MITIGATE } FENCE_MITIGATE; #endif +/** @brief Fence types to enable or disable when using MAV_CMD_DO_FENCE_ENABLE. + Note that at least one of these flags must be set in MAV_CMD_DO_FENCE_ENABLE.param2. + If none are set, the flight stack will ignore the field and enable/disable its default set of fences (usually all of them). + */ +#ifndef HAVE_ENUM_FENCE_TYPE +#define HAVE_ENUM_FENCE_TYPE +typedef enum FENCE_TYPE +{ + FENCE_TYPE_ALT_MAX=1, /* Maximum altitude fence | */ + FENCE_TYPE_CIRCLE=2, /* Circle fence | */ + FENCE_TYPE_POLYGON=4, /* Polygon fence | */ + FENCE_TYPE_ALT_MIN=8, /* Minimum altitude fence | */ + FENCE_TYPE_ENUM_END=9, /* | */ +} FENCE_TYPE; +#endif + /** @brief Enumeration of possible mount operation modes. This message is used by obsolete/deprecated gimbal messages. */ #ifndef HAVE_ENUM_MAV_MOUNT_MODE #define HAVE_ENUM_MAV_MOUNT_MODE typedef enum MAV_MOUNT_MODE { - MAV_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from permant memory and stop stabilization | */ + MAV_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization | */ MAV_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. | */ MAV_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */ MAV_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */ MAV_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */ MAV_MOUNT_MODE_SYSID_TARGET=5, /* Gimbal tracks system with specified system ID | */ - MAV_MOUNT_MODE_HOME_LOCATION=6, /* Gimbal tracks home location | */ + MAV_MOUNT_MODE_HOME_LOCATION=6, /* Gimbal tracks home position | */ MAV_MOUNT_MODE_ENUM_END=7, /* | */ } MAV_MOUNT_MODE; #endif -/** @brief Gimbal device (low level) capability flags (bitmap) */ +/** @brief Gimbal device (low level) capability flags (bitmap). */ #ifndef HAVE_ENUM_GIMBAL_DEVICE_CAP_FLAGS #define HAVE_ENUM_GIMBAL_DEVICE_CAP_FLAGS typedef enum GIMBAL_DEVICE_CAP_FLAGS { - GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT=1, /* Gimbal device supports a retracted position | */ - GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL=2, /* Gimbal device supports a horizontal, forward looking position, stabilized | */ + GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT=1, /* Gimbal device supports a retracted position. | */ + GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL=2, /* Gimbal device supports a horizontal, forward looking position, stabilized. | */ GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS=4, /* Gimbal device supports rotating around roll axis. | */ - GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW=8, /* Gimbal device supports to follow a roll angle relative to the vehicle | */ - GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_LOCK=16, /* Gimbal device supports locking to an roll angle (generally that's the default with roll stabilized) | */ + GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW=8, /* Gimbal device supports to follow a roll angle relative to the vehicle. | */ + GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_LOCK=16, /* Gimbal device supports locking to a roll angle (generally that's the default with roll stabilized). | */ GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS=32, /* Gimbal device supports rotating around pitch axis. | */ - GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW=64, /* Gimbal device supports to follow a pitch angle relative to the vehicle | */ - GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK=128, /* Gimbal device supports locking to an pitch angle (generally that's the default with pitch stabilized) | */ + GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW=64, /* Gimbal device supports to follow a pitch angle relative to the vehicle. | */ + GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK=128, /* Gimbal device supports locking to a pitch angle (generally that's the default with pitch stabilized). | */ GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS=256, /* Gimbal device supports rotating around yaw axis. | */ - GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW=512, /* Gimbal device supports to follow a yaw angle relative to the vehicle (generally that's the default) | */ - GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK=1024, /* Gimbal device supports locking to an absolute heading (often this is an option available) | */ - GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW=2048, /* Gimbal device supports yawing/panning infinetely (e.g. using slip disk). | */ - GIMBAL_DEVICE_CAP_FLAGS_ENUM_END=2049, /* | */ + GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW=512, /* Gimbal device supports to follow a yaw angle relative to the vehicle (generally that's the default). | */ + GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK=1024, /* Gimbal device supports locking to an absolute heading, i.e., yaw angle relative to North (earth frame, often this is an option available). | */ + GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW=2048, /* Gimbal device supports yawing/panning infinitely (e.g. using slip disk). | */ + GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_YAW_IN_EARTH_FRAME=4096, /* Gimbal device supports yaw angles and angular velocities relative to North (earth frame). This usually requires support by an autopilot via AUTOPILOT_STATE_FOR_GIMBAL_DEVICE. Support can go on and off during runtime, which is reported by the flag GIMBAL_DEVICE_FLAGS_CAN_ACCEPT_YAW_IN_EARTH_FRAME. | */ + GIMBAL_DEVICE_CAP_FLAGS_HAS_RC_INPUTS=8192, /* Gimbal device supports radio control inputs as an alternative input for controlling the gimbal orientation. | */ + GIMBAL_DEVICE_CAP_FLAGS_ENUM_END=8193, /* | */ } GIMBAL_DEVICE_CAP_FLAGS; #endif -/** @brief Gimbal manager high level capability flags (bitmap). The first 16 bits are identical to the GIMBAL_DEVICE_CAP_FLAGS which are identical with GIMBAL_DEVICE_FLAGS. However, the gimbal manager does not need to copy the flags from the gimbal but can also enhance the capabilities and thus add flags. */ +/** @brief Gimbal manager high level capability flags (bitmap). The first 16 bits are identical to the GIMBAL_DEVICE_CAP_FLAGS. However, the gimbal manager does not need to copy the flags from the gimbal but can also enhance the capabilities and thus add flags. */ #ifndef HAVE_ENUM_GIMBAL_MANAGER_CAP_FLAGS #define HAVE_ENUM_GIMBAL_MANAGER_CAP_FLAGS typedef enum GIMBAL_MANAGER_CAP_FLAGS @@ -284,6 +300,8 @@ typedef enum GIMBAL_MANAGER_CAP_FLAGS GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_FOLLOW=512, /* Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW. | */ GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK=1024, /* Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK. | */ GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_INFINITE_YAW=2048, /* Based on GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW. | */ + GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_YAW_IN_EARTH_FRAME=4096, /* Based on GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_YAW_IN_EARTH_FRAME. | */ + GIMBAL_MANAGER_CAP_FLAGS_HAS_RC_INPUTS=8192, /* Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_RC_INPUTS. | */ GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_LOCAL=65536, /* Gimbal manager supports to point to a local position. | */ GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL=131072, /* Gimbal manager supports to point to a global latitude, longitude, altitude position. | */ GIMBAL_MANAGER_CAP_FLAGS_ENUM_END=131073, /* | */ @@ -295,26 +313,36 @@ typedef enum GIMBAL_MANAGER_CAP_FLAGS #define HAVE_ENUM_GIMBAL_DEVICE_FLAGS typedef enum GIMBAL_DEVICE_FLAGS { - GIMBAL_DEVICE_FLAGS_RETRACT=1, /* Set to retracted safe position (no stabilization), takes presedence over all other flags. | */ - GIMBAL_DEVICE_FLAGS_NEUTRAL=2, /* Set to neutral position (horizontal, forward looking, with stabiliziation), takes presedence over all other flags except RETRACT. | */ - GIMBAL_DEVICE_FLAGS_ROLL_LOCK=4, /* Lock roll angle to absolute angle relative to horizon (not relative to drone). This is generally the default with a stabilizing gimbal. | */ - GIMBAL_DEVICE_FLAGS_PITCH_LOCK=8, /* Lock pitch angle to absolute angle relative to horizon (not relative to drone). This is generally the default. | */ - GIMBAL_DEVICE_FLAGS_YAW_LOCK=16, /* Lock yaw angle to absolute angle relative to North (not relative to drone). If this flag is set, the quaternion is in the Earth frame with the x-axis pointing North (yaw absolute). If this flag is not set, the quaternion frame is in the Earth frame rotated so that the x-axis is pointing forward (yaw relative to vehicle). | */ - GIMBAL_DEVICE_FLAGS_ENUM_END=17, /* | */ + GIMBAL_DEVICE_FLAGS_RETRACT=1, /* Set to retracted safe position (no stabilization), takes precedence over all other flags. | */ + GIMBAL_DEVICE_FLAGS_NEUTRAL=2, /* Set to neutral/default position, taking precedence over all other flags except RETRACT. Neutral is commonly forward-facing and horizontal (roll=pitch=yaw=0) but may be any orientation. | */ + GIMBAL_DEVICE_FLAGS_ROLL_LOCK=4, /* Lock roll angle to absolute angle relative to horizon (not relative to vehicle). This is generally the default with a stabilizing gimbal. | */ + GIMBAL_DEVICE_FLAGS_PITCH_LOCK=8, /* Lock pitch angle to absolute angle relative to horizon (not relative to vehicle). This is generally the default with a stabilizing gimbal. | */ + GIMBAL_DEVICE_FLAGS_YAW_LOCK=16, /* Lock yaw angle to absolute angle relative to North (not relative to vehicle). If this flag is set, the yaw angle and z component of angular velocity are relative to North (earth frame, x-axis pointing North), else they are relative to the vehicle heading (vehicle frame, earth frame rotated so that the x-axis is pointing forward). | */ + GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME=32, /* Yaw angle and z component of angular velocity are relative to the vehicle heading (vehicle frame, earth frame rotated such that the x-axis is pointing forward). | */ + GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME=64, /* Yaw angle and z component of angular velocity are relative to North (earth frame, x-axis is pointing North). | */ + GIMBAL_DEVICE_FLAGS_ACCEPTS_YAW_IN_EARTH_FRAME=128, /* Gimbal device can accept yaw angle inputs relative to North (earth frame). This flag is only for reporting (attempts to set this flag are ignored). | */ + GIMBAL_DEVICE_FLAGS_RC_EXCLUSIVE=256, /* The gimbal orientation is set exclusively by the RC signals feed to the gimbal's radio control inputs. MAVLink messages for setting the gimbal orientation (GIMBAL_DEVICE_SET_ATTITUDE) are ignored. | */ + GIMBAL_DEVICE_FLAGS_RC_MIXED=512, /* The gimbal orientation is determined by combining/mixing the RC signals feed to the gimbal's radio control inputs and the MAVLink messages for setting the gimbal orientation (GIMBAL_DEVICE_SET_ATTITUDE). How these two controls are combined or mixed is not defined by the protocol but is up to the implementation. | */ + GIMBAL_DEVICE_FLAGS_ENUM_END=513, /* | */ } GIMBAL_DEVICE_FLAGS; #endif -/** @brief Flags for high level gimbal manager operation The first 16 bytes are identical to the GIMBAL_DEVICE_FLAGS. */ +/** @brief Flags for high level gimbal manager operation The first 16 bits are identical to the GIMBAL_DEVICE_FLAGS. */ #ifndef HAVE_ENUM_GIMBAL_MANAGER_FLAGS #define HAVE_ENUM_GIMBAL_MANAGER_FLAGS typedef enum GIMBAL_MANAGER_FLAGS { - GIMBAL_MANAGER_FLAGS_RETRACT=1, /* Based on GIMBAL_DEVICE_FLAGS_RETRACT | */ - GIMBAL_MANAGER_FLAGS_NEUTRAL=2, /* Based on GIMBAL_DEVICE_FLAGS_NEUTRAL | */ - GIMBAL_MANAGER_FLAGS_ROLL_LOCK=4, /* Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK | */ - GIMBAL_MANAGER_FLAGS_PITCH_LOCK=8, /* Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK | */ - GIMBAL_MANAGER_FLAGS_YAW_LOCK=16, /* Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK | */ - GIMBAL_MANAGER_FLAGS_ENUM_END=17, /* | */ + GIMBAL_MANAGER_FLAGS_RETRACT=1, /* Based on GIMBAL_DEVICE_FLAGS_RETRACT. | */ + GIMBAL_MANAGER_FLAGS_NEUTRAL=2, /* Based on GIMBAL_DEVICE_FLAGS_NEUTRAL. | */ + GIMBAL_MANAGER_FLAGS_ROLL_LOCK=4, /* Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK. | */ + GIMBAL_MANAGER_FLAGS_PITCH_LOCK=8, /* Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK. | */ + GIMBAL_MANAGER_FLAGS_YAW_LOCK=16, /* Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK. | */ + GIMBAL_MANAGER_FLAGS_YAW_IN_VEHICLE_FRAME=32, /* Based on GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME. | */ + GIMBAL_MANAGER_FLAGS_YAW_IN_EARTH_FRAME=64, /* Based on GIMBAL_DEVICE_FLAGS_YAW_IN_EARTH_FRAME. | */ + GIMBAL_MANAGER_FLAGS_ACCEPTS_YAW_IN_EARTH_FRAME=128, /* Based on GIMBAL_DEVICE_FLAGS_ACCEPTS_YAW_IN_EARTH_FRAME. | */ + GIMBAL_MANAGER_FLAGS_RC_EXCLUSIVE=256, /* Based on GIMBAL_DEVICE_FLAGS_RC_EXCLUSIVE. | */ + GIMBAL_MANAGER_FLAGS_RC_MIXED=512, /* Based on GIMBAL_DEVICE_FLAGS_RC_MIXED. | */ + GIMBAL_MANAGER_FLAGS_ENUM_END=513, /* | */ } GIMBAL_MANAGER_FLAGS; #endif @@ -328,11 +356,12 @@ typedef enum GIMBAL_DEVICE_ERROR_FLAGS GIMBAL_DEVICE_ERROR_FLAGS_AT_YAW_LIMIT=4, /* Gimbal device is limited by hardware yaw limit. | */ GIMBAL_DEVICE_ERROR_FLAGS_ENCODER_ERROR=8, /* There is an error with the gimbal encoders. | */ GIMBAL_DEVICE_ERROR_FLAGS_POWER_ERROR=16, /* There is an error with the gimbal power source. | */ - GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR=32, /* There is an error with the gimbal motor's. | */ + GIMBAL_DEVICE_ERROR_FLAGS_MOTOR_ERROR=32, /* There is an error with the gimbal motors. | */ GIMBAL_DEVICE_ERROR_FLAGS_SOFTWARE_ERROR=64, /* There is an error with the gimbal's software. | */ GIMBAL_DEVICE_ERROR_FLAGS_COMMS_ERROR=128, /* There is an error with the gimbal's communication. | */ - GIMBAL_DEVICE_ERROR_FLAGS_CALIBRATION_RUNNING=256, /* Gimbal is currently calibrating. | */ - GIMBAL_DEVICE_ERROR_FLAGS_ENUM_END=257, /* | */ + GIMBAL_DEVICE_ERROR_FLAGS_CALIBRATION_RUNNING=256, /* Gimbal device is currently calibrating. | */ + GIMBAL_DEVICE_ERROR_FLAGS_NO_MANAGER=512, /* Gimbal device is not assigned to a gimbal manager. | */ + GIMBAL_DEVICE_ERROR_FLAGS_ENUM_END=513, /* | */ } GIMBAL_DEVICE_ERROR_FLAGS; #endif @@ -352,10 +381,17 @@ typedef enum GRIPPER_ACTIONS #define HAVE_ENUM_WINCH_ACTIONS typedef enum WINCH_ACTIONS { - WINCH_RELAXED=0, /* Relax winch. | */ - WINCH_RELATIVE_LENGTH_CONTROL=1, /* Wind or unwind specified length of cable, optionally using specified rate. | */ - WINCH_RATE_CONTROL=2, /* Wind or unwind cable at specified rate. | */ - WINCH_ACTIONS_ENUM_END=3, /* | */ + WINCH_RELAXED=0, /* Allow motor to freewheel. | */ + WINCH_RELATIVE_LENGTH_CONTROL=1, /* Wind or unwind specified length of line, optionally using specified rate. | */ + WINCH_RATE_CONTROL=2, /* Wind or unwind line at specified rate. | */ + WINCH_LOCK=3, /* Perform the locking sequence to relieve motor while in the fully retracted position. Only action and instance command parameters are used, others are ignored. | */ + WINCH_DELIVER=4, /* Sequence of drop, slow down, touch down, reel up, lock. Only action and instance command parameters are used, others are ignored. | */ + WINCH_HOLD=5, /* Engage motor and hold current position. Only action and instance command parameters are used, others are ignored. | */ + WINCH_RETRACT=6, /* Return the reel to the fully retracted position. Only action and instance command parameters are used, others are ignored. | */ + WINCH_LOAD_LINE=7, /* Load the reel with line. The winch will calculate the total loaded length and stop when the tension exceeds a threshold. Only action and instance command parameters are used, others are ignored. | */ + WINCH_ABANDON_LINE=8, /* Spool out the entire length of the line. Only action and instance command parameters are used, others are ignored. | */ + WINCH_LOAD_PAYLOAD=9, /* Spools out just enough to present the hook to the user to load the payload. Only action and instance command parameters are used, others are ignored | */ + WINCH_ACTIONS_ENUM_END=10, /* | */ } WINCH_ACTIONS; #endif @@ -406,7 +442,6 @@ typedef enum ESC_CONNECTION_TYPE #define HAVE_ENUM_ESC_FAILURE_FLAGS typedef enum ESC_FAILURE_FLAGS { - ESC_FAILURE_NONE=0, /* No ESC failure. | */ ESC_FAILURE_OVER_CURRENT=1, /* Over current failure. | */ ESC_FAILURE_OVER_VOLTAGE=2, /* Over voltage failure. | */ ESC_FAILURE_OVER_TEMPERATURE=4, /* Over temperature failure. | */ @@ -449,6 +484,19 @@ typedef enum STORAGE_TYPE } STORAGE_TYPE; #endif +/** @brief Flags to indicate usage for a particular storage (see STORAGE_INFORMATION.storage_usage and MAV_CMD_SET_STORAGE_USAGE). */ +#ifndef HAVE_ENUM_STORAGE_USAGE_FLAG +#define HAVE_ENUM_STORAGE_USAGE_FLAG +typedef enum STORAGE_USAGE_FLAG +{ + STORAGE_USAGE_FLAG_SET=1, /* Always set to 1 (indicates STORAGE_INFORMATION.storage_usage is supported). | */ + STORAGE_USAGE_FLAG_PHOTO=2, /* Storage for saving photos. | */ + STORAGE_USAGE_FLAG_VIDEO=4, /* Storage for saving videos. | */ + STORAGE_USAGE_FLAG_LOGS=8, /* Storage for saving logs. | */ + STORAGE_USAGE_FLAG_ENUM_END=9, /* | */ +} STORAGE_USAGE_FLAG; +#endif + /** @brief Yaw behaviour during orbit flight. */ #ifndef HAVE_ENUM_ORBIT_YAW_BEHAVIOUR #define HAVE_ENUM_ORBIT_YAW_BEHAVIOUR @@ -459,7 +507,8 @@ typedef enum ORBIT_YAW_BEHAVIOUR ORBIT_YAW_BEHAVIOUR_UNCONTROLLED=2, /* Yaw uncontrolled. | */ ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE=3, /* Vehicle front follows flight path (tangential to circle). | */ ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED=4, /* Yaw controlled by RC input. | */ - ORBIT_YAW_BEHAVIOUR_ENUM_END=5, /* | */ + ORBIT_YAW_BEHAVIOUR_UNCHANGED=5, /* Vehicle uses current yaw behaviour (unchanged). The vehicle-default yaw behaviour is used if this value is specified when orbit is first commanded. | */ + ORBIT_YAW_BEHAVIOUR_ENUM_END=6, /* | */ } ORBIT_YAW_BEHAVIOUR; #endif @@ -505,39 +554,146 @@ typedef enum WIFI_CONFIG_AP_MODE } WIFI_CONFIG_AP_MODE; #endif -/** @brief Possible values for COMPONENT_INFORMATION.comp_metadata_type. */ +/** @brief Supported component metadata types. These are used in the "general" metadata file returned by COMPONENT_METADATA to provide information about supported metadata types. The types are not used directly in MAVLink messages. */ #ifndef HAVE_ENUM_COMP_METADATA_TYPE #define HAVE_ENUM_COMP_METADATA_TYPE typedef enum COMP_METADATA_TYPE { - COMP_METADATA_TYPE_VERSION=0, /* Version information which also includes information on other optional supported COMP_METADATA_TYPE's. Must be supported. Only downloadable from vehicle. | */ + COMP_METADATA_TYPE_GENERAL=0, /* General information about the component. General metadata includes information about other metadata types supported by the component. Files of this type must be supported, and must be downloadable from vehicle using a MAVLink FTP URI. | */ COMP_METADATA_TYPE_PARAMETER=1, /* Parameter meta data. | */ - COMP_METADATA_TYPE_COMMANDS=2, /* Meta data which specifies the commands the vehicle supports. (WIP) | */ - COMP_METADATA_TYPE_ENUM_END=3, /* | */ + COMP_METADATA_TYPE_COMMANDS=2, /* Meta data that specifies which commands and command parameters the vehicle supports. (WIP) | */ + COMP_METADATA_TYPE_PERIPHERALS=3, /* Meta data that specifies external non-MAVLink peripherals. | */ + COMP_METADATA_TYPE_EVENTS=4, /* Meta data for the events interface. | */ + COMP_METADATA_TYPE_ACTUATORS=5, /* Meta data for actuator configuration (motors, servos and vehicle geometry) and testing. | */ + COMP_METADATA_TYPE_ENUM_END=6, /* | */ } COMP_METADATA_TYPE; #endif -/** @brief Possible transport layers to set and get parameters via mavlink during a parameter transaction. */ -#ifndef HAVE_ENUM_PARAM_TRANSACTION_TRANSPORT -#define HAVE_ENUM_PARAM_TRANSACTION_TRANSPORT -typedef enum PARAM_TRANSACTION_TRANSPORT -{ - PARAM_TRANSACTION_TRANSPORT_PARAM=0, /* Transaction over param transport. | */ - PARAM_TRANSACTION_TRANSPORT_PARAM_EXT=1, /* Transaction over param_ext transport. | */ - PARAM_TRANSACTION_TRANSPORT_ENUM_END=2, /* | */ -} PARAM_TRANSACTION_TRANSPORT; -#endif - -/** @brief Possible parameter transaction actions. */ -#ifndef HAVE_ENUM_PARAM_TRANSACTION_ACTION -#define HAVE_ENUM_PARAM_TRANSACTION_ACTION -typedef enum PARAM_TRANSACTION_ACTION -{ - PARAM_TRANSACTION_ACTION_START=0, /* Commit the current parameter transaction. | */ - PARAM_TRANSACTION_ACTION_COMMIT=1, /* Commit the current parameter transaction. | */ - PARAM_TRANSACTION_ACTION_CANCEL=2, /* Cancel the current parameter transaction. | */ - PARAM_TRANSACTION_ACTION_ENUM_END=3, /* | */ -} PARAM_TRANSACTION_ACTION; +/** @brief Actuator configuration, used to change a setting on an actuator. Component information metadata can be used to know which outputs support which commands. */ +#ifndef HAVE_ENUM_ACTUATOR_CONFIGURATION +#define HAVE_ENUM_ACTUATOR_CONFIGURATION +typedef enum ACTUATOR_CONFIGURATION +{ + ACTUATOR_CONFIGURATION_NONE=0, /* Do nothing. | */ + ACTUATOR_CONFIGURATION_BEEP=1, /* Command the actuator to beep now. | */ + ACTUATOR_CONFIGURATION_3D_MODE_ON=2, /* Permanently set the actuator (ESC) to 3D mode (reversible thrust). | */ + ACTUATOR_CONFIGURATION_3D_MODE_OFF=3, /* Permanently set the actuator (ESC) to non 3D mode (non-reversible thrust). | */ + ACTUATOR_CONFIGURATION_SPIN_DIRECTION1=4, /* Permanently set the actuator (ESC) to spin direction 1 (which can be clockwise or counter-clockwise). | */ + ACTUATOR_CONFIGURATION_SPIN_DIRECTION2=5, /* Permanently set the actuator (ESC) to spin direction 2 (opposite of direction 1). | */ + ACTUATOR_CONFIGURATION_ENUM_END=6, /* | */ +} ACTUATOR_CONFIGURATION; +#endif + +/** @brief Actuator output function. Values greater or equal to 1000 are autopilot-specific. */ +#ifndef HAVE_ENUM_ACTUATOR_OUTPUT_FUNCTION +#define HAVE_ENUM_ACTUATOR_OUTPUT_FUNCTION +typedef enum ACTUATOR_OUTPUT_FUNCTION +{ + ACTUATOR_OUTPUT_FUNCTION_NONE=0, /* No function (disabled). | */ + ACTUATOR_OUTPUT_FUNCTION_MOTOR1=1, /* Motor 1 | */ + ACTUATOR_OUTPUT_FUNCTION_MOTOR2=2, /* Motor 2 | */ + ACTUATOR_OUTPUT_FUNCTION_MOTOR3=3, /* Motor 3 | */ + ACTUATOR_OUTPUT_FUNCTION_MOTOR4=4, /* Motor 4 | */ + ACTUATOR_OUTPUT_FUNCTION_MOTOR5=5, /* Motor 5 | */ + ACTUATOR_OUTPUT_FUNCTION_MOTOR6=6, /* Motor 6 | */ + ACTUATOR_OUTPUT_FUNCTION_MOTOR7=7, /* Motor 7 | */ + ACTUATOR_OUTPUT_FUNCTION_MOTOR8=8, /* Motor 8 | */ + ACTUATOR_OUTPUT_FUNCTION_MOTOR9=9, /* Motor 9 | */ + ACTUATOR_OUTPUT_FUNCTION_MOTOR10=10, /* Motor 10 | */ + ACTUATOR_OUTPUT_FUNCTION_MOTOR11=11, /* Motor 11 | */ + ACTUATOR_OUTPUT_FUNCTION_MOTOR12=12, /* Motor 12 | */ + ACTUATOR_OUTPUT_FUNCTION_MOTOR13=13, /* Motor 13 | */ + ACTUATOR_OUTPUT_FUNCTION_MOTOR14=14, /* Motor 14 | */ + ACTUATOR_OUTPUT_FUNCTION_MOTOR15=15, /* Motor 15 | */ + ACTUATOR_OUTPUT_FUNCTION_MOTOR16=16, /* Motor 16 | */ + ACTUATOR_OUTPUT_FUNCTION_SERVO1=33, /* Servo 1 | */ + ACTUATOR_OUTPUT_FUNCTION_SERVO2=34, /* Servo 2 | */ + ACTUATOR_OUTPUT_FUNCTION_SERVO3=35, /* Servo 3 | */ + ACTUATOR_OUTPUT_FUNCTION_SERVO4=36, /* Servo 4 | */ + ACTUATOR_OUTPUT_FUNCTION_SERVO5=37, /* Servo 5 | */ + ACTUATOR_OUTPUT_FUNCTION_SERVO6=38, /* Servo 6 | */ + ACTUATOR_OUTPUT_FUNCTION_SERVO7=39, /* Servo 7 | */ + ACTUATOR_OUTPUT_FUNCTION_SERVO8=40, /* Servo 8 | */ + ACTUATOR_OUTPUT_FUNCTION_SERVO9=41, /* Servo 9 | */ + ACTUATOR_OUTPUT_FUNCTION_SERVO10=42, /* Servo 10 | */ + ACTUATOR_OUTPUT_FUNCTION_SERVO11=43, /* Servo 11 | */ + ACTUATOR_OUTPUT_FUNCTION_SERVO12=44, /* Servo 12 | */ + ACTUATOR_OUTPUT_FUNCTION_SERVO13=45, /* Servo 13 | */ + ACTUATOR_OUTPUT_FUNCTION_SERVO14=46, /* Servo 14 | */ + ACTUATOR_OUTPUT_FUNCTION_SERVO15=47, /* Servo 15 | */ + ACTUATOR_OUTPUT_FUNCTION_SERVO16=48, /* Servo 16 | */ + ACTUATOR_OUTPUT_FUNCTION_ENUM_END=49, /* | */ +} ACTUATOR_OUTPUT_FUNCTION; +#endif + +/** @brief Axes that will be autotuned by MAV_CMD_DO_AUTOTUNE_ENABLE. + Note that at least one flag must be set in MAV_CMD_DO_AUTOTUNE_ENABLE.param2: if none are set, the flight stack will tune its default set of axes. */ +#ifndef HAVE_ENUM_AUTOTUNE_AXIS +#define HAVE_ENUM_AUTOTUNE_AXIS +typedef enum AUTOTUNE_AXIS +{ + AUTOTUNE_AXIS_ROLL=1, /* Autotune roll axis. | */ + AUTOTUNE_AXIS_PITCH=2, /* Autotune pitch axis. | */ + AUTOTUNE_AXIS_YAW=4, /* Autotune yaw axis. | */ + AUTOTUNE_AXIS_ENUM_END=5, /* | */ +} AUTOTUNE_AXIS; +#endif + +/** @brief + Actions for reading/writing parameters between persistent and volatile storage when using MAV_CMD_PREFLIGHT_STORAGE. + (Commonly parameters are loaded from persistent storage (flash/EEPROM) into volatile storage (RAM) on startup and written back when they are changed.) + */ +#ifndef HAVE_ENUM_PREFLIGHT_STORAGE_PARAMETER_ACTION +#define HAVE_ENUM_PREFLIGHT_STORAGE_PARAMETER_ACTION +typedef enum PREFLIGHT_STORAGE_PARAMETER_ACTION +{ + PARAM_READ_PERSISTENT=0, /* Read all parameters from persistent storage. Replaces values in volatile storage. | */ + PARAM_WRITE_PERSISTENT=1, /* Write all parameter values to persistent storage (flash/EEPROM) | */ + PARAM_RESET_CONFIG_DEFAULT=2, /* Reset all user configurable parameters to their default value (including airframe selection, sensor calibration data, safety settings, and so on). Does not reset values that contain operation counters and vehicle computed statistics. | */ + PARAM_RESET_SENSOR_DEFAULT=3, /* Reset only sensor calibration parameters to factory defaults (or firmware default if not available) | */ + PARAM_RESET_ALL_DEFAULT=4, /* Reset all parameters, including operation counters, to default values | */ + PREFLIGHT_STORAGE_PARAMETER_ACTION_ENUM_END=5, /* | */ +} PREFLIGHT_STORAGE_PARAMETER_ACTION; +#endif + +/** @brief + Actions for reading and writing plan information (mission, rally points, geofence) between persistent and volatile storage when using MAV_CMD_PREFLIGHT_STORAGE. + (Commonly missions are loaded from persistent storage (flash/EEPROM) into volatile storage (RAM) on startup and written back when they are changed.) + */ +#ifndef HAVE_ENUM_PREFLIGHT_STORAGE_MISSION_ACTION +#define HAVE_ENUM_PREFLIGHT_STORAGE_MISSION_ACTION +typedef enum PREFLIGHT_STORAGE_MISSION_ACTION +{ + MISSION_READ_PERSISTENT=0, /* Read current mission data from persistent storage | */ + MISSION_WRITE_PERSISTENT=1, /* Write current mission data to persistent storage | */ + MISSION_RESET_DEFAULT=2, /* Erase all mission data stored on the vehicle (both persistent and volatile storage) | */ + PREFLIGHT_STORAGE_MISSION_ACTION_ENUM_END=3, /* | */ +} PREFLIGHT_STORAGE_MISSION_ACTION; +#endif + +/** @brief Reboot/shutdown action for selected component in MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN. */ +#ifndef HAVE_ENUM_REBOOT_SHUTDOWN_ACTION +#define HAVE_ENUM_REBOOT_SHUTDOWN_ACTION +typedef enum REBOOT_SHUTDOWN_ACTION +{ + REBOOT_SHUTDOWN_ACTION_NONE=0, /* Do nothing. | */ + REBOOT_SHUTDOWN_ACTION_REBOOT=1, /* Reboot component. | */ + REBOOT_SHUTDOWN_ACTION_SHUTDOWN=2, /* Shutdown component. | */ + REBOOT_SHUTDOWN_ACTION_REBOOT_TO_BOOTLOADER=3, /* Reboot component and keep it in the bootloader until upgraded. | */ + REBOOT_SHUTDOWN_ACTION_POWER_ON=4, /* Power on component. Do nothing if component is already powered (ACK command with MAV_RESULT_ACCEPTED). | */ + REBOOT_SHUTDOWN_ACTION_ENUM_END=5, /* | */ +} REBOOT_SHUTDOWN_ACTION; +#endif + +/** @brief Specifies the conditions under which the MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN command should be accepted. */ +#ifndef HAVE_ENUM_REBOOT_SHUTDOWN_CONDITIONS +#define HAVE_ENUM_REBOOT_SHUTDOWN_CONDITIONS +typedef enum REBOOT_SHUTDOWN_CONDITIONS +{ + REBOOT_SHUTDOWN_CONDITIONS_SAFETY_INTERLOCKED=0, /* Reboot/Shutdown only if allowed by safety checks, such as being landed. | */ + REBOOT_SHUTDOWN_CONDITIONS_FORCE=20190226, /* Force reboot/shutdown of the autopilot/component regardless of system state. | */ + REBOOT_SHUTDOWN_CONDITIONS_ENUM_END=20190227, /* | */ +} REBOOT_SHUTDOWN_CONDITIONS; #endif /** @brief Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. NaN and INT32_MAX may be used in float/integer params (respectively) to indicate optional/default values (e.g. to use the component's current yaw or latitude rather than a specific value). See https://mavlink.io/en/guide/xml_schema.html#MAV_CMD for information about the structure of the MAV_CMD entries */ @@ -545,10 +701,10 @@ typedef enum PARAM_TRANSACTION_ACTION #define HAVE_ENUM_MAV_CMD typedef enum MAV_CMD { - MAV_CMD_NAV_WAYPOINT=16, /* Navigate to waypoint. |Hold time. (ignored by fixed wing, time to stay at waypoint for rotary wing)| Acceptance radius (if the sphere with this radius is hit, the waypoint counts as reached)| 0 to pass through the WP, if > 0 radius to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at waypoint (rotary wing). NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_WAYPOINT=16, /* Navigate to waypoint. This is intended for use in missions (for guided commands outside of missions use MAV_CMD_DO_REPOSITION). |Hold time. (ignored by fixed wing, time to stay at waypoint for rotary wing)| Acceptance radius (if the sphere with this radius is hit, the waypoint counts as reached)| 0 to pass through the WP, if > 0 radius to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at waypoint (rotary wing). NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this waypoint an unlimited amount of time |Empty| Empty| Loiter radius around waypoint for forward-only moving vehicles (not multicopters). If positive loiter clockwise, else counter-clockwise| Desired yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this waypoint for X turns |Number of turns.| Leave loiter circle only once heading towards the next waypoint (0 = False)| Loiter radius around waypoint for forward-only moving vehicles (not multicopters). If positive loiter clockwise, else counter-clockwise| Loiter circle exit location and/or path to next waypoint ("xtrack") for forward-only moving vehicles (not multicopters). 0 for the vehicle to converge towards the center xtrack when it leaves the loiter (the line between the centers of the current and next waypoint), 1 to converge to the direct line between the location that the vehicle exits the loiter radius and the next waypoint. Otherwise the angle (in degrees) between the tangent of the loiter circle and the center xtrack at which the vehicle must leave the loiter (and converge to the center xtrack). NaN to use the current system default xtrack behaviour.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_LOITER_TIME=19, /* Loiter at the specified latitude, longitude and altitude for a certain amount of time. Multicopter vehicles stop at the point (within a vehicle-specific acceptance radius). Forward-only moving vehicles (e.g. fixed-wing) circle the point with the specified radius/direction. If the Heading Required parameter (2) is non-zero forward moving aircraft will only leave the loiter circle once heading towards the next waypoint. |Loiter time (only starts once Lat, Lon and Alt is reached).| Leave loiter circle only once heading towards the next waypoint (0 = False)| Loiter radius around waypoint for forward-only moving vehicles (not multicopters). If positive loiter clockwise, else counter-clockwise.| Loiter circle exit location and/or path to next waypoint ("xtrack") for forward-only moving vehicles (not multicopters). 0 for the vehicle to converge towards the center xtrack when it leaves the loiter (the line between the centers of the current and next waypoint), 1 to converge to the direct line between the location that the vehicle exits the loiter radius and the next waypoint. Otherwise the angle (in degrees) between the tangent of the loiter circle and the center xtrack at which the vehicle must leave the loiter (and converge to the center xtrack). NaN to use the current system default xtrack behaviour.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this waypoint for X turns |Number of turns.| Leave loiter circle only when track heads towards the next waypoint (MAV_BOOL_FALSE: Leave when turns complete). Values not equal to 0 or 1 are invalid.| Loiter radius around waypoint for forward-only moving vehicles (not multicopters). If positive loiter clockwise, else counter-clockwise| Loiter circle exit location and/or path to next waypoint ("xtrack") for forward-only moving vehicles (not multicopters). 0 for the vehicle to converge towards the center xtrack when it leaves the loiter (the line between the centers of the current and next waypoint), 1 to converge to the direct line between the location that the vehicle exits the loiter radius and the next waypoint. Otherwise the angle (in degrees) between the tangent of the loiter circle and the center xtrack at which the vehicle must leave the loiter (and converge to the center xtrack). NaN to use the current system default xtrack behaviour.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_TIME=19, /* Loiter at the specified latitude, longitude and altitude for a certain amount of time. Multicopter vehicles stop at the point (within a vehicle-specific acceptance radius). Forward-only moving vehicles (e.g. fixed-wing) circle the point with the specified radius/direction. If the Heading Required parameter (2) is non-zero forward moving aircraft will only leave the loiter circle once heading towards the next waypoint. |Loiter time (only starts once Lat, Lon and Alt is reached).| Leave loiter circle only when track heading towards the next waypoint (MAV_BOOL_FALSE: Leave on time expiry). Values not equal to 0 or 1 are invalid.| Loiter radius around waypoint for forward-only moving vehicles (not multicopters). If positive loiter clockwise, else counter-clockwise.| Loiter circle exit location and/or path to next waypoint ("xtrack") for forward-only moving vehicles (not multicopters). 0 for the vehicle to converge towards the center xtrack when it leaves the loiter (the line between the centers of the current and next waypoint), 1 to converge to the direct line between the location that the vehicle exits the loiter radius and the next waypoint. Otherwise the angle (in degrees) between the tangent of the loiter circle and the center xtrack at which the vehicle must leave the loiter (and converge to the center xtrack). NaN to use the current system default xtrack behaviour.| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_NAV_LAND=21, /* Land at location. |Minimum target altitude if landing is aborted (0 = undefined/use system default).| Precision land mode.| Empty.| Desired yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).| Latitude.| Longitude.| Landing altitude (ground level in current frame).| */ MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand. Vehicles that support multiple takeoff modes (e.g. VTOL quadplane) should take off using the currently configured mode. |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).| Latitude| Longitude| Altitude| */ @@ -556,42 +712,73 @@ typedef enum MAV_CMD MAV_CMD_NAV_TAKEOFF_LOCAL=24, /* Takeoff from local position (local frame only) |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Takeoff ascend rate| Yaw angle (if magnetometer or another yaw estimation source present), ignored without one of these| Y-axis position| X-axis position| Z-axis position| */ MAV_CMD_NAV_FOLLOW=25, /* Vehicle following, i.e. this waypoint represents the position of a moving vehicle |Following logic to use (e.g. loitering or sinusoidal following) - depends on specific autopilot implementation| Ground speed of vehicle to be followed| Radius around waypoint. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT=30, /* Continue on the current course and climb/descend to specified altitude. When the altitude is reached continue to the next command (i.e., don't proceed to the next command until the desired altitude is reached. |Climb or Descend (0 = Neutral, command completes when within 5m of this command's altitude, 1 = Climbing, command completes when at or above this command's altitude, 2 = Descending, command completes when at or below this command's altitude.| Empty| Empty| Empty| Empty| Empty| Desired altitude| */ - MAV_CMD_NAV_LOITER_TO_ALT=31, /* Begin loiter at the specified Latitude and Longitude. If Lat=Lon=0, then loiter at the current position. Don't consider the navigation command complete (don't leave loiter) until the altitude has been reached. Additionally, if the Heading Required parameter is non-zero the aircraft will not leave the loiter until heading toward the next waypoint. |Leave loiter circle only once heading towards the next waypoint (0 = False)| Loiter radius around waypoint for forward-only moving vehicles (not multicopters). If positive loiter clockwise, negative counter-clockwise, 0 means no change to standard loiter.| Empty| Loiter circle exit location and/or path to next waypoint ("xtrack") for forward-only moving vehicles (not multicopters). 0 for the vehicle to converge towards the center xtrack when it leaves the loiter (the line between the centers of the current and next waypoint), 1 to converge to the direct line between the location that the vehicle exits the loiter radius and the next waypoint. Otherwise the angle (in degrees) between the tangent of the loiter circle and the center xtrack at which the vehicle must leave the loiter (and converge to the center xtrack). NaN to use the current system default xtrack behaviour.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_TO_ALT=31, /* Begin loiter at the specified Latitude and Longitude. If Lat=Lon=0, then loiter at the current position. Don't consider the navigation command complete (don't leave loiter) until the altitude has been reached. Additionally, if the Heading Required parameter is non-zero the aircraft will not leave the loiter until heading toward the next waypoint. |Leave loiter circle only when track heading towards the next waypoint (MAV_BOOL_FALSE: Leave when altitude reached). Values not equal to 0 or 1 are invalid.| Loiter radius around waypoint for forward-only moving vehicles (not multicopters). If positive loiter clockwise, negative counter-clockwise, 0 means no change to standard loiter.| Empty| Loiter circle exit location and/or path to next waypoint ("xtrack") for forward-only moving vehicles (not multicopters). 0 for the vehicle to converge towards the center xtrack when it leaves the loiter (the line between the centers of the current and next waypoint), 1 to converge to the direct line between the location that the vehicle exits the loiter radius and the next waypoint. Otherwise the angle (in degrees) between the tangent of the loiter circle and the center xtrack at which the vehicle must leave the loiter (and converge to the center xtrack). NaN to use the current system default xtrack behaviour.| Latitude| Longitude| Altitude| */ MAV_CMD_DO_FOLLOW=32, /* Begin following a target |System ID (of the FOLLOW_TARGET beacon). Send 0 to disable follow-me and return to the default position hold mode.| Reserved| Reserved| Altitude mode: 0: Keep current altitude, 1: keep altitude difference to target, 2: go to a fixed altitude above home.| Altitude above home. (used if mode=2)| Reserved| Time to land in which the MAV should go to the default position hold mode after a message RX timeout.| */ MAV_CMD_DO_FOLLOW_REPOSITION=33, /* Reposition the MAV after a follow target command has been sent |Camera q1 (where 0 is on the ray from the camera to the tracking device)| Camera q2| Camera q3| Camera q4| altitude offset from target| X offset from target| Y offset from target| */ - MAV_CMD_DO_ORBIT=34, /* Start orbiting on the circumference of a circle defined by the parameters. Setting any value NaN results in using defaults. |Radius of the circle. positive: Orbit clockwise. negative: Orbit counter-clockwise.| Tangential Velocity. NaN: Vehicle configuration default.| Yaw behavior of the vehicle.| Reserved (e.g. for dynamic center beacon options)| Center point latitude (if no MAV_FRAME specified) / X coordinate according to MAV_FRAME. NaN: Use current vehicle position or current center if already orbiting.| Center point longitude (if no MAV_FRAME specified) / Y coordinate according to MAV_FRAME. NaN: Use current vehicle position or current center if already orbiting.| Center point altitude (MSL) (if no MAV_FRAME specified) / Z coordinate according to MAV_FRAME. NaN: Use current vehicle position or current center if already orbiting.| */ + MAV_CMD_DO_ORBIT=34, /* Start orbiting on the circumference of a circle defined by the parameters. Setting values to NaN/INT32_MAX (as appropriate) results in using defaults. |Radius of the circle. Positive: orbit clockwise. Negative: orbit counter-clockwise. NaN: Use vehicle default radius, or current radius if already orbiting.| Tangential Velocity. NaN: Use vehicle default velocity, or current velocity if already orbiting.| Yaw behavior of the vehicle.| Orbit around the centre point for this many radians (i.e. for a three-quarter orbit set 270*Pi/180). 0: Orbit forever. NaN: Use vehicle default, or current value if already orbiting.| Center point latitude (if no MAV_FRAME specified) / X coordinate according to MAV_FRAME. INT32_MAX (or NaN if sent in COMMAND_LONG): Use current vehicle position, or current center if already orbiting.| Center point longitude (if no MAV_FRAME specified) / Y coordinate according to MAV_FRAME. INT32_MAX (or NaN if sent in COMMAND_LONG): Use current vehicle position, or current center if already orbiting.| Center point altitude (MSL) (if no MAV_FRAME specified) / Z coordinate according to MAV_FRAME. NaN: Use current vehicle altitude.| */ MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode.| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to waypoint using a spline path. |Hold time. (ignored by fixed wing, time to stay at waypoint for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ MAV_CMD_NAV_VTOL_TAKEOFF=84, /* Takeoff from ground using VTOL mode, and transition to forward flight with specified heading. The command should be ignored by vehicles that dont support both VTOL and fixed-wing flight (multicopters, boats,etc.). |Empty| Front transition heading.| Empty| Yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_VTOL_LAND=85, /* Land using VTOL mode |Empty| Empty| Approach altitude (with the same reference as the Altitude field). NaN if unspecified.| Yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).| Latitude| Longitude| Altitude (ground level)| */ - MAV_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_NAV_VTOL_LAND=85, /* Land using VTOL mode |Landing behaviour.| Empty| Approach altitude (with the same reference as the Altitude field). NaN if unspecified.| Yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).| Latitude| Longitude| Altitude (ground level) relative to the current coordinate frame. NaN to use system default landing altitude (ignore value).| */ + MAV_CMD_NAV_GUIDED_ENABLE=92, /* Hand control over to an external controller |Guided mode on (MAV_BOOL_FALSE: Off). Values not equal to 0 or 1 are invalid.| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_NAV_DELAY=93, /* Delay the next navigation command a number of seconds or until a specified time |Delay (-1 to enable time-of-day fields)| hour (24h format, UTC, -1 to ignore)| minute (24h format, UTC, -1 to ignore)| second (24h format, UTC, -1 to ignore)| Empty| Empty| Empty| */ MAV_CMD_NAV_PAYLOAD_PLACE=94, /* Descend and place payload. Vehicle moves to specified location, descends until it detects a hanging payload has reached the ground, and then releases the payload. If ground is not detected before the reaching the maximum descent value (param1), the command will complete without releasing the payload. |Maximum distance to descend.| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend to target altitude at specified rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate.| Empty| Empty| Empty| Empty| Empty| Target Altitude| */ MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance.| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle, 0 is north| angular speed| direction: -1: counter clockwise, 1: clockwise| 0: absolute angle, 1: relative offset| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle [0-360]. Absolute angles: 0 is north. Relative angle: 0 is initial yaw. Direction set by param3.| angular speed| direction: -1: counter clockwise, 0: shortest direction, 1: clockwise| Relative offset (MAV_BOOL_FALSE: absolute angle). Values not equal to 0 or 1 are invalid.| Empty| Empty| Empty| */ MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode| Custom mode - this is system specific, please refer to the individual autopilot specifications for details.| Custom sub mode - this is system specific, please refer to the individual autopilot specifications for details.| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode flags. MAV_MODE values can be used to set some mode flag combinations.| Custom system-specific mode (see target autopilot specifications for mode information). If MAV_MODE_FLAG_CUSTOM_MODE_ENABLED is set in param1 (mode) this mode is used: otherwise the field is ignored.| Custom sub mode - this is system specific, please refer to the individual autopilot specifications for details.| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed, 2=Climb Speed, 3=Descent Speed)| Speed (-1 indicates no change)| Throttle (-1 indicates no change)| 0: absolute, 1: relative| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Yaw angle. NaN to use default heading| Latitude| Longitude| Altitude| */ + MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. The value persists until it is overridden or there is a mode change |Speed type of value set in param2 (such as airspeed, ground speed, and so on)| Speed (-1 indicates no change, -2 indicates return to default vehicle speed)| Throttle (-1 indicates no change, -2 indicates return to default vehicle throttle value)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_DO_SET_HOME=179, /* + Sets the home position to either to the current position or a specified position. + The home position is the default position that the system will return to and land on. + The position is set automatically by the system during the takeoff (and may also be set using this command). + Note: the current home position may be emitted in a HOME_POSITION message on request (using MAV_CMD_REQUEST_MESSAGE with param1=242). + |Use current location (MAV_BOOL_FALSE: use specified location). Values not equal to 0 or 1 are invalid.| Roll angle (of surface). Range: -180..180 degrees. NAN or 0 means value not set. 0.01 indicates zero roll.| Pitch angle (of surface). Range: -90..90 degrees. NAN or 0 means value not set. 0.01 means zero pitch.| Yaw angle. NaN to use default heading. Range: -180..180 degrees.| Latitude| Longitude| Altitude| */ MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay instance number.| Setting. (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cycles with a desired period. |Relay instance number.| Cycle count.| Cycle time.| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo instance number.| Pulse Width Modulation.| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo instance number.| Pulse Width Modulation.| Cycle count.| Cycle time.| Empty| Empty| Empty| */ - MAV_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately. + Flight termination immediately and irreversibly terminates the current flight, returning the vehicle to ground. + The vehicle will ignore RC or other input until it has been power-cycled. + Termination may trigger safety measures, including: disabling motors and deployment of parachute on multicopters, and setting flight surfaces to initiate a landing pattern on fixed-wing). + On multicopters without a parachute it may trigger a crash landing. + Support for this command can be tested using the protocol bit: MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION. + Support for this command can also be tested by sending the command with param1=0 (< 0.5); the ACK should be either MAV_RESULT_FAILED or MAV_RESULT_UNSUPPORTED. + |Flight termination activated if > 0.5. Otherwise not activated and ACK with MAV_RESULT_FAILED.| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_CHANGE_ALTITUDE=186, /* Change altitude set point. |Altitude.| Frame of new altitude.| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_SET_ACTUATOR=187, /* Sets actuators (e.g. servos) to a desired value. The actuator numbers are mapped to specific outputs (e.g. on any MAIN or AUX PWM or UAVCAN) using a flight-stack specific mechanism (i.e. a parameter). |Actuator 1 value, scaled from [-1 to 1]. NaN to ignore.| Actuator 2 value, scaled from [-1 to 1]. NaN to ignore.| Actuator 3 value, scaled from [-1 to 1]. NaN to ignore.| Actuator 4 value, scaled from [-1 to 1]. NaN to ignore.| Actuator 5 value, scaled from [-1 to 1]. NaN to ignore.| Actuator 6 value, scaled from [-1 to 1]. NaN to ignore.| Index of actuator set (i.e if set to 1, Actuator 1 becomes Actuator 7)| */ - MAV_CMD_DO_LAND_START=189, /* Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty| */ + MAV_CMD_DO_RETURN_PATH_START=188, /* Mission item to specify the start of a failsafe/landing return-path segment (the end of the segment is the next MAV_CMD_DO_LAND_START item). + A vehicle that is using missions for landing (e.g. in a return mode) will join the mission on the closest path of the return-path segment (instead of MAV_CMD_DO_LAND_START or the nearest waypoint). + The main use case is to minimize the failsafe flight path in corridor missions, where the inbound/outbound paths are constrained (by geofences) to the same particular path. + The MAV_CMD_NAV_RETURN_PATH_START would be placed at the start of the return path. + If a failsafe occurs on the outbound path the vehicle will move to the nearest point on the return path (which is parallel for this kind of mission), effectively turning round and following the shortest path to landing. + If a failsafe occurs on the inbound path the vehicle is already on the return segment and will continue to landing. + The Latitude/Longitude/Altitude are optional, and may be set to 0 if not needed. + If specified, the item defines the waypoint at which the return segment starts. + If sent using as a command, the vehicle will perform a mission landing (using the land segment if defined) or reject the command if mission landings are not supported, or no mission landing is defined. When used as a command any position information in the command is ignored. + |Empty| Empty| Empty| Empty| Latitudee. 0: not used.| Longitudee. 0: not used.| Altitudee. 0: not used.| */ + MAV_CMD_DO_LAND_START=189, /* Mission item to mark the start of a mission landing pattern, or a command to land with a mission landing pattern. + + When used in a mission, this is a marker for the start of a sequence of mission items that represent a landing pattern. + It should be followed by a navigation item that defines the first waypoint of the landing sequence. + The start marker positional params are used only for selecting what landing pattern to use if several are defined in the mission (the selected pattern will be the one with the marker position that is closest to the vehicle when a landing is commanded). + If the marker item position has zero-values for latitude, longitude, and altitude, then landing pattern selection is instead based on the position of the first waypoint in the landing sequence. + + When sent as a command it triggers a landing using a mission landing pattern. + The location parameters are not used in this case, and should be set to 0. + |Empty| Empty| Empty| Empty| Latitude for landing sequence selection, or 0 (see description). Ignored in commands (set 0).| Longitude for landing sequence selection, or 0 (see description). Ignored in commands (set 0).| Altitude for landing sequence selection, or 0 (see description). Ignored in commands (set 0).| */ MAV_CMD_DO_RALLY_LAND=190, /* Mission command to perform a landing from a rally point. |Break altitude| Landing speed| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_GO_AROUND=191, /* Mission command to safely abort an autonomous landing. |Altitude| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPOSITION=192, /* Reposition the vehicle to a specific WGS84 global position. |Ground speed, less than 0 (-1) for default| Bitmask of option flags.| Reserved| Yaw heading. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). For planes indicates loiter direction (0: clockwise, 1: counter clockwise)| Latitude| Longitude| Altitude| */ - MAV_CMD_DO_PAUSE_CONTINUE=193, /* If in a GPS controlled position mode, hold the current position or continue. |0: Pause current mission or reposition command, hold current position. 1: Continue mission. A VTOL capable vehicle should enter hover mode (multicopter and VTOL planes). A plane should loiter with the default loiter radius.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ - MAV_CMD_DO_SET_REVERSE=194, /* Set moving direction to forward or reverse. |Direction (0=Forward, 1=Reverse)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_REPOSITION=192, /* Reposition the vehicle to a specific WGS84 global position. This command is intended for guided commands (for missions use MAV_CMD_NAV_WAYPOINT instead). |Ground speed, less than 0 (-1) for default| Bitmask of option flags.| Loiter radius for planes. Positive values only, direction is controlled by Yaw value. A value of zero or NaN is ignored. | Yaw heading (heading reference defined in Bitmask field). NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). For planes indicates loiter direction (0: clockwise, 1: counter clockwise)| Latitude| Longitude| Altitude| */ + MAV_CMD_DO_PAUSE_CONTINUE=193, /* If in a GPS controlled position mode, hold the current position or continue. |Continue mission (MAV_BOOL_TRUE), Pause current mission or reposition command, hold current position (MAV_BOOL_FALSE). Values not equal to 0 or 1 are invalid. A VTOL capable vehicle should enter hover mode (multicopter and VTOL planes). A plane should loiter with the default loiter radius.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ + MAV_CMD_DO_SET_REVERSE=194, /* Set moving direction to forward or reverse. |Reverse direction (MAV_BOOL_FALSE: Forward direction). Values not equal to 0 or 1 are invalid.| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_SET_ROI_LOCATION=195, /* Sets the region of interest (ROI) to a location. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal is not to react to this message. |Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).| Empty| Empty| Empty| Latitude of ROI location| Longitude of ROI location| Altitude of ROI location| */ MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET=196, /* Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal device is not to react to this message. |Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).| Empty| Empty| Empty| Pitch offset from next waypoint, positive pitching up| Roll offset from next waypoint, positive rolling to the right| Yaw offset from next waypoint, positive yawing to the right| */ MAV_CMD_DO_SET_ROI_NONE=197, /* Cancels any previous ROI command returning the vehicle/sensors to default flight characteristics. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal device is not to react to this message. After this command the gimbal manager should go back to manual input if available, and otherwise assume a neutral position. |Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).| Empty| Empty| Empty| Empty| Empty| Empty| */ @@ -600,88 +787,159 @@ typedef enum MAV_CMD MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode.| Waypoint index/ target ID (depends on param 1).| Region of interest index. (allows a vehicle to manage multiple ROI's)| Empty| MAV_ROI_WPNEXT: pitch offset from next waypoint, MAV_ROI_LOCATION: latitude| MAV_ROI_WPNEXT: roll offset from next waypoint, MAV_ROI_LOCATION: longitude| MAV_ROI_WPNEXT: yaw offset from next waypoint, MAV_ROI_LOCATION: altitude| */ MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Configure digital camera. This is a fallback message for systems that have not yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html ). |Modes: P, TV, AV, M, Etc.| Shutter speed: Divisor number for one second.| Aperture: F stop number.| ISO number e.g. 80, 100, 200, Etc.| Exposure type enumerator.| Command Identity.| Main engine cut-off time before camera trigger. (0 means no cut-off)| */ MAV_CMD_DO_DIGICAM_CONTROL=203, /* Control digital camera. This is a fallback message for systems that have not yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html ). |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Test shot identifier. If set to 1, image will only be captured, but not counted towards internal frame count.| */ - MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| roll input (0 = angle body frame, 1 = angular rate, 2 = angle absolute frame)| pitch input (0 = angle body frame, 1 = angular rate, 2 = angle absolute frame)| yaw input (0 = angle body frame, 1 = angular rate, 2 = angle absolute frame)| */ + MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode| Stabilize roll (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid.| Stabilize pitch (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid.| Stabilize yaw (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid.| Roll input (0 = angle body frame, 1 = angular rate, 2 = angle absolute frame)| Pitch input (0 = angle body frame, 1 = angular rate, 2 = angle absolute frame)| Yaw input (0 = angle body frame, 1 = angular rate, 2 = angle absolute frame)| */ MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch depending on mount mode (degrees or degrees/second depending on pitch input).| roll depending on mount mode (degrees or degrees/second depending on roll input).| yaw depending on mount mode (degrees or degrees/second depending on yaw input).| altitude depending on mount mode.| latitude, set if appropriate mount mode.| longitude, set if appropriate mount mode.| Mount mode.| */ - MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set camera trigger distance for this flight. The camera is triggered each time this distance is exceeded. This command can also be used to set the shutter integration time for the camera. |Camera trigger distance. 0 to stop triggering.| Camera shutter integration time. -1 or 0 to ignore| Trigger camera once immediately. (0 = no trigger, 1 = trigger)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable, 2=disable_floor_only)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set camera trigger distance for this flight. The camera is triggered each time this distance is exceeded. This command can also be used to set the shutter integration time for the camera. |Camera trigger distance. 0 to stop triggering.| Camera shutter integration time. -1 or 0 to ignore| Trigger camera once, immediately (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid.| Target camera ID. 7 to 255: MAVLink camera component id. 1 to 6 for cameras attached to the autopilot, which don't have a distinct component id. 0: all cameras. This is used to target specific autopilot-connected cameras. It is also used to target specific cameras when the MAV_CMD is used in a mission.| Empty| Empty| Empty| */ + MAV_CMD_DO_FENCE_ENABLE=207, /* + Enable the geofence. + This can be used in a mission or via the command protocol. + The persistence/lifetime of the setting is undefined. + Depending on flight stack implementation it may persist until superseded, or it may revert to a system default at the end of a mission. + Flight stacks typically reset the setting to system defaults on reboot. + |enable? (0=disable, 1=enable, 2=disable_floor_only)| Fence types to enable or disable as a bitmask. 0: field is unused/all fences should be enabled or disabled (for compatibility reasons). Parameter is ignored if param1=2.| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_PARACHUTE=208, /* Mission item/command to release a parachute or enable/disable auto release. |Action| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_MOTOR_TEST=209, /* Mission command to perform motor test. |Motor instance number. (from 1 to max number of motors on the vehicle)| Throttle type.| Throttle.| Timeout.| Motor count. (number of motors to test to test in sequence, waiting for the timeout above between them; 0=1 motor, 1=1 motor, 2=2 motors...)| Motor test order.| Empty| */ - MAV_CMD_DO_INVERTED_FLIGHT=210, /* Change to/from inverted flight. |Inverted flight. (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_GRIPPER=211, /* Mission command to operate a gripper. |Gripper instance number.| Gripper action to perform.| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_AUTOTUNE_ENABLE=212, /* Enable/disable autotune. |Enable (1: enable, 0:disable).| Empty.| Empty.| Empty.| Empty.| Empty.| Empty.| */ - MAV_CMD_NAV_SET_YAW_SPEED=213, /* Sets a desired vehicle turn angle and speed change. |Yaw angle to adjust steering by.| Speed.| Final angle. (0=absolute, 1=relative)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL=214, /* Mission command to set camera trigger interval for this flight. If triggering is enabled, the camera is triggered each time this interval expires. This command can also be used to set the shutter integration time for the camera. |Camera trigger cycle time. -1 or 0 to ignore.| Camera shutter integration time. Should be less than trigger cycle time. -1 or 0 to ignore.| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_MOTOR_TEST=209, /* Command to perform motor test. |Motor instance number (from 1 to max number of motors on the vehicle).| Throttle type (whether the Throttle Value in param3 is a percentage, PWM value, etc.)| Throttle value.| Timeout between tests that are run in sequence.| Motor count. Number of motors to test in sequence: 0/1=one motor, 2= two motors, etc. The Timeout (param4) is used between tests.| Motor test order.| Empty| */ + MAV_CMD_DO_INVERTED_FLIGHT=210, /* Change to/from inverted flight. |Inverted flight (MAV_BOOL_False: normal flight). Values not equal to 0 or 1 are invalid.| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_GRIPPER=211, /* Mission command to operate a gripper. |Gripper ID. 1-6 for an autopilot connected gripper. In missions this may be set to 1-6 for an autopilot gripper, or the gripper component id for a MAVLink gripper. 0 targets all grippers.| Gripper action to perform.| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_AUTOTUNE_ENABLE=212, /* Enable/disable autotune. |Enable autotune (MAV_BOOL_FALSE: disable autotune). Values not equal to 0 or 1 are invalid.| Specify axes for which autotuning is enabled/disabled. 0 indicates the field is unused (for compatibility reasons). If 0 the autopilot will follow its default behaviour, which is usually to tune all axes.| Empty.| Empty.| Empty.| Empty.| Empty.| */ + MAV_CMD_NAV_SET_YAW_SPEED=213, /* Sets a desired vehicle turn angle and speed change. |Yaw angle to adjust steering by.| Speed.| Relative final angle (MAV_BOOL_FALSE: Absolute angle). Values not equal to 0 or 1 are invalid.| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL=214, /* Mission command to set camera trigger interval for this flight. If triggering is enabled, the camera is triggered each time this interval expires. This command can also be used to set the shutter integration time for the camera. |Camera trigger cycle time. -1 or 0 to ignore.| Camera shutter integration time. Should be less than trigger cycle time. -1 or 0 to ignore.| Target camera ID. 7 to 255: MAVLink camera component id. 1 to 6 for cameras attached to the autopilot, which don't have a distinct component id. 0: all cameras. This is used to target specific autopilot-connected cameras. It is also used to target specific cameras when the MAV_CMD is used in a mission.| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |quaternion param q1, w (1 in null-rotation)| quaternion param q2, x (0 in null-rotation)| quaternion param q3, y (0 in null-rotation)| quaternion param q4, z (0 in null-rotation)| Empty| Empty| Empty| */ MAV_CMD_DO_GUIDED_MASTER=221, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_GUIDED_LIMITS=222, /* Set limits for external control |Timeout - maximum time that external controller will be allowed to control vehicle. 0 means no timeout.| Altitude (MSL) min - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit.| Altitude (MSL) max - if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit.| Horizontal move limit - if vehicle moves more than this distance from its location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal move limit.| Empty| Empty| Empty| */ - MAV_CMD_DO_ENGINE_CONTROL=223, /* Control vehicle engine. This is interpreted by the vehicles engine controller to change the target engine state. It is intended for vehicles with internal combustion engines |0: Stop engine, 1:Start Engine| 0: Warm start, 1:Cold start. Controls use of choke where applicable| Height delay. This is for commanding engine start only after the vehicle has gained the specified height. Used in VTOL vehicles during takeoff to start engine after the aircraft is off the ground. Zero for no delay.| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MISSION_CURRENT=224, /* Set the mission item with sequence number seq as current item. This means that the MAV will continue to this mission item on the shortest path (not following the mission items in-between). |Mission sequence value to set| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_ENGINE_CONTROL=223, /* Control vehicle engine. This is interpreted by the vehicles engine controller to change the target engine state. It is intended for vehicles with internal combustion engines |Start engine (MAV_BOOL_False: Stop engine). Values not equal to 0 or 1 are invalid.| Cold start engine (MAV_BOOL_FALSE: Warm start). Values not equal to 0 or 1 are invalid. Controls use of choke where applicable| Height delay. This is for commanding engine start only after the vehicle has gained the specified height. Used in VTOL vehicles during takeoff to start engine after the aircraft is off the ground. Zero for no delay.| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_MISSION_CURRENT=224, /* + Set the mission item with sequence number seq as the current item and emit MISSION_CURRENT (whether or not the mission number changed). + If a mission is currently being executed, the system will continue to this new mission item on the shortest path, skipping any intermediate mission items. + Note that mission jump repeat counters are not reset unless param2 is set (see MAV_CMD_DO_JUMP param2). + + This command may trigger a mission state-machine change on some systems: for example from MISSION_STATE_NOT_STARTED or MISSION_STATE_PAUSED to MISSION_STATE_ACTIVE. + If the system is in mission mode, on those systems this command might therefore start, restart or resume the mission. + If the system is not in mission mode this command must not trigger a switch to mission mode. + + The mission may be "reset" using param2. + Resetting sets jump counters to initial values (to reset counters without changing the current mission item set the param1 to `-1`). + Resetting also explicitly changes a mission state of MISSION_STATE_COMPLETE to MISSION_STATE_PAUSED or MISSION_STATE_ACTIVE, potentially allowing it to resume when it is (next) in a mission mode. + + The command will ACK with MAV_RESULT_FAILED if the sequence number is out of range (including if there is no mission item). + |Mission sequence value to set. -1 for the current mission item (use to reset mission without changing current mission item).| Reset mission (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid. Resets jump counters to initial values and changes mission state "completed" to be "active" or "paused".| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. Except for Temperature Calibration, only one sensor should be set in a single message and all others should be zero. |1: gyro calibration, 3: gyro temperature calibration| 1: magnetometer calibration| 1: ground pressure calibration| 1: radio RC calibration, 2: RC trim calibration| 1: accelerometer calibration, 2: board level calibration, 3: accelerometer temperature calibration, 4: simple accelerometer calibration| 1: APM: compass/motor interference calibration (PX4: airspeed calibration, deprecated), 2: airspeed calibration| 1: ESC calibration, 3: barometer temperature calibration| */ + MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. Except for Temperature Calibration, only one sensor should be set in a single message and all others should be zero. |1: gyro calibration, 3: gyro temperature calibration| Magnetometer calibration. Values not equal to 0 or 1 are invalid.| Ground pressure calibration. Values not equal to 0 or 1 are invalid.| 1: radio RC calibration, 2: RC trim calibration| 1: accelerometer calibration, 2: board level calibration, 3: accelerometer temperature calibration, 4: simple accelerometer calibration| 1: APM: compass/motor interference calibration (PX4: airspeed calibration, deprecated), 2: airspeed calibration| 1: ESC calibration, 3: barometer temperature calibration| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow, 5: second magnetometer, 6: third magnetometer| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ MAV_CMD_PREFLIGHT_UAVCAN=243, /* Trigger UAVCAN configuration (actuator ID assignment and direction mapping). Note that this maps to the legacy UAVCAN v0 function UAVCAN_ENUMERATE, which is intended to be executed just once during initial vehicle configuration (it is not a normal pre-flight command and has been poorly named). |1: Trigger actuator ID assignment and direction mapping. 0: Cancel command.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ - MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM, 2: Reset to defaults| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM, 2: Reset to defaults| Onboard logging: 0: Ignore, 1: Start default rate logging, -1: Stop logging, > 1: logging rate (e.g. set to 1000 for 1000 Hz logging)| Reserved| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot, 3: Reboot autopilot and keep it in the bootloader until upgraded.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer, 3: Reboot onboard computer and keep it in the bootloader until upgraded.| WIP: 0: Do nothing for camera, 1: Reboot onboard camera, 2: Shutdown onboard camera, 3: Reboot onboard camera and keep it in the bootloader until upgraded| WIP: 0: Do nothing for mount (e.g. gimbal), 1: Reboot mount, 2: Shutdown mount, 3: Reboot mount and keep it in the bootloader until upgraded| Reserved (set to 0)| Reserved (set to 0)| WIP: ID (e.g. camera ID -1 for all IDs)| */ - MAV_CMD_DO_UPGRADE=247, /* Request a target system to start an upgrade of one (or all) of its components. For example, the command might be sent to a companion computer to cause it to upgrade a connected flight controller. The system doing the upgrade will report progress using the normal command protocol sequence for a long running operation. Command protocol information: https://mavlink.io/en/services/command.html. |Component id of the component to be upgraded. If set to 0, all components should be upgraded.| 0: Do not reboot component after the action is executed, 1: Reboot component after the action is executed.| Reserved| Reserved| Reserved| Reserved| WIP: upgrade progress report rate (can be used for more granular control).| */ + MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Action to perform on the persistent parameter storage| Action to perform on the persistent mission storage| Onboard logging: 0: Ignore, 1: Start default rate logging, -1: Stop logging, > 1: logging rate (e.g. set to 1000 for 1000 Hz logging)| Reserved| Empty| Empty| Empty| */ + MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |Action to take for autopilot.| Action to take for onboard computer.| Action to take for component specified in param4.| MAVLink Component ID targeted in param3 (0 for all components).| Reserved (set to 0)| Conditions under which reboot/shutdown is allowed.| WIP: ID (e.g. camera ID -1 for all IDs)| */ MAV_CMD_OVERRIDE_GOTO=252, /* Override current mission with command to pause mission, pause mission and move to position, continue/resume mission. When param 1 indicates that the mission is paused (MAV_GOTO_DO_HOLD), param 2 defines whether it holds in place or moves to another position. |MAV_GOTO_DO_HOLD: pause mission and either hold or move to specified position (depending on param2), MAV_GOTO_DO_CONTINUE: resume mission.| MAV_GOTO_HOLD_AT_CURRENT_POSITION: hold at current position, MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position.| Coordinate frame of hold point.| Desired yaw angle.| Latitude/X position.| Longitude/Y position.| Altitude/Z position.| */ MAV_CMD_OBLIQUE_SURVEY=260, /* Mission command to set a Camera Auto Mount Pivoting Oblique Survey (Replaces CAM_TRIGG_DIST for this purpose). The camera is triggered each time this distance is exceeded, then the mount moves to the next position. Params 4~6 set-up the angle limits and number of positions for oblique survey, where mount-enabled vehicles automatically roll the camera between shots to emulate an oblique camera setup (providing an increased HFOV). This command can also be used to set the shutter integration time for the camera. |Camera trigger distance. 0 to stop triggering.| Camera shutter integration time. 0 to ignore| The minimum interval in which the camera is capable of taking subsequent pictures repeatedly. 0 to ignore.| Total number of roll positions at which the camera will capture photos (images captures spread evenly across the limits defined by param5).| Angle limits that the camera can be rolled to left and right of center.| Fixed pitch angle that the camera will hold in oblique mode if the mount is actuated in the pitch axis.| Empty| */ + MAV_CMD_DO_SET_STANDARD_MODE=262, /* Enable the specified standard MAVLink mode. + If the specified mode is not supported, the vehicle should ACK with MAV_RESULT_FAILED. + See https://mavlink.io/en/services/standard_modes.html + |The mode to set.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:NaN)| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |0: disarm, 1: arm| 0: arm-disarm unless prevented by safety checks (i.e. when landed), 21196: force arming/disarming (e.g. allow arming to override preflight checks and disarming in flight)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ILLUMINATOR_ON_OFF=405, /* Turns illuminators ON/OFF. An illuminator is a light source that is used for lighting up dark areas external to the sytstem: e.g. a torch or searchlight (as opposed to a light source for illuminating the system itself, e.g. an indicator light). |0: Illuminators OFF, 1: Illuminators ON| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_GET_HOME_POSITION=410, /* Request the home position from the vehicle. |Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ + MAV_CMD_ACTUATOR_TEST=310, /* Actuator testing command. This is similar to MAV_CMD_DO_MOTOR_TEST but operates on the level of output functions, i.e. it is possible to test Motor1 independent from which output it is configured on. Autopilots must NACK this command with MAV_RESULT_TEMPORARILY_REJECTED while armed. |Output value: 1 means maximum positive output, 0 to center servos or minimum motor thrust (expected to spin), -1 for maximum negative (if not supported by the motors, i.e. motor is not reversible, smaller than 0 maps to NaN). And NaN maps to disarmed (stop the motors).| Timeout after which the test command expires and the output is restored to the previous value. A timeout has to be set for safety reasons. A timeout of 0 means to restore the previous value immediately.| Reserved (default:0)| Reserved (default:0)| Actuator Output function| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_CONFIGURE_ACTUATOR=311, /* Actuator configuration command. |Actuator configuration action| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Actuator Output function| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |Arm (MAV_BOOL_FALSE: disarm). Values not equal to 0 or 1 are invalid.| 0: arm-disarm unless prevented by safety checks (i.e. when landed), 21196: force arming/disarming (e.g. allow arming to override preflight checks and disarming in flight)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_RUN_PREARM_CHECKS=401, /* Instructs a target system to run pre-arm checks. + This allows preflight checks to be run on demand, which may be useful on systems that normally run them at low rate, or which do not trigger checks when the armable state might have changed. + This command should return MAV_RESULT_ACCEPTED if it will run the checks. + The results of the checks are usually then reported in SYS_STATUS messages (this is system-specific). + The command should return MAV_RESULT_TEMPORARILY_REJECTED if the system is already armed. + |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ILLUMINATOR_ON_OFF=405, /* Turns illuminators ON/OFF. An illuminator is a light source that is used for lighting up dark areas external to the system: e.g. a torch or searchlight (as opposed to a light source for illuminating the system itself, e.g. an indicator light). |Illuminators on/off (MAV_BOOL_TRUE: illuminators on). Values not equal to 0 or 1 are invalid.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_DO_ILLUMINATOR_CONFIGURE=406, /* Configures illuminator settings. An illuminator is a light source that is used for lighting up dark areas external to the system: e.g. a torch or searchlight (as opposed to a light source for illuminating the system itself, e.g. an indicator light). |Mode| 0%: Off, 100%: Max Brightness| Strobe period in seconds where 0 means strobing is not used| Strobe duty cycle where 100% means it is on constantly and 0 means strobing is not used| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_GET_HOME_POSITION=410, /* Request the home position from the vehicle. + The vehicle will ACK the command and then emit the HOME_POSITION message. |Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ MAV_CMD_INJECT_FAILURE=420, /* Inject artificial failure for testing purposes. Note that autopilots should implement an additional protection before accepting this command such as a specific param setting. |The unit which is affected by the failure.| The type how the failure manifests itself.| Instance affected by failure (0 to signal all).| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing. |0:Spektrum.| RC type.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_GET_MESSAGE_INTERVAL=510, /* Request the interval between messages for a particular MAVLink message ID. The receiver should ACK the command and then emit its response in a MESSAGE_INTERVAL message. |The MAVLink message ID| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_SET_MESSAGE_INTERVAL=511, /* Set the interval between messages for a particular MAVLink message ID. This interface replaces REQUEST_DATA_STREAM. |The MAVLink message ID| The interval between two messages. Set to -1 to disable and 0 to request default rate.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Target address of message stream (if message has target address fields). 0: Flight-stack default (recommended), 1: address of requestor, 2: broadcast.| */ - MAV_CMD_REQUEST_MESSAGE=512, /* Request the target system(s) emit a single instance of a specified message (i.e. a "one-shot" version of MAV_CMD_SET_MESSAGE_INTERVAL). |The MAVLink message ID of the requested message.| Use for index ID, if required. Otherwise, the use of this parameter (if any) must be defined in the requested message. By default assumed not used (0).| The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0).| The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0).| The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0).| The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0).| Target address for requested message (if message has target address fields). 0: Flight-stack default, 1: address of requestor, 2: broadcast.| */ - MAV_CMD_REQUEST_PROTOCOL_VERSION=519, /* Request MAVLink protocol version compatibility. All receivers should ACK the command and then emit their capabilities in an PROTOCOL_VERSION message |1: Request supported protocol versions by all nodes on the network| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES=520, /* Request autopilot capabilities. The receiver should ACK the command and then emit its capabilities in an AUTOPILOT_VERSION message |1: Request autopilot version| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_REQUEST_CAMERA_INFORMATION=521, /* Request camera information (CAMERA_INFORMATION). |0: No action 1: Request camera capabilities| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_REQUEST_CAMERA_SETTINGS=522, /* Request camera settings (CAMERA_SETTINGS). |0: No Action 1: Request camera settings| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_REQUEST_STORAGE_INFORMATION=525, /* Request storage information (STORAGE_INFORMATION). Use the command's target_component to target a specific component's storage. |Storage ID (0 for all, 1 for first, 2 for second, etc.)| 0: No Action 1: Request storage information| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_STORAGE_FORMAT=526, /* Format a storage medium. Once format is complete, a STORAGE_INFORMATION message is sent. Use the command's target_component to target a specific component's storage. |Storage ID (1 for first, 2 for second, etc.)| Format storage (and reset image log). 0: No action 1: Format storage| Reset Image Log (without formatting storage medium). This will reset CAMERA_CAPTURE_STATUS.image_count and CAMERA_IMAGE_CAPTURED.image_index. 0: No action 1: Reset Image Log| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS=527, /* Request camera capture status (CAMERA_CAPTURE_STATUS) |0: No Action 1: Request camera capture status| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_REQUEST_FLIGHT_INFORMATION=528, /* Request flight information (FLIGHT_INFORMATION) |1: Request flight information| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_RESET_CAMERA_SETTINGS=529, /* Reset all camera settings to Factory Default |0: No Action 1: Reset all settings| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_SET_CAMERA_MODE=530, /* Set camera running mode. Use NaN for reserved values. GCS will send a MAV_CMD_REQUEST_VIDEO_STREAM_STATUS command after a mode change if the camera supports video streaming. |Reserved (Set to 0)| Camera mode| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:0)| Reserved (default:0)| Reserved (default:NaN)| */ - MAV_CMD_SET_CAMERA_ZOOM=531, /* Set camera zoom. Camera must respond with a CAMERA_SETTINGS message (on success). |Zoom type| Zoom value. The range of valid values depend on the zoom type.| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:0)| Reserved (default:0)| Reserved (default:NaN)| */ - MAV_CMD_SET_CAMERA_FOCUS=532, /* Set camera focus. Camera must respond with a CAMERA_SETTINGS message (on success). |Focus type| Focus value| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:0)| Reserved (default:0)| Reserved (default:NaN)| */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing. |RC type.| RC sub type.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_GET_MESSAGE_INTERVAL=510, /* + Request the interval between messages for a particular MAVLink message ID. + The receiver should ACK the command and then emit its response in a MESSAGE_INTERVAL message. + |The MAVLink message ID| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_SET_MESSAGE_INTERVAL=511, /* Set the interval between messages for a particular MAVLink message ID. This interface replaces REQUEST_DATA_STREAM. |The MAVLink message ID| The interval between two messages. -1: disable. 0: request default rate (which may be zero).| Use for index ID, if required. Otherwise, the use of this parameter (if any) must be defined in the requested message. By default assumed not used (0). When used as an index ID, 0 means "all instances", "1" means the first instance in the sequence (the emitted message will have an id of 0 if message ids are 0-indexed, or 1 if index numbers start from one).| The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0).| The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0/NaN).| The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0/NaN).| Target address of message stream (if message has target address fields). 0: Flight-stack default (recommended), 1: address of requester, 2: broadcast.| */ + MAV_CMD_REQUEST_MESSAGE=512, /* Request the target system(s) emit a single instance of a specified message (i.e. a "one-shot" version of MAV_CMD_SET_MESSAGE_INTERVAL). |The MAVLink message ID of the requested message.| Use for index ID, if required. Otherwise, the use of this parameter (if any) must be defined in the requested message. By default assumed not used (0).| The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0).| The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0).| The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0).| The use of this parameter (if any), must be defined in the requested message. By default assumed not used (0).| Target address for requested message (if message has target address fields). 0: Flight-stack default, 1: address of requester, 2: broadcast.| */ + MAV_CMD_REQUEST_PROTOCOL_VERSION=519, /* Request MAVLink protocol version compatibility. All receivers should ACK the command and then emit their capabilities in an PROTOCOL_VERSION message |Request supported protocol versions by all nodes on the network (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid.| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES=520, /* Request autopilot capabilities. The receiver should ACK the command and then emit its capabilities in an AUTOPILOT_VERSION message |Request autopilot version (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid.| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_REQUEST_CAMERA_INFORMATION=521, /* Request camera information (CAMERA_INFORMATION). |Request camera capabilities (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid.| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_REQUEST_CAMERA_SETTINGS=522, /* Request camera settings (CAMERA_SETTINGS). |Request camera settings (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid.| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_REQUEST_STORAGE_INFORMATION=525, /* Request storage information (STORAGE_INFORMATION). Use the command's target_component to target a specific component's storage. |Storage ID (0 for all, 1 for first, 2 for second, etc.)| Request storage information (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid.| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_STORAGE_FORMAT=526, /* Format a storage medium. Once format is complete, a STORAGE_INFORMATION message is sent. Use the command's target_component to target a specific component's storage. |Storage ID (1 for first, 2 for second, etc.)| Format storage (and reset image log). Values not equal to 0 or 1 are invalid.| Reset Image Log (without formatting storage medium). This will reset CAMERA_CAPTURE_STATUS.image_count and CAMERA_IMAGE_CAPTURED.image_index. Values not equal to 0 or 1 are invalid.| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS=527, /* Request camera capture status (CAMERA_CAPTURE_STATUS) |Request camera capture status (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid.| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_REQUEST_FLIGHT_INFORMATION=528, /* Request flight information (FLIGHT_INFORMATION) |Request flight information (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid.| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_RESET_CAMERA_SETTINGS=529, /* Reset all camera settings to Factory Default |Reset all settings (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid.| Target camera ID. 7 to 255: MAVLink camera component id. 1 to 6 for cameras attached to the autopilot, which don't have a distinct component id. 0: all cameras. This is used to target specific autopilot-connected cameras. It is also used to target specific cameras when the MAV_CMD is used in a mission.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_SET_CAMERA_MODE=530, /* Set camera running mode. Use NaN for reserved values. GCS will send a MAV_CMD_REQUEST_VIDEO_STREAM_STATUS command after a mode change if the camera supports video streaming. |Target camera ID. 7 to 255: MAVLink camera component id. 1 to 6 for cameras attached to the autopilot, which don't have a distinct component id. 0: all cameras. This is used to target specific autopilot-connected cameras. It is also used to target specific cameras when the MAV_CMD is used in a mission.| Camera mode| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:0)| Reserved (default:0)| Reserved (default:NaN)| */ + MAV_CMD_SET_CAMERA_ZOOM=531, /* Set camera zoom. Camera must respond with a CAMERA_SETTINGS message (on success). |Zoom type| Zoom value. The range of valid values depend on the zoom type.| Target camera ID. 7 to 255: MAVLink camera component id. 1 to 6 for cameras attached to the autopilot, which don't have a distinct component id. 0: all cameras. This is used to target specific autopilot-connected cameras. It is also used to target specific cameras when the MAV_CMD is used in a mission.| Reserved (default:NaN)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_SET_CAMERA_FOCUS=532, /* Set camera focus. Camera must respond with a CAMERA_SETTINGS message (on success). |Focus type| Focus value| Target camera ID. 7 to 255: MAVLink camera component id. 1 to 6 for cameras attached to the autopilot, which don't have a distinct component id. 0: all cameras. This is used to target specific autopilot-connected cameras. It is also used to target specific cameras when the MAV_CMD is used in a mission.| Reserved (default:NaN)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_SET_STORAGE_USAGE=533, /* Set that a particular storage is the preferred location for saving photos, videos, and/or other media (e.g. to set that an SD card is used for storing videos). + There can only be one preferred save location for each particular media type: setting a media usage flag will clear/reset that same flag if set on any other storage. + If no flag is set the system should use its default storage. + A target system can choose to always use default storage, in which case it should ACK the command with MAV_RESULT_UNSUPPORTED. + A target system can choose to not allow a particular storage to be set as preferred storage, in which case it should ACK the command with MAV_RESULT_DENIED. |Storage ID (1 for first, 2 for second, etc.)| Usage flags| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_SET_CAMERA_SOURCE=534, /* Set camera source. Changes the camera's active sources on cameras with multiple image sensors. |Component Id of camera to address or 1-6 for non-MAVLink cameras, 0 for all cameras.| Primary Source| Secondary Source. If non-zero the second source will be displayed as picture-in-picture.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ MAV_CMD_JUMP_TAG=600, /* Tagged jump target. Can be jumped to with MAV_CMD_DO_JUMP_TAG. |Tag.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ MAV_CMD_DO_JUMP_TAG=601, /* Jump to the matching tag in the mission list. Repeat this action for the specified number of times. A mission should contain a single matching tag for each jump. If this is not the case then a jump to a missing tag should complete the mission, and a jump where there are multiple matching tags should always select the one with the lowest mission sequence number. |Target tag to jump to.| Repeat count.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_PARAM_TRANSACTION=900, /* Request to start or end a parameter transaction. Multiple kinds of transport layers can be used to exchange parameters in the transaction (param, param_ext and mavftp). The command response can either be a success/failure or an in progress in case the receiving side takes some time to apply the parameters. |Action to be performed (start, commit, cancel, etc.)| Possible transport layers to set and get parameters via mavlink during a parameter transaction.| Identifier for a specific transaction.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW=1000, /* High level setpoint to be sent to a gimbal manager to set a gimbal attitude. It is possible to set combinations of the values below. E.g. an angle as well as a desired angular rate can be used to get to this angle at a certain angular rate, or an angular rate only will result in continuous turning. NaN is to be used to signal unset. Note: a gimbal is never to react to this command but only the gimbal manager. |Pitch angle (positive to pitch up, relative to vehicle for FOLLOW mode, relative to world horizon for LOCK mode).| Yaw angle (positive to yaw to the right, relative to vehicle for FOLLOW mode, absolute to North for LOCK mode).| Pitch rate (positive to pitch up).| Yaw rate (positive to yaw to the right).| Gimbal manager flags to use.| Reserved (default:0)| Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).| */ + MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW=1000, /* Set gimbal manager pitch/yaw setpoints (low rate command). It is possible to set combinations of the values below. E.g. an angle as well as a desired angular rate can be used to get to this angle at a certain angular rate, or an angular rate only will result in continuous turning. NaN is to be used to signal unset. Note: only the gimbal manager will react to this command - it will be ignored by a gimbal device. Use GIMBAL_MANAGER_SET_PITCHYAW if you need to stream pitch/yaw setpoints at higher rate. |Pitch angle (positive to pitch up, relative to vehicle for FOLLOW mode, relative to world horizon for LOCK mode).| Yaw angle (positive to yaw to the right, relative to vehicle for FOLLOW mode, absolute to North for LOCK mode).| Pitch rate (positive to pitch up).| Yaw rate (positive to yaw to the right).| Gimbal manager flags to use.| Reserved (default:0)| Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).| */ MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE=1001, /* Gimbal configuration to set which sysid/compid is in primary and secondary control. |Sysid for primary control (0: no one in control, -1: leave unchanged, -2: set itself in control (for missions where the own sysid is still unknown), -3: remove control if currently in control).| Compid for primary control (0: no one in control, -1: leave unchanged, -2: set itself in control (for missions where the own sysid is still unknown), -3: remove control if currently in control).| Sysid for secondary control (0: no one in control, -1: leave unchanged, -2: set itself in control (for missions where the own sysid is still unknown), -3: remove control if currently in control).| Compid for secondary control (0: no one in control, -1: leave unchanged, -2: set itself in control (for missions where the own sysid is still unknown), -3: remove control if currently in control).| Reserved (default:0)| Reserved (default:0)| Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).| */ - MAV_CMD_IMAGE_START_CAPTURE=2000, /* Start image capture sequence. Sends CAMERA_IMAGE_CAPTURED after each capture. Use NaN for reserved values. |Reserved (Set to 0)| Desired elapsed time between two consecutive pictures (in seconds). Minimum values depend on hardware (typically greater than 2 seconds).| Total number of images to capture. 0 to capture forever/until MAV_CMD_IMAGE_STOP_CAPTURE.| Capture sequence number starting from 1. This is only valid for single-capture (param3 == 1), otherwise set to 0. Increment the capture ID for each capture command to prevent double captures when a command is re-transmitted.| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| */ - MAV_CMD_IMAGE_STOP_CAPTURE=2001, /* Stop image capture sequence Use NaN for reserved values. |Reserved (Set to 0)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:0)| Reserved (default:0)| Reserved (default:NaN)| */ + MAV_CMD_IMAGE_START_CAPTURE=2000, /* Start image capture sequence. CAMERA_IMAGE_CAPTURED must be emitted after each capture. + + Param1 (id) may be used to specify the target camera: 0: all cameras, 1 to 6: autopilot-connected cameras, 7-255: MAVLink camera component ID. + It is needed in order to target specific cameras connected to the autopilot, or specific sensors in a multi-sensor camera (neither of which have a distinct MAVLink component ID). + It is also needed to specify the target camera in missions. + + When used in a mission, an autopilot should execute the MAV_CMD for a specified local camera (param1 = 1-6), or resend it as a command if it is intended for a MAVLink camera (param1 = 7 - 255), setting the command's target_component as the param1 value (and setting param1 in the command to zero). + If the param1 is 0 the autopilot should do both. + + When sent in a command the target MAVLink address is set using target_component. + If addressed specifically to an autopilot: param1 should be used in the same way as it is for missions (though command should NACK with MAV_RESULT_DENIED if a specified local camera does not exist). + If addressed to a MAVLink camera, param 1 can be used to address all cameras (0), or to separately address 1 to 7 individual sensors. Other values should be NACKed with MAV_RESULT_DENIED. + If the command is broadcast (target_component is 0) then param 1 should be set to 0 (any other value should be NACKED with MAV_RESULT_DENIED). An autopilot would trigger any local cameras and forward the command to all channels. + |Target camera ID. 7 to 255: MAVLink camera component id. 1 to 6 for cameras attached to the autopilot, which don't have a distinct component id. 0: all cameras. This is used to target specific autopilot-connected cameras. It is also used to target specific cameras when the MAV_CMD is used in a mission.| Desired elapsed time between two consecutive pictures (in seconds). Minimum values depend on hardware (typically greater than 2 seconds).| Total number of images to capture. 0 to capture forever/until MAV_CMD_IMAGE_STOP_CAPTURE.| Capture sequence number starting from 1. This is only valid for single-capture (param3 == 1), otherwise set to 0. Increment the capture ID for each capture command to prevent double captures when a command is re-transmitted.| Reserved (default:0)| Reserved (default:0)| Reserved (default:NaN)| */ + MAV_CMD_IMAGE_STOP_CAPTURE=2001, /* Stop image capture sequence. + + Param1 (id) may be used to specify the target camera: 0: all cameras, 1 to 6: autopilot-connected cameras, 7-255: MAVLink camera component ID. + It is needed in order to target specific cameras connected to the autopilot, or specific sensors in a multi-sensor camera (neither of which have a distinct MAVLink component ID). + It is also needed to specify the target camera in missions. + + When used in a mission, an autopilot should execute the MAV_CMD for a specified local camera (param1 = 1-6), or resend it as a command if it is intended for a MAVLink camera (param1 = 7 - 255), setting the command's target_component as the param1 value (and setting param1 in the command to zero). + If the param1 is 0 the autopilot should do both. + + When sent in a command the target MAVLink address is set using target_component. + If addressed specifically to an autopilot: param1 should be used in the same way as it is for missions (though command should NACK with MAV_RESULT_DENIED if a specified local camera does not exist). + If addressed to a MAVLink camera, param1 can be used to address all cameras (0), or to separately address 1 to 7 individual sensors. Other values should be NACKed with MAV_RESULT_DENIED. + If the command is broadcast (target_component is 0) then param 1 should be set to 0 (any other value should be NACKED with MAV_RESULT_DENIED). An autopilot would trigger any local cameras and forward the command to all channels. + |Target camera ID. 7 to 255: MAVLink camera component id. 1 to 6 for cameras attached to the autopilot, which don't have a distinct component id. 0: all cameras. This is used to target specific autopilot-connected cameras. It is also used to target specific cameras when the MAV_CMD is used in a mission.| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:0)| Reserved (default:0)| Reserved (default:NaN)| */ MAV_CMD_REQUEST_CAMERA_IMAGE_CAPTURE=2002, /* Re-request a CAMERA_IMAGE_CAPTURED message. |Sequence number for missing CAMERA_IMAGE_CAPTURED message| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:0)| Reserved (default:0)| Reserved (default:NaN)| */ - MAV_CMD_DO_TRIGGER_CONTROL=2003, /* Enable or disable on-board camera triggering system. |Trigger enable/disable (0 for disable, 1 for start), -1 to ignore| 1 to reset the trigger sequence, -1 or 0 to ignore| 1 to pause triggering, but without switching the camera off or retracting it. -1 to ignore| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_CAMERA_TRACK_POINT=2004, /* If the camera supports point visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_POINT is set), this command allows to initiate the tracking. |Point to track x value (normalized 0..1, 0 is left, 1 is right).| Point to track y value (normalized 0..1, 0 is top, 1 is bottom).| Point radius (normalized 0..1, 0 is image left, 1 is image right).| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_CAMERA_TRACK_RECTANGLE=2005, /* If the camera supports rectangle visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE is set), this command allows to initiate the tracking. |Top left corner of rectangle x value (normalized 0..1, 0 is left, 1 is right).| Top left corner of rectangle y value (normalized 0..1, 0 is top, 1 is bottom).| Bottom right corner of rectangle x value (normalized 0..1, 0 is left, 1 is right).| Bottom right corner of rectangle y value (normalized 0..1, 0 is top, 1 is bottom).| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_CAMERA_STOP_TRACKING=2010, /* Stops ongoing tracking. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_VIDEO_START_CAPTURE=2500, /* Starts video capture (recording). |Video Stream ID (0 for all streams)| Frequency CAMERA_CAPTURE_STATUS messages should be sent while recording (0 for no messages, otherwise frequency)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| */ - MAV_CMD_VIDEO_STOP_CAPTURE=2501, /* Stop the current video capture (recording). |Video Stream ID (0 for all streams)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| */ - MAV_CMD_VIDEO_START_STREAMING=2502, /* Start video streaming |Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_VIDEO_STOP_STREAMING=2503, /* Stop the given video stream |Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_DO_TRIGGER_CONTROL=2003, /* Enable or disable on-board camera triggering system. |Trigger enable/disable (0 for disable, 1 for start), -1 to ignore| 1 to reset the trigger sequence, -1 or 0 to ignore| 1 to pause triggering, but without switching the camera off or retracting it. -1 to ignore| Target camera ID. 7 to 255: MAVLink camera component id. 1 to 6 for cameras attached to the autopilot, which don't have a distinct component id. 0: all cameras. This is used to target specific autopilot-connected cameras. It is also used to target specific cameras when the MAV_CMD is used in a mission.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_CAMERA_TRACK_POINT=2004, /* If the camera supports point visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_POINT is set), this command allows to initiate the tracking. |Point to track x value (normalized 0..1, 0 is left, 1 is right).| Point to track y value (normalized 0..1, 0 is top, 1 is bottom).| Point radius (normalized 0..1, 0 is one pixel, 1 is full image width).| Target camera ID. 7 to 255: MAVLink camera component id. 1 to 6 for cameras attached to the autopilot, which don't have a distinct component id. 0: all cameras. This is used to target specific autopilot-connected cameras. It is also used to target specific cameras when the MAV_CMD is used in a mission.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_CAMERA_TRACK_RECTANGLE=2005, /* If the camera supports rectangle visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE is set), this command allows to initiate the tracking. |Top left corner of rectangle x value (normalized 0..1, 0 is left, 1 is right).| Top left corner of rectangle y value (normalized 0..1, 0 is top, 1 is bottom).| Bottom right corner of rectangle x value (normalized 0..1, 0 is left, 1 is right).| Bottom right corner of rectangle y value (normalized 0..1, 0 is top, 1 is bottom).| Target camera ID. 7 to 255: MAVLink camera component id. 1 to 6 for cameras attached to the autopilot, which don't have a distinct component id. 0: all cameras. This is used to target specific autopilot-connected cameras. It is also used to target specific cameras when the MAV_CMD is used in a mission.| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_CAMERA_STOP_TRACKING=2010, /* Stops ongoing tracking. |Target camera ID. 7 to 255: MAVLink camera component id. 1 to 6 for cameras attached to the autopilot, which don't have a distinct component id. 0: all cameras. This is used to target specific autopilot-connected cameras. It is also used to target specific cameras when the MAV_CMD is used in a mission.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_VIDEO_START_CAPTURE=2500, /* Starts video capture (recording). |Video Stream ID (0 for all streams)| Frequency CAMERA_CAPTURE_STATUS messages should be sent while recording (0 for no messages, otherwise frequency)| Target camera ID. 7 to 255: MAVLink camera component id. 1 to 6 for cameras attached to the autopilot, which don't have a distinct component id. 0: all cameras. This is used to target specific autopilot-connected cameras. It is also used to target specific cameras when the MAV_CMD is used in a mission.| Reserved (default:NaN)| Reserved (default:0)| Reserved (default:0)| Reserved (default:NaN)| */ + MAV_CMD_VIDEO_STOP_CAPTURE=2501, /* Stop the current video capture (recording). |Video Stream ID (0 for all streams)| Target camera ID. 7 to 255: MAVLink camera component id. 1 to 6 for cameras attached to the autopilot, which don't have a distinct component id. 0: all cameras. This is used to target specific autopilot-connected cameras. It is also used to target specific cameras when the MAV_CMD is used in a mission.| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:0)| Reserved (default:0)| Reserved (default:NaN)| */ + MAV_CMD_VIDEO_START_STREAMING=2502, /* Start video streaming |Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.)| Target camera ID. 7 to 255: MAVLink camera component id. 1 to 6 for cameras attached to the autopilot, which don't have a distinct component id. 0: all cameras. This is used to target specific autopilot-connected cameras. It is also used to target specific cameras when the MAV_CMD is used in a mission.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_VIDEO_STOP_STREAMING=2503, /* Stop the given video stream |Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.)| Target camera ID. 7 to 255: MAVLink camera component id. 1 to 6 for cameras attached to the autopilot, which don't have a distinct component id. 0: all cameras. This is used to target specific autopilot-connected cameras. It is also used to target specific cameras when the MAV_CMD is used in a mission.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION=2504, /* Request video stream information (VIDEO_STREAM_INFORMATION) |Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ MAV_CMD_REQUEST_VIDEO_STREAM_STATUS=2505, /* Request video stream status (VIDEO_STREAM_STATUS) |Video Stream ID (0 for all streams, 1 for first, 2 for second, etc.)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ MAV_CMD_LOGGING_START=2510, /* Request to start streaming logging data over MAVLink (see also LOGGING_DATA message) |Format: 0: ULog| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| */ MAV_CMD_LOGGING_STOP=2511, /* Request to stop streaming log data over MAVLink |Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| */ - MAV_CMD_AIRFRAME_CONFIGURATION=2520, /* |Landing gear ID (default: 0, -1 for all)| Landing gear position (Down: 0, Up: 1, NaN for no change)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| */ - MAV_CMD_CONTROL_HIGH_LATENCY=2600, /* Request to start/stop transmitting over the high latency telemetry |Control transmission over high latency telemetry (0: stop, 1: start)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_AIRFRAME_CONFIGURATION=2520, /* |Landing gear ID (default: 0, -1 for all)| Landing gear position (Down: 0, Up: 1, NaN for no change)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:0)| Reserved (default:0)| Reserved (default:NaN)| */ + MAV_CMD_CONTROL_HIGH_LATENCY=2600, /* Request to start/stop transmitting over the high latency telemetry |Start transmission over high latency telemetry (MAV_BOOL_FALSE: stop transmission). Values not equal to 0 or 1 are invalid.| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PANORAMA_CREATE=2800, /* Create a panorama at the current position |Viewing angle horizontal of the panorama (+- 0.5 the total angle)| Viewing angle vertical of panorama.| Speed of the horizontal rotation.| Speed of the vertical rotation.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_DO_VTOL_TRANSITION=3000, /* Request VTOL transition |The target VTOL state. Only MAV_VTOL_STATE_MC and MAV_VTOL_STATE_FW can be used.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ARM_AUTHORIZATION_REQUEST=3001, /* Request authorization to arm the vehicle to a external entity, the arm authorizer is responsible to request all data that is needs from the vehicle before authorize or deny the request. If approved the progress of command_ack message should be set with period of time that this authorization is valid in seconds or in case it was denied it should be set with one of the reasons in ARM_AUTH_DENIED_REASON. + MAV_CMD_DO_VTOL_TRANSITION=3000, /* Request VTOL transition |The target VTOL state. For normal transitions, only MAV_VTOL_STATE_MC and MAV_VTOL_STATE_FW can be used.| Force immediate transition to the specified MAV_VTOL_STATE. 1: Force immediate, 0: normal transition. Can be used, for example, to trigger an emergency "Quadchute". Caution: Can be dangerous/damage vehicle, depending on autopilot implementation of this command.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ARM_AUTHORIZATION_REQUEST=3001, /* Request authorization to arm the vehicle to a external entity, the arm authorizer is responsible to request all data that is needs from the vehicle before authorize or deny the request. + If approved the COMMAND_ACK message progress field should be set with period of time that this authorization is valid in seconds. + If the authorization is denied COMMAND_ACK.result_param2 should be set with one of the reasons in ARM_AUTH_DENIED_REASON. |Vehicle system id, this way ground station can request arm authorization on behalf of any vehicle| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ MAV_CMD_SET_GUIDED_SUBMODE_STANDARD=4000, /* This command sets the submode to standard guided when vehicle is in guided mode. The vehicle holds position and altitude and the user can input the desired velocities along all three axes. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ MAV_CMD_SET_GUIDED_SUBMODE_CIRCLE=4001, /* This command sets submode circle when vehicle is in guided mode. Vehicle flies along a circle facing the center of the circle. The user can input the velocity along the circle and change the radius. If no input is given the vehicle will hold position. |Radius of desired circle in CIRCLE_MODE| User defined| User defined| User defined| Target latitude of center of circle in CIRCLE_MODE| Target longitude of center of circle in CIRCLE_MODE| Reserved (default:0)| */ - MAV_CMD_CONDITION_GATE=4501, /* Delay mission state machine until gate has been reached. |Geometry: 0: orthogonal to path between previous and next waypoint.| Altitude: 0: ignore altitude| Empty| Empty| Latitude| Longitude| Altitude| */ + MAV_CMD_CONDITION_GATE=4501, /* Delay mission state machine until gate has been reached. |Geometry: 0: orthogonal to path between previous and next waypoint.| Use altitude (MAV_BOOL_FALSE: ignore altitude). Values not equal to 0 or 1 are invalid.| Empty| Empty| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_FENCE_RETURN_POINT=5000, /* Fence return point (there can only be one such point in a geofence definition). If rally points are supported they should be used instead. |Reserved| Reserved| Reserved| Reserved| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION=5001, /* Fence vertex for an inclusion polygon (the polygon must not be self-intersecting). The vehicle must stay within this area. Minimum of 3 vertices required. - |Polygon vertex count| Vehicle must be inside ALL inclusion zones in a single group, vehicle must be inside at least one group, must be the same for all points in each polygon| Reserved| Reserved| Latitude| Longitude| Reserved| */ + The vertices for a polygon must be sent sequentially, each with param1 set to the total number of vertices in the polygon. + |Polygon vertex count. This is the number of vertices in the current polygon (all vertices will have the same number).| Vehicle must be inside ALL inclusion zones in a single group, vehicle must be inside at least one group, must be the same for all points in each polygon| Reserved| Reserved| Latitude| Longitude| Reserved| */ MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION=5002, /* Fence vertex for an exclusion polygon (the polygon must not be self-intersecting). The vehicle must stay outside this area. Minimum of 3 vertices required. - |Polygon vertex count| Reserved| Reserved| Reserved| Latitude| Longitude| Reserved| */ + The vertices for a polygon must be sent sequentially, each with param1 set to the total number of vertices in the polygon. + |Polygon vertex count. This is the number of vertices in the current polygon (all vertices will have the same number).| Reserved| Reserved| Reserved| Latitude| Longitude| Reserved| */ MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION=5003, /* Circular fence area. The vehicle must stay inside this area. |Radius.| Vehicle must be inside ALL inclusion zones in a single group, vehicle must be inside at least one group| Reserved| Reserved| Latitude| Longitude| Reserved| */ MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION=5004, /* Circular fence area. The vehicle must stay outside this area. @@ -689,7 +947,9 @@ typedef enum MAV_CMD MAV_CMD_NAV_RALLY_POINT=5100, /* Rally point. You can have multiple rally points defined. |Reserved| Reserved| Reserved| Reserved| Latitude| Longitude| Altitude| */ MAV_CMD_UAVCAN_GET_NODE_INFO=5200, /* Commands the vehicle to respond with a sequence of messages UAVCAN_NODE_INFO, one message per every UAVCAN node that is online. Note that some of the response messages can be lost, which the receiver can detect easily by checking whether every received UAVCAN_NODE_STATUS has a matching message UAVCAN_NODE_INFO received earlier; if not, this command should be sent again in order to request re-transmission of the node information messages. |Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| */ - MAV_CMD_PAYLOAD_PREPARE_DEPLOY=30001, /* Deploy payload on a Lat / Lon / Alt position. This includes the navigation to reach the required release position and velocity. |Operation mode. 0: prepare single payload deploy (overwriting previous requests), but do not execute it. 1: execute payload deploy immediately (rejecting further deploy commands during execution, but allowing abort). 2: add payload deploy to existing deployment list.| Desired approach vector in compass heading. A negative value indicates the system can define the approach vector at will.| Desired ground speed at release time. This can be overridden by the airframe in case it needs to meet minimum airspeed. A negative value indicates the system can define the ground speed at will.| Minimum altitude clearance to the release position. A negative value indicates the system can define the clearance at will.| Latitude. Note, if used in MISSION_ITEM (deprecated) the units are degrees (unscaled)| Longitude. Note, if used in MISSION_ITEM (deprecated) the units are degrees (unscaled)| Altitude (MSL)| */ + MAV_CMD_DO_SET_SAFETY_SWITCH_STATE=5300, /* Change state of safety switch. |New safety switch state.| Empty.| Empty.| Empty| Empty.| Empty.| Empty.| */ + MAV_CMD_DO_ADSB_OUT_IDENT=10001, /* Trigger the start of an ADSB-out IDENT. This should only be used when requested to do so by an Air Traffic Controller in controlled airspace. This starts the IDENT which is then typically held for 18 seconds by the hardware per the Mode A, C, and S transponder spec. |Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| Reserved (set to 0)| */ + MAV_CMD_PAYLOAD_PREPARE_DEPLOY=30001, /* Deploy payload on a Lat / Lon / Alt position. This includes the navigation to reach the required release position and velocity. |Operation mode. 0: prepare single payload deploy (overwriting previous requests), but do not execute it. 1: execute payload deploy immediately (rejecting further deploy commands during execution, but allowing abort). 2: add payload deploy to existing deployment list.| Desired approach vector in compass heading. A negative value indicates the system can define the approach vector at will.| Desired ground speed at release time. This can be overridden by the airframe in case it needs to meet minimum airspeed. A negative value indicates the system can define the ground speed at will.| Minimum altitude clearance to the release position. A negative value indicates the system can define the clearance at will.| Latitude.| Longitude.| Altitude (MSL)| */ MAV_CMD_PAYLOAD_CONTROL_DEPLOY=30002, /* Control the payload deployment. |Operation mode. 0: Abort deployment, continue normal mission. 1: switch to payload deployment mode. 100: delete first payload deployment request. 101: delete all payload deployment requests.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ MAV_CMD_WAYPOINT_USER_1=31000, /* User defined waypoint item. Ground Station will show the Vehicle as flying through this item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude (MSL)| */ MAV_CMD_WAYPOINT_USER_2=31001, /* User defined waypoint item. Ground Station will show the Vehicle as flying through this item. |User defined| User defined| User defined| User defined| Latitude unscaled| Longitude unscaled| Altitude (MSL)| */ @@ -706,9 +966,11 @@ typedef enum MAV_CMD MAV_CMD_USER_3=31012, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ MAV_CMD_USER_4=31013, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ MAV_CMD_USER_5=31014, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ + MAV_CMD_CAN_FORWARD=32000, /* Request forwarding of CAN packets from the given CAN bus to this component. CAN Frames are sent using CAN_FRAME and CANFD_FRAME messages |Bus number (0 to disable forwarding, 1 for first bus, 2 for 2nd bus, 3 for 3rd bus).| Empty.| Empty.| Empty.| Empty.| Empty.| Empty.| */ MAV_CMD_FIXED_MAG_CAL_YAW=42006, /* Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location. |Yaw of vehicle in earth frame.| CompassMask, 0 for all.| Latitude.| Longitude.| Empty.| Empty.| Empty.| */ - MAV_CMD_DO_WINCH=42600, /* Command to operate winch. |Winch instance number.| Action to perform.| Length of cable to release (negative to wind).| Release rate (negative to wind).| Empty.| Empty.| Empty.| */ - MAV_CMD_ENUM_END=42601, /* | */ + MAV_CMD_DO_WINCH=42600, /* Command to operate winch. |Winch instance number.| Action to perform.| Length of line to release (negative to wind).| Release rate (negative to wind).| Empty.| Empty.| Empty.| */ + MAV_CMD_EXTERNAL_POSITION_ESTIMATE=43003, /* Provide an external position estimate for use when dead-reckoning. This is meant to be used for occasional position resets that may be provided by a external system such as a remote pilot using landmarks over a video link. |Timestamp that this message was sent as a time in the transmitters time domain. The sender should wrap this time back to zero based on required timing accuracy for the application and the limitations of a 32 bit float. For example, wrapping at 10 hours would give approximately 1ms accuracy. Recipient must handle time wrap in any timing jitter correction applied to this field. Wrap rollover time should not be at not more than 250 seconds, which would give approximately 10 microsecond accuracy.| The time spent in processing the sensor data that is the basis for this position. The recipient can use this to improve time alignment of the data. Set to zero if not known.| estimated one standard deviation accuracy of the measurement. Set to NaN if not known.| Empty| Latitude| Longitude| Altitude, not used. Should be sent as NaN. May be supported in a future version of this message.| */ + MAV_CMD_ENUM_END=43004, /* | */ } MAV_CMD; #endif @@ -724,7 +986,7 @@ typedef enum MAV_DATA_STREAM MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS | */ MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW | */ MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. | */ - MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. | */ + MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION_INT messages. | */ MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot | */ MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot | */ MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */ @@ -748,24 +1010,6 @@ typedef enum MAV_ROI } MAV_ROI; #endif -/** @brief ACK / NACK / ERROR values as a result of MAV_CMDs and for mission item transmission. */ -#ifndef HAVE_ENUM_MAV_CMD_ACK -#define HAVE_ENUM_MAV_CMD_ACK -typedef enum MAV_CMD_ACK -{ - MAV_CMD_ACK_OK=0, /* Command / mission item is ok. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_FAIL=1, /* Generic error message if none of the other reasons fails or if no detailed error reporting is implemented. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_ACCESS_DENIED=2, /* The system is refusing to accept this command from this source / communication partner. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_NOT_SUPPORTED=3, /* Command or mission item is not supported, other commands would be accepted. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_COORDINATE_FRAME_NOT_SUPPORTED=4, /* The coordinate frame of this command / mission item is not supported. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_COORDINATES_OUT_OF_RANGE=5, /* The coordinate frame of this command is ok, but he coordinate values exceed the safety limits of this system. This is a generic error, please use the more specific error messages below if possible. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_X_LAT_OUT_OF_RANGE=6, /* The X or latitude value is out of range. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_Y_LON_OUT_OF_RANGE=7, /* The Y or longitude value is out of range. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_Z_ALT_OUT_OF_RANGE=8, /* The Z or altitude value is out of range. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ENUM_END=9, /* | */ -} MAV_CMD_ACK; -#endif - /** @brief Specifies the datatype of a MAVLink parameter. */ #ifndef HAVE_ENUM_MAV_PARAM_TYPE #define HAVE_ENUM_MAV_PARAM_TYPE @@ -817,7 +1061,10 @@ typedef enum MAV_RESULT MAV_RESULT_FAILED=4, /* Command is valid, but execution has failed. This is used to indicate any non-temporary or unexpected problem, i.e. any problem that must be fixed before the command can succeed/be retried. For example, attempting to write a file when out of memory, attempting to arm when sensors are not calibrated, etc. | */ MAV_RESULT_IN_PROGRESS=5, /* Command is valid and is being executed. This will be followed by further progress updates, i.e. the component may send further COMMAND_ACK messages with result MAV_RESULT_IN_PROGRESS (at a rate decided by the implementation), and must terminate by sending a COMMAND_ACK message with final result of the operation. The COMMAND_ACK.progress field can be used to indicate the progress of the operation. | */ MAV_RESULT_CANCELLED=6, /* Command has been cancelled (as a result of receiving a COMMAND_CANCEL message). | */ - MAV_RESULT_ENUM_END=7, /* | */ + MAV_RESULT_COMMAND_LONG_ONLY=7, /* Command is only accepted when sent as a COMMAND_LONG. | */ + MAV_RESULT_COMMAND_INT_ONLY=8, /* Command is only accepted when sent as a COMMAND_INT. | */ + MAV_RESULT_COMMAND_UNSUPPORTED_MAV_FRAME=9, /* Command is invalid because a frame is required and the specified frame is not supported. | */ + MAV_RESULT_ENUM_END=10, /* | */ } MAV_RESULT; #endif @@ -981,32 +1228,6 @@ typedef enum MAV_SENSOR_ORIENTATION } MAV_SENSOR_ORIENTATION; #endif -/** @brief Bitmask of (optional) autopilot capabilities (64 bit). If a bit is set, the autopilot supports this capability. */ -#ifndef HAVE_ENUM_MAV_PROTOCOL_CAPABILITY -#define HAVE_ENUM_MAV_PROTOCOL_CAPABILITY -typedef enum MAV_PROTOCOL_CAPABILITY -{ - MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT=1, /* Autopilot supports MISSION float message type. | */ - MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT=2, /* Autopilot supports the new param float message type. | */ - MAV_PROTOCOL_CAPABILITY_MISSION_INT=4, /* Autopilot supports MISSION_ITEM_INT scaled integer message type. | */ - MAV_PROTOCOL_CAPABILITY_COMMAND_INT=8, /* Autopilot supports COMMAND_INT scaled integer message type. | */ - MAV_PROTOCOL_CAPABILITY_PARAM_UNION=16, /* Autopilot supports the new param union message type. | */ - MAV_PROTOCOL_CAPABILITY_FTP=32, /* Autopilot supports the new FILE_TRANSFER_PROTOCOL message type. | */ - MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET=64, /* Autopilot supports commanding attitude offboard. | */ - MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED=128, /* Autopilot supports commanding position and velocity targets in local NED frame. | */ - MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT=256, /* Autopilot supports commanding position and velocity targets in global scaled integers. | */ - MAV_PROTOCOL_CAPABILITY_TERRAIN=512, /* Autopilot supports terrain protocol / data handling. | */ - MAV_PROTOCOL_CAPABILITY_SET_ACTUATOR_TARGET=1024, /* Autopilot supports direct actuator control. | */ - MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION=2048, /* Autopilot supports the flight termination command. | */ - MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION=4096, /* Autopilot supports onboard compass calibration. | */ - MAV_PROTOCOL_CAPABILITY_MAVLINK2=8192, /* Autopilot supports MAVLink version 2. | */ - MAV_PROTOCOL_CAPABILITY_MISSION_FENCE=16384, /* Autopilot supports mission fence protocol. | */ - MAV_PROTOCOL_CAPABILITY_MISSION_RALLY=32768, /* Autopilot supports mission rally point protocol. | */ - MAV_PROTOCOL_CAPABILITY_FLIGHT_INFORMATION=65536, /* Autopilot supports the flight information protocol. | */ - MAV_PROTOCOL_CAPABILITY_ENUM_END=65537, /* | */ -} MAV_PROTOCOL_CAPABILITY; -#endif - /** @brief Type of mission items being requested/sent in mission protocol. */ #ifndef HAVE_ENUM_MAV_MISSION_TYPE #define HAVE_ENUM_MAV_MISSION_TYPE @@ -1061,7 +1282,7 @@ typedef enum MAV_BATTERY_FUNCTION MAV_BATTERY_FUNCTION_ALL=1, /* Battery supports all flight systems | */ MAV_BATTERY_FUNCTION_PROPULSION=2, /* Battery for the propulsion system | */ MAV_BATTERY_FUNCTION_AVIONICS=3, /* Avionics battery | */ - MAV_BATTERY_TYPE_PAYLOAD=4, /* Payload battery | */ + MAV_BATTERY_FUNCTION_PAYLOAD=4, /* Payload battery | */ MAV_BATTERY_FUNCTION_ENUM_END=5, /* | */ } MAV_BATTERY_FUNCTION; #endif @@ -1107,10 +1328,24 @@ typedef enum MAV_BATTERY_FAULT MAV_BATTERY_FAULT_OVER_TEMPERATURE=16, /* Over-temperature fault. | */ MAV_BATTERY_FAULT_UNDER_TEMPERATURE=32, /* Under-temperature fault. | */ MAV_BATTERY_FAULT_INCOMPATIBLE_VOLTAGE=64, /* Vehicle voltage is not compatible with this battery (batteries on same power rail should have similar voltage). | */ - MAV_BATTERY_FAULT_ENUM_END=65, /* | */ + MAV_BATTERY_FAULT_INCOMPATIBLE_FIRMWARE=128, /* Battery firmware is not compatible with current autopilot firmware. | */ + BATTERY_FAULT_INCOMPATIBLE_CELLS_CONFIGURATION=256, /* Battery is not compatible due to cell configuration (e.g. 5s1p when vehicle requires 6s). | */ + MAV_BATTERY_FAULT_ENUM_END=257, /* | */ } MAV_BATTERY_FAULT; #endif +/** @brief Fuel types for use in FUEL_TYPE. Fuel types specify the units for the maximum, available and consumed fuel, and for the flow rates. */ +#ifndef HAVE_ENUM_MAV_FUEL_TYPE +#define HAVE_ENUM_MAV_FUEL_TYPE +typedef enum MAV_FUEL_TYPE +{ + MAV_FUEL_TYPE_UNKNOWN=0, /* Not specified. Fuel levels are normalized (i.e. maximum is 1, and other levels are relative to 1). | */ + MAV_FUEL_TYPE_LIQUID=1, /* A generic liquid fuel. Fuel levels are in millilitres (ml). Fuel rates are in millilitres/second. | */ + MAV_FUEL_TYPE_GAS=2, /* A gas tank. Fuel levels are in kilo-Pascal (kPa), and flow rates are in milliliters per second (ml/s). | */ + MAV_FUEL_TYPE_ENUM_END=3, /* | */ +} MAV_FUEL_TYPE; +#endif + /** @brief Flags to report status/failure cases for a power generator (used in GENERATOR_STATUS). Note that FAULTS are conditions that cause the generator to fail. Warnings are conditions that require attention before the next use (they indicate the system is not operating properly). */ #ifndef HAVE_ENUM_MAV_GENERATOR_STATUS_FLAG #define HAVE_ENUM_MAV_GENERATOR_STATUS_FLAG @@ -1236,10 +1471,24 @@ typedef enum ADSB_FLAGS typedef enum MAV_DO_REPOSITION_FLAGS { MAV_DO_REPOSITION_FLAGS_CHANGE_MODE=1, /* The aircraft should immediately transition into guided. This should not be set for follow me applications | */ - MAV_DO_REPOSITION_FLAGS_ENUM_END=2, /* | */ + MAV_DO_REPOSITION_FLAGS_RELATIVE_YAW=2, /* Yaw relative to the vehicle current heading (if not set, relative to North). | */ + MAV_DO_REPOSITION_FLAGS_ENUM_END=3, /* | */ } MAV_DO_REPOSITION_FLAGS; #endif +/** @brief Speed setpoint types used in MAV_CMD_DO_CHANGE_SPEED */ +#ifndef HAVE_ENUM_SPEED_TYPE +#define HAVE_ENUM_SPEED_TYPE +typedef enum SPEED_TYPE +{ + SPEED_TYPE_AIRSPEED=0, /* Airspeed | */ + SPEED_TYPE_GROUNDSPEED=1, /* Groundspeed | */ + SPEED_TYPE_CLIMB_SPEED=2, /* Climb speed | */ + SPEED_TYPE_DESCENT_SPEED=3, /* Descent speed | */ + SPEED_TYPE_ENUM_END=4, /* | */ +} SPEED_TYPE; +#endif + /** @brief Flags in ESTIMATOR_STATUS message */ #ifndef HAVE_ENUM_ESTIMATOR_STATUS_FLAGS #define HAVE_ENUM_ESTIMATOR_STATUS_FLAGS @@ -1261,27 +1510,27 @@ typedef enum ESTIMATOR_STATUS_FLAGS } ESTIMATOR_STATUS_FLAGS; #endif -/** @brief */ +/** @brief Sequence that motors are tested when using MAV_CMD_DO_MOTOR_TEST. */ #ifndef HAVE_ENUM_MOTOR_TEST_ORDER #define HAVE_ENUM_MOTOR_TEST_ORDER typedef enum MOTOR_TEST_ORDER { - MOTOR_TEST_ORDER_DEFAULT=0, /* default autopilot motor test method | */ - MOTOR_TEST_ORDER_SEQUENCE=1, /* motor numbers are specified as their index in a predefined vehicle-specific sequence | */ - MOTOR_TEST_ORDER_BOARD=2, /* motor numbers are specified as the output as labeled on the board | */ + MOTOR_TEST_ORDER_DEFAULT=0, /* Default autopilot motor test method. | */ + MOTOR_TEST_ORDER_SEQUENCE=1, /* Motor numbers are specified as their index in a predefined vehicle-specific sequence. | */ + MOTOR_TEST_ORDER_BOARD=2, /* Motor numbers are specified as the output as labeled on the board. | */ MOTOR_TEST_ORDER_ENUM_END=3, /* | */ } MOTOR_TEST_ORDER; #endif -/** @brief */ +/** @brief Defines how throttle value is represented in MAV_CMD_DO_MOTOR_TEST. */ #ifndef HAVE_ENUM_MOTOR_TEST_THROTTLE_TYPE #define HAVE_ENUM_MOTOR_TEST_THROTTLE_TYPE typedef enum MOTOR_TEST_THROTTLE_TYPE { - MOTOR_TEST_THROTTLE_PERCENT=0, /* throttle as a percentage from 0 ~ 100 | */ - MOTOR_TEST_THROTTLE_PWM=1, /* throttle as an absolute PWM value (normally in range of 1000~2000) | */ - MOTOR_TEST_THROTTLE_PILOT=2, /* throttle pass-through from pilot's transmitter | */ - MOTOR_TEST_COMPASS_CAL=3, /* per-motor compass calibration test | */ + MOTOR_TEST_THROTTLE_PERCENT=0, /* Throttle as a percentage (0 ~ 100) | */ + MOTOR_TEST_THROTTLE_PWM=1, /* Throttle as an absolute PWM value (normally in range of 1000~2000). | */ + MOTOR_TEST_THROTTLE_PILOT=2, /* Throttle pass-through from pilot's transmitter. | */ + MOTOR_TEST_COMPASS_CAL=3, /* Per-motor compass calibration test. | */ MOTOR_TEST_THROTTLE_TYPE_ENUM_END=4, /* | */ } MOTOR_TEST_THROTTLE_TYPE; #endif @@ -1415,7 +1664,9 @@ typedef enum CAMERA_CAP_FLAGS CAMERA_CAP_FLAGS_HAS_TRACKING_POINT=512, /* Camera supports tracking of a point on the camera view. | */ CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE=1024, /* Camera supports tracking of a selection rectangle on the camera view. | */ CAMERA_CAP_FLAGS_HAS_TRACKING_GEO_STATUS=2048, /* Camera supports tracking geo status (CAMERA_TRACKING_GEO_STATUS). | */ - CAMERA_CAP_FLAGS_ENUM_END=2049, /* | */ + CAMERA_CAP_FLAGS_HAS_THERMAL_RANGE=4096, /* Camera supports absolute thermal range (request CAMERA_THERMAL_RANGE with MAV_CMD_REQUEST_MESSAGE). | */ + CAMERA_CAP_FLAGS_HAS_MTI=8192, /* Camera supports Moving Target Indicators (MTI) on the camera view (using MAV_CMD_CAMERA_START_MTI). | */ + CAMERA_CAP_FLAGS_ENUM_END=8193, /* | */ } CAMERA_CAP_FLAGS; #endif @@ -1426,7 +1677,8 @@ typedef enum VIDEO_STREAM_STATUS_FLAGS { VIDEO_STREAM_STATUS_FLAGS_RUNNING=1, /* Stream is active (running) | */ VIDEO_STREAM_STATUS_FLAGS_THERMAL=2, /* Stream is thermal imaging | */ - VIDEO_STREAM_STATUS_FLAGS_ENUM_END=3, /* | */ + VIDEO_STREAM_STATUS_FLAGS_THERMAL_RANGE_ENABLED=4, /* Stream can report absolute thermal range (see CAMERA_THERMAL_RANGE). | */ + VIDEO_STREAM_STATUS_FLAGS_ENUM_END=5, /* | */ } VIDEO_STREAM_STATUS_FLAGS; #endif @@ -1438,11 +1690,23 @@ typedef enum VIDEO_STREAM_TYPE VIDEO_STREAM_TYPE_RTSP=0, /* Stream is RTSP | */ VIDEO_STREAM_TYPE_RTPUDP=1, /* Stream is RTP UDP (URI gives the port number) | */ VIDEO_STREAM_TYPE_TCP_MPEG=2, /* Stream is MPEG on TCP | */ - VIDEO_STREAM_TYPE_MPEG_TS_H264=3, /* Stream is h.264 on MPEG TS (URI gives the port number) | */ + VIDEO_STREAM_TYPE_MPEG_TS=3, /* Stream is MPEG TS (URI gives the port number) | */ VIDEO_STREAM_TYPE_ENUM_END=4, /* | */ } VIDEO_STREAM_TYPE; #endif +/** @brief Video stream encodings */ +#ifndef HAVE_ENUM_VIDEO_STREAM_ENCODING +#define HAVE_ENUM_VIDEO_STREAM_ENCODING +typedef enum VIDEO_STREAM_ENCODING +{ + VIDEO_STREAM_ENCODING_UNKNOWN=0, /* Stream encoding is unknown | */ + VIDEO_STREAM_ENCODING_H264=1, /* Stream encoding is H.264 | */ + VIDEO_STREAM_ENCODING_H265=2, /* Stream encoding is H.265 | */ + VIDEO_STREAM_ENCODING_ENUM_END=3, /* | */ +} VIDEO_STREAM_ENCODING; +#endif + /** @brief Camera tracking status flags */ #ifndef HAVE_ENUM_CAMERA_TRACKING_STATUS_FLAGS #define HAVE_ENUM_CAMERA_TRACKING_STATUS_FLAGS @@ -1451,7 +1715,9 @@ typedef enum CAMERA_TRACKING_STATUS_FLAGS CAMERA_TRACKING_STATUS_FLAGS_IDLE=0, /* Camera is not tracking | */ CAMERA_TRACKING_STATUS_FLAGS_ACTIVE=1, /* Camera is tracking | */ CAMERA_TRACKING_STATUS_FLAGS_ERROR=2, /* Camera tracking in error state | */ - CAMERA_TRACKING_STATUS_FLAGS_ENUM_END=3, /* | */ + CAMERA_TRACKING_STATUS_FLAGS_MTI=4, /* Camera Moving Target Indicators (MTI) are active | */ + CAMERA_TRACKING_STATUS_FLAGS_COASTING=8, /* Camera tracking target is obscured and is being predicted | */ + CAMERA_TRACKING_STATUS_FLAGS_ENUM_END=9, /* | */ } CAMERA_TRACKING_STATUS_FLAGS; #endif @@ -1472,7 +1738,6 @@ typedef enum CAMERA_TRACKING_MODE #define HAVE_ENUM_CAMERA_TRACKING_TARGET_DATA typedef enum CAMERA_TRACKING_TARGET_DATA { - CAMERA_TRACKING_TARGET_DATA_NONE=0, /* No target data | */ CAMERA_TRACKING_TARGET_DATA_EMBEDDED=1, /* Target data embedded in image data (proprietary) | */ CAMERA_TRACKING_TARGET_DATA_RENDERED=2, /* Target data rendered in image | */ CAMERA_TRACKING_TARGET_DATA_IN_STATUS=4, /* Target data within status message (Point or Rectangle) | */ @@ -1486,10 +1751,11 @@ typedef enum CAMERA_TRACKING_TARGET_DATA typedef enum CAMERA_ZOOM_TYPE { ZOOM_TYPE_STEP=0, /* Zoom one step increment (-1 for wide, 1 for tele) | */ - ZOOM_TYPE_CONTINUOUS=1, /* Continuous zoom up/down until stopped (-1 for wide, 1 for tele, 0 to stop zooming) | */ - ZOOM_TYPE_RANGE=2, /* Zoom value as proportion of full camera range (a value between 0.0 and 100.0) | */ - ZOOM_TYPE_FOCAL_LENGTH=3, /* Zoom value/variable focal length in milimetres. Note that there is no message to get the valid zoom range of the camera, so this can type can only be used for cameras where the zoom range is known (implying that this cannot reliably be used in a GCS for an arbitrary camera) | */ - CAMERA_ZOOM_TYPE_ENUM_END=4, /* | */ + ZOOM_TYPE_CONTINUOUS=1, /* Continuous normalized zoom in/out rate until stopped. Range -1..1, negative: wide, positive: narrow/tele, 0 to stop zooming. Other values should be clipped to the range. | */ + ZOOM_TYPE_RANGE=2, /* Zoom value as proportion of full camera range (a percentage value between 0.0 and 100.0) | */ + ZOOM_TYPE_FOCAL_LENGTH=3, /* Zoom value/variable focal length in millimetres. Note that there is no message to get the valid zoom range of the camera, so this can type can only be used for cameras where the zoom range is known (implying that this cannot reliably be used in a GCS for an arbitrary camera) | */ + ZOOM_TYPE_HORIZONTAL_FOV=4, /* Zoom value as horizontal field of view in degrees. | */ + CAMERA_ZOOM_TYPE_ENUM_END=5, /* | */ } CAMERA_ZOOM_TYPE; #endif @@ -1499,14 +1765,30 @@ typedef enum CAMERA_ZOOM_TYPE typedef enum SET_FOCUS_TYPE { FOCUS_TYPE_STEP=0, /* Focus one step increment (-1 for focusing in, 1 for focusing out towards infinity). | */ - FOCUS_TYPE_CONTINUOUS=1, /* Continuous focus up/down until stopped (-1 for focusing in, 1 for focusing out towards infinity, 0 to stop focusing) | */ + FOCUS_TYPE_CONTINUOUS=1, /* Continuous normalized focus in/out rate until stopped. Range -1..1, negative: in, positive: out towards infinity, 0 to stop focusing. Other values should be clipped to the range. | */ FOCUS_TYPE_RANGE=2, /* Focus value as proportion of full camera focus range (a value between 0.0 and 100.0) | */ FOCUS_TYPE_METERS=3, /* Focus value in metres. Note that there is no message to get the valid focus range of the camera, so this can type can only be used for cameras where the range is known (implying that this cannot reliably be used in a GCS for an arbitrary camera). | */ - SET_FOCUS_TYPE_ENUM_END=4, /* | */ + FOCUS_TYPE_AUTO=4, /* Focus automatically. | */ + FOCUS_TYPE_AUTO_SINGLE=5, /* Single auto focus. Mainly used for still pictures. Usually abbreviated as AF-S. | */ + FOCUS_TYPE_AUTO_CONTINUOUS=6, /* Continuous auto focus. Mainly used for dynamic scenes. Abbreviated as AF-C. | */ + SET_FOCUS_TYPE_ENUM_END=7, /* | */ } SET_FOCUS_TYPE; #endif -/** @brief Result from PARAM_EXT_SET message (or a PARAM_SET within a transaction). */ +/** @brief Camera sources for MAV_CMD_SET_CAMERA_SOURCE */ +#ifndef HAVE_ENUM_CAMERA_SOURCE +#define HAVE_ENUM_CAMERA_SOURCE +typedef enum CAMERA_SOURCE +{ + CAMERA_SOURCE_DEFAULT=0, /* Default camera source. | */ + CAMERA_SOURCE_RGB=1, /* RGB camera source. | */ + CAMERA_SOURCE_IR=2, /* IR camera source. | */ + CAMERA_SOURCE_NDVI=3, /* NDVI camera source. | */ + CAMERA_SOURCE_ENUM_END=4, /* | */ +} CAMERA_SOURCE; +#endif + +/** @brief Result from PARAM_EXT_SET message. */ #ifndef HAVE_ENUM_PARAM_ACK #define HAVE_ENUM_PARAM_ACK typedef enum PARAM_ACK @@ -1514,7 +1796,7 @@ typedef enum PARAM_ACK PARAM_ACK_ACCEPTED=0, /* Parameter value ACCEPTED and SET | */ PARAM_ACK_VALUE_UNSUPPORTED=1, /* Parameter value UNKNOWN/UNSUPPORTED | */ PARAM_ACK_FAILED=2, /* Parameter failed to set | */ - PARAM_ACK_IN_PROGRESS=3, /* Parameter value received but not yet set/accepted. A subsequent PARAM_ACK_TRANSACTION or PARAM_EXT_ACK with the final result will follow once operation is completed. This is returned immediately for parameters that take longer to set, indicating taht the the parameter was recieved and does not need to be resent. | */ + PARAM_ACK_IN_PROGRESS=3, /* Parameter value received but not yet set/accepted. A subsequent PARAM_EXT_ACK with the final result will follow once operation is completed. This is returned immediately for parameters that take longer to set, indicating that the the parameter was received and does not need to be resent. | */ PARAM_ACK_ENUM_END=4, /* | */ } PARAM_ACK; #endif @@ -1546,17 +1828,29 @@ typedef enum MAV_ARM_AUTH_DENIED_REASON } MAV_ARM_AUTH_DENIED_REASON; #endif -/** @brief RC type */ +/** @brief RC type. Used in MAV_CMD_START_RX_PAIR. */ #ifndef HAVE_ENUM_RC_TYPE #define HAVE_ENUM_RC_TYPE typedef enum RC_TYPE { - RC_TYPE_SPEKTRUM_DSM2=0, /* Spektrum DSM2 | */ - RC_TYPE_SPEKTRUM_DSMX=1, /* Spektrum DSMX | */ + RC_TYPE_SPEKTRUM=0, /* Spektrum | */ + RC_TYPE_CRSF=1, /* CRSF | */ RC_TYPE_ENUM_END=2, /* | */ } RC_TYPE; #endif +/** @brief RC sub-type of types defined in RC_TYPE. Used in MAV_CMD_START_RX_PAIR. Ignored if value does not correspond to the set RC_TYPE. */ +#ifndef HAVE_ENUM_RC_SUB_TYPE +#define HAVE_ENUM_RC_SUB_TYPE +typedef enum RC_SUB_TYPE +{ + RC_SUB_TYPE_SPEKTRUM_DSM2=0, /* Spektrum DSM2 | */ + RC_SUB_TYPE_SPEKTRUM_DSMX=1, /* Spektrum DSMX | */ + RC_SUB_TYPE_SPEKTRUM_DSMX8=2, /* Spektrum DSMX8 | */ + RC_SUB_TYPE_ENUM_END=3, /* | */ +} RC_SUB_TYPE; +#endif + /** @brief Bitmap to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored. If bit 9 is set the floats afx afy afz should be interpreted as force instead of acceleration. */ #ifndef HAVE_ENUM_POSITION_TARGET_TYPEMASK #define HAVE_ENUM_POSITION_TARGET_TYPEMASK @@ -1586,6 +1880,7 @@ typedef enum ATTITUDE_TARGET_TYPEMASK ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE=1, /* Ignore body roll rate | */ ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE=2, /* Ignore body pitch rate | */ ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE=4, /* Ignore body yaw rate | */ + ATTITUDE_TARGET_TYPEMASK_THRUST_BODY_SET=32, /* Use 3D body thrust setpoint instead of throttle | */ ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE=64, /* Ignore throttle | */ ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE=128, /* Ignore attitude | */ ATTITUDE_TARGET_TYPEMASK_ENUM_END=129, /* | */ @@ -1623,20 +1918,6 @@ typedef enum UTM_DATA_AVAIL_FLAGS } UTM_DATA_AVAIL_FLAGS; #endif -/** @brief Cellular network radio type */ -#ifndef HAVE_ENUM_CELLULAR_NETWORK_RADIO_TYPE -#define HAVE_ENUM_CELLULAR_NETWORK_RADIO_TYPE -typedef enum CELLULAR_NETWORK_RADIO_TYPE -{ - CELLULAR_NETWORK_RADIO_TYPE_NONE=0, /* | */ - CELLULAR_NETWORK_RADIO_TYPE_GSM=1, /* | */ - CELLULAR_NETWORK_RADIO_TYPE_CDMA=2, /* | */ - CELLULAR_NETWORK_RADIO_TYPE_WCDMA=3, /* | */ - CELLULAR_NETWORK_RADIO_TYPE_LTE=4, /* | */ - CELLULAR_NETWORK_RADIO_TYPE_ENUM_END=5, /* | */ -} CELLULAR_NETWORK_RADIO_TYPE; -#endif - /** @brief These flags encode the cellular network status */ #ifndef HAVE_ENUM_CELLULAR_STATUS_FLAG #define HAVE_ENUM_CELLULAR_STATUS_FLAG @@ -1667,11 +1948,25 @@ typedef enum CELLULAR_NETWORK_FAILED_REASON CELLULAR_NETWORK_FAILED_REASON_NONE=0, /* No error | */ CELLULAR_NETWORK_FAILED_REASON_UNKNOWN=1, /* Error state is unknown | */ CELLULAR_NETWORK_FAILED_REASON_SIM_MISSING=2, /* SIM is required for the modem but missing | */ - CELLULAR_NETWORK_FAILED_REASON_SIM_ERROR=3, /* SIM is available, but not usuable for connection | */ + CELLULAR_NETWORK_FAILED_REASON_SIM_ERROR=3, /* SIM is available, but not usable for connection | */ CELLULAR_NETWORK_FAILED_REASON_ENUM_END=4, /* | */ } CELLULAR_NETWORK_FAILED_REASON; #endif +/** @brief Cellular network radio type */ +#ifndef HAVE_ENUM_CELLULAR_NETWORK_RADIO_TYPE +#define HAVE_ENUM_CELLULAR_NETWORK_RADIO_TYPE +typedef enum CELLULAR_NETWORK_RADIO_TYPE +{ + CELLULAR_NETWORK_RADIO_TYPE_NONE=0, /* | */ + CELLULAR_NETWORK_RADIO_TYPE_GSM=1, /* | */ + CELLULAR_NETWORK_RADIO_TYPE_CDMA=2, /* | */ + CELLULAR_NETWORK_RADIO_TYPE_WCDMA=3, /* | */ + CELLULAR_NETWORK_RADIO_TYPE_LTE=4, /* | */ + CELLULAR_NETWORK_RADIO_TYPE_ENUM_END=5, /* | */ +} CELLULAR_NETWORK_RADIO_TYPE; +#endif + /** @brief Precision land modes (used in MAV_CMD_NAV_LAND). */ #ifndef HAVE_ENUM_PRECISION_LAND_MODE #define HAVE_ENUM_PRECISION_LAND_MODE @@ -1712,7 +2007,10 @@ typedef enum MAV_TUNNEL_PAYLOAD_TYPE MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED7=207, /* Registered for STorM32 gimbal controller. | */ MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED8=208, /* Registered for STorM32 gimbal controller. | */ MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED9=209, /* Registered for STorM32 gimbal controller. | */ - MAV_TUNNEL_PAYLOAD_TYPE_ENUM_END=210, /* | */ + MAV_TUNNEL_PAYLOAD_TYPE_MODALAI_REMOTE_OSD=210, /* Registered for ModalAI remote OSD protocol. | */ + MAV_TUNNEL_PAYLOAD_TYPE_MODALAI_ESC_UART_PASSTHRU=211, /* Registered for ModalAI ESC UART passthru protocol. | */ + MAV_TUNNEL_PAYLOAD_TYPE_MODALAI_IO_UART_PASSTHRU=212, /* Registered for ModalAI vendor use. | */ + MAV_TUNNEL_PAYLOAD_TYPE_ENUM_END=213, /* | */ } MAV_TUNNEL_PAYLOAD_TYPE; #endif @@ -1725,7 +2023,8 @@ typedef enum MAV_ODID_ID_TYPE MAV_ODID_ID_TYPE_SERIAL_NUMBER=1, /* Manufacturer Serial Number (ANSI/CTA-2063 format). | */ MAV_ODID_ID_TYPE_CAA_REGISTRATION_ID=2, /* CAA (Civil Aviation Authority) registered ID. Format: [ICAO Country Code].[CAA Assigned ID]. | */ MAV_ODID_ID_TYPE_UTM_ASSIGNED_UUID=3, /* UTM (Unmanned Traffic Management) assigned UUID (RFC4122). | */ - MAV_ODID_ID_TYPE_ENUM_END=4, /* | */ + MAV_ODID_ID_TYPE_SPECIFIC_SESSION_ID=4, /* A 20 byte ID for a specific flight/session. The exact ID type is indicated by the first byte of uas_id and these type values are managed by ICAO. | */ + MAV_ODID_ID_TYPE_ENUM_END=5, /* | */ } MAV_ODID_ID_TYPE; #endif @@ -1763,7 +2062,8 @@ typedef enum MAV_ODID_STATUS MAV_ODID_STATUS_GROUND=1, /* The UA is on the ground. | */ MAV_ODID_STATUS_AIRBORNE=2, /* The UA is in the air. | */ MAV_ODID_STATUS_EMERGENCY=3, /* The UA is having an emergency. | */ - MAV_ODID_STATUS_ENUM_END=4, /* | */ + MAV_ODID_STATUS_REMOTE_ID_SYSTEM_FAILURE=4, /* The remote ID system is failing or unreliable in some way. | */ + MAV_ODID_STATUS_ENUM_END=5, /* | */ } MAV_ODID_STATUS; #endif @@ -1865,7 +2165,8 @@ typedef enum MAV_ODID_AUTH_TYPE MAV_ODID_AUTH_TYPE_OPERATOR_ID_SIGNATURE=2, /* Signature for the Operator ID. | */ MAV_ODID_AUTH_TYPE_MESSAGE_SET_SIGNATURE=3, /* Signature for the entire message set. | */ MAV_ODID_AUTH_TYPE_NETWORK_REMOTE_ID=4, /* Authentication is provided by Network Remote ID. | */ - MAV_ODID_AUTH_TYPE_ENUM_END=5, /* | */ + MAV_ODID_AUTH_TYPE_SPECIFIC_AUTHENTICATION=5, /* The exact authentication type is indicated by the first byte of authentication_data and these type values are managed by ICAO. | */ + MAV_ODID_AUTH_TYPE_ENUM_END=6, /* | */ } MAV_ODID_AUTH_TYPE; #endif @@ -1874,8 +2175,10 @@ typedef enum MAV_ODID_AUTH_TYPE #define HAVE_ENUM_MAV_ODID_DESC_TYPE typedef enum MAV_ODID_DESC_TYPE { - MAV_ODID_DESC_TYPE_TEXT=0, /* Free-form text description of the purpose of the flight. | */ - MAV_ODID_DESC_TYPE_ENUM_END=1, /* | */ + MAV_ODID_DESC_TYPE_TEXT=0, /* Optional free-form text description of the purpose of the flight. | */ + MAV_ODID_DESC_TYPE_EMERGENCY=1, /* Optional additional clarification when status == MAV_ODID_STATUS_EMERGENCY. | */ + MAV_ODID_DESC_TYPE_EXTENDED_STATUS=2, /* Optional additional clarification when status != MAV_ODID_STATUS_EMERGENCY. | */ + MAV_ODID_DESC_TYPE_ENUM_END=3, /* | */ } MAV_ODID_DESC_TYPE; #endif @@ -1884,9 +2187,9 @@ typedef enum MAV_ODID_DESC_TYPE #define HAVE_ENUM_MAV_ODID_OPERATOR_LOCATION_TYPE typedef enum MAV_ODID_OPERATOR_LOCATION_TYPE { - MAV_ODID_OPERATOR_LOCATION_TYPE_TAKEOFF=0, /* The location of the operator is the same as the take-off location. | */ - MAV_ODID_OPERATOR_LOCATION_TYPE_LIVE_GNSS=1, /* The location of the operator is based on live GNSS data. | */ - MAV_ODID_OPERATOR_LOCATION_TYPE_FIXED=2, /* The location of the operator is a fixed location. | */ + MAV_ODID_OPERATOR_LOCATION_TYPE_TAKEOFF=0, /* The location/altitude of the operator is the same as the take-off location. | */ + MAV_ODID_OPERATOR_LOCATION_TYPE_LIVE_GNSS=1, /* The location/altitude of the operator is dynamic. E.g. based on live GNSS data. | */ + MAV_ODID_OPERATOR_LOCATION_TYPE_FIXED=2, /* The location/altitude of the operator are fixed values. | */ MAV_ODID_OPERATOR_LOCATION_TYPE_ENUM_END=3, /* | */ } MAV_ODID_OPERATOR_LOCATION_TYPE; #endif @@ -1942,6 +2245,17 @@ typedef enum MAV_ODID_OPERATOR_ID_TYPE } MAV_ODID_OPERATOR_ID_TYPE; #endif +/** @brief */ +#ifndef HAVE_ENUM_MAV_ODID_ARM_STATUS +#define HAVE_ENUM_MAV_ODID_ARM_STATUS +typedef enum MAV_ODID_ARM_STATUS +{ + MAV_ODID_ARM_STATUS_GOOD_TO_ARM=0, /* Passing arming checks. | */ + MAV_ODID_ARM_STATUS_PRE_ARM_FAIL_GENERIC=1, /* Generic arming failure, see error string for details. | */ + MAV_ODID_ARM_STATUS_ENUM_END=2, /* | */ +} MAV_ODID_ARM_STATUS; +#endif + /** @brief Tune formats (used for vehicle buzzer/tone generation). */ #ifndef HAVE_ENUM_TUNE_FORMAT #define HAVE_ENUM_TUNE_FORMAT @@ -1953,17 +2267,6 @@ typedef enum TUNE_FORMAT } TUNE_FORMAT; #endif -/** @brief Component capability flags (Bitmap) */ -#ifndef HAVE_ENUM_COMPONENT_CAP_FLAGS -#define HAVE_ENUM_COMPONENT_CAP_FLAGS -typedef enum COMPONENT_CAP_FLAGS -{ - COMPONENT_CAP_FLAGS_PARAM=1, /* Component has parameters, and supports the parameter protocol (PARAM messages). | */ - COMPONENT_CAP_FLAGS_PARAM_EXT=2, /* Component has parameters, and supports the extended parameter protocol (PARAM_EXT messages). | */ - COMPONENT_CAP_FLAGS_ENUM_END=3, /* | */ -} COMPONENT_CAP_FLAGS; -#endif - /** @brief Type of AIS vessel, enum duplicated from AIS standard, https://gpsd.gitlab.io/gpsd/AIVDM.html */ #ifndef HAVE_ENUM_AIS_TYPE #define HAVE_ENUM_AIS_TYPE @@ -2032,12 +2335,12 @@ typedef enum AIS_TYPE AIS_TYPE_PASSENGER=60, /* | */ AIS_TYPE_PASSENGER_HAZARDOUS_A=61, /* | */ AIS_TYPE_PASSENGER_HAZARDOUS_B=62, /* | */ - AIS_TYPE_AIS_TYPE_PASSENGER_HAZARDOUS_C=63, /* | */ + AIS_TYPE_PASSENGER_HAZARDOUS_C=63, /* | */ AIS_TYPE_PASSENGER_HAZARDOUS_D=64, /* | */ AIS_TYPE_PASSENGER_RESERVED_1=65, /* | */ AIS_TYPE_PASSENGER_RESERVED_2=66, /* | */ AIS_TYPE_PASSENGER_RESERVED_3=67, /* | */ - AIS_TYPE_AIS_TYPE_PASSENGER_RESERVED_4=68, /* | */ + AIS_TYPE_PASSENGER_RESERVED_4=68, /* | */ AIS_TYPE_PASSENGER_UNKNOWN=69, /* | */ AIS_TYPE_CARGO=70, /* | */ AIS_TYPE_CARGO_HAZARDOUS_A=71, /* | */ @@ -2078,22 +2381,22 @@ typedef enum AIS_TYPE #define HAVE_ENUM_AIS_NAV_STATUS typedef enum AIS_NAV_STATUS { - UNDER_WAY=0, /* Under way using engine. | */ - AIS_NAV_ANCHORED=1, /* | */ - AIS_NAV_UN_COMMANDED=2, /* | */ - AIS_NAV_RESTRICTED_MANOEUVERABILITY=3, /* | */ - AIS_NAV_DRAUGHT_CONSTRAINED=4, /* | */ - AIS_NAV_MOORED=5, /* | */ - AIS_NAV_AGROUND=6, /* | */ - AIS_NAV_FISHING=7, /* | */ - AIS_NAV_SAILING=8, /* | */ - AIS_NAV_RESERVED_HSC=9, /* | */ - AIS_NAV_RESERVED_WIG=10, /* | */ - AIS_NAV_RESERVED_1=11, /* | */ - AIS_NAV_RESERVED_2=12, /* | */ - AIS_NAV_RESERVED_3=13, /* | */ - AIS_NAV_AIS_SART=14, /* Search And Rescue Transponder. | */ - AIS_NAV_UNKNOWN=15, /* Not available (default). | */ + AIS_NAV_STATUS_UNDER_WAY=0, /* Under way using engine. | */ + AIS_NAV_STATUS_ANCHORED=1, /* | */ + AIS_NAV_STATUS_UN_COMMANDED=2, /* | */ + AIS_NAV_STATUS_RESTRICTED_MANOEUVERABILITY=3, /* | */ + AIS_NAV_STATUS_DRAUGHT_CONSTRAINED=4, /* | */ + AIS_NAV_STATUS_MOORED=5, /* | */ + AIS_NAV_STATUS_AGROUND=6, /* | */ + AIS_NAV_STATUS_FISHING=7, /* | */ + AIS_NAV_STATUS_SAILING=8, /* | */ + AIS_NAV_STATUS_RESERVED_HSC=9, /* | */ + AIS_NAV_STATUS_RESERVED_WIG=10, /* | */ + AIS_NAV_STATUS_RESERVED_1=11, /* | */ + AIS_NAV_STATUS_RESERVED_2=12, /* | */ + AIS_NAV_STATUS_RESERVED_3=13, /* | */ + AIS_NAV_STATUS_AIS_SART=14, /* Search And Rescue Transponder. | */ + AIS_NAV_STATUS_UNKNOWN=15, /* Not available (default). | */ AIS_NAV_STATUS_ENUM_END=16, /* | */ } AIS_NAV_STATUS; #endif @@ -2161,16 +2464,40 @@ typedef enum FAILURE_TYPE } FAILURE_TYPE; #endif +/** @brief */ +#ifndef HAVE_ENUM_NAV_VTOL_LAND_OPTIONS +#define HAVE_ENUM_NAV_VTOL_LAND_OPTIONS +typedef enum NAV_VTOL_LAND_OPTIONS +{ + NAV_VTOL_LAND_OPTIONS_DEFAULT=0, /* Default autopilot landing behaviour. | */ + NAV_VTOL_LAND_OPTIONS_FW_DESCENT=1, /* Descend in fixed wing mode, transitioning to multicopter mode for vertical landing when close to the ground. + The fixed wing descent pattern is at the discretion of the vehicle (e.g. transition altitude, loiter direction, radius, and speed, etc.). + | */ + NAV_VTOL_LAND_OPTIONS_HOVER_DESCENT=2, /* Land in multicopter mode on reaching the landing coordinates (the whole landing is by "hover descent"). | */ + NAV_VTOL_LAND_OPTIONS_ENUM_END=3, /* | */ +} NAV_VTOL_LAND_OPTIONS; +#endif + /** @brief Winch status flags used in WINCH_STATUS */ #ifndef HAVE_ENUM_MAV_WINCH_STATUS_FLAG #define HAVE_ENUM_MAV_WINCH_STATUS_FLAG typedef enum MAV_WINCH_STATUS_FLAG { MAV_WINCH_STATUS_HEALTHY=1, /* Winch is healthy | */ - MAV_WINCH_STATUS_FULLY_RETRACTED=2, /* Winch thread is fully retracted | */ + MAV_WINCH_STATUS_FULLY_RETRACTED=2, /* Winch line is fully retracted | */ MAV_WINCH_STATUS_MOVING=4, /* Winch motor is moving | */ - MAV_WINCH_STATUS_CLUTCH_ENGAGED=8, /* Winch clutch is engaged allowing motor to move freely | */ - MAV_WINCH_STATUS_FLAG_ENUM_END=9, /* | */ + MAV_WINCH_STATUS_CLUTCH_ENGAGED=8, /* Winch clutch is engaged allowing motor to move freely. | */ + MAV_WINCH_STATUS_LOCKED=16, /* Winch is locked by locking mechanism. | */ + MAV_WINCH_STATUS_DROPPING=32, /* Winch is gravity dropping payload. | */ + MAV_WINCH_STATUS_ARRESTING=64, /* Winch is arresting payload descent. | */ + MAV_WINCH_STATUS_GROUND_SENSE=128, /* Winch is using torque measurements to sense the ground. | */ + MAV_WINCH_STATUS_RETRACTING=256, /* Winch is returning to the fully retracted position. | */ + MAV_WINCH_STATUS_REDELIVER=512, /* Winch is redelivering the payload. This is a failover state if the line tension goes above a threshold during RETRACTING. | */ + MAV_WINCH_STATUS_ABANDON_LINE=1024, /* Winch is abandoning the line and possibly payload. Winch unspools the entire calculated line length. This is a failover state from REDELIVER if the number of attempts exceeds a threshold. | */ + MAV_WINCH_STATUS_LOCKING=2048, /* Winch is engaging the locking mechanism. | */ + MAV_WINCH_STATUS_LOAD_LINE=4096, /* Winch is spooling on line. | */ + MAV_WINCH_STATUS_LOAD_PAYLOAD=8192, /* Winch is loading a payload. | */ + MAV_WINCH_STATUS_FLAG_ENUM_END=8193, /* | */ } MAV_WINCH_STATUS_FLAG; #endif @@ -2191,6 +2518,286 @@ typedef enum MAG_CAL_STATUS } MAG_CAL_STATUS; #endif +/** @brief Reason for an event error response. */ +#ifndef HAVE_ENUM_MAV_EVENT_ERROR_REASON +#define HAVE_ENUM_MAV_EVENT_ERROR_REASON +typedef enum MAV_EVENT_ERROR_REASON +{ + MAV_EVENT_ERROR_REASON_UNAVAILABLE=0, /* The requested event is not available (anymore). | */ + MAV_EVENT_ERROR_REASON_ENUM_END=1, /* | */ +} MAV_EVENT_ERROR_REASON; +#endif + +/** @brief Flags for CURRENT_EVENT_SEQUENCE. */ +#ifndef HAVE_ENUM_MAV_EVENT_CURRENT_SEQUENCE_FLAGS +#define HAVE_ENUM_MAV_EVENT_CURRENT_SEQUENCE_FLAGS +typedef enum MAV_EVENT_CURRENT_SEQUENCE_FLAGS +{ + MAV_EVENT_CURRENT_SEQUENCE_FLAGS_RESET=1, /* A sequence reset has happened (e.g. vehicle reboot). | */ + MAV_EVENT_CURRENT_SEQUENCE_FLAGS_ENUM_END=2, /* | */ +} MAV_EVENT_CURRENT_SEQUENCE_FLAGS; +#endif + +/** @brief Flags in the HIL_SENSOR message indicate which fields have updated since the last message */ +#ifndef HAVE_ENUM_HIL_SENSOR_UPDATED_FLAGS +#define HAVE_ENUM_HIL_SENSOR_UPDATED_FLAGS +typedef enum HIL_SENSOR_UPDATED_FLAGS +{ + HIL_SENSOR_UPDATED_XACC=1, /* The value in the xacc field has been updated | */ + HIL_SENSOR_UPDATED_YACC=2, /* The value in the yacc field has been updated | */ + HIL_SENSOR_UPDATED_ZACC=4, /* The value in the zacc field has been updated | */ + HIL_SENSOR_UPDATED_XGYRO=8, /* The value in the xgyro field has been updated | */ + HIL_SENSOR_UPDATED_YGYRO=16, /* The value in the ygyro field has been updated | */ + HIL_SENSOR_UPDATED_ZGYRO=32, /* The value in the zgyro field has been updated | */ + HIL_SENSOR_UPDATED_XMAG=64, /* The value in the xmag field has been updated | */ + HIL_SENSOR_UPDATED_YMAG=128, /* The value in the ymag field has been updated | */ + HIL_SENSOR_UPDATED_ZMAG=256, /* The value in the zmag field has been updated | */ + HIL_SENSOR_UPDATED_ABS_PRESSURE=512, /* The value in the abs_pressure field has been updated | */ + HIL_SENSOR_UPDATED_DIFF_PRESSURE=1024, /* The value in the diff_pressure field has been updated | */ + HIL_SENSOR_UPDATED_PRESSURE_ALT=2048, /* The value in the pressure_alt field has been updated | */ + HIL_SENSOR_UPDATED_TEMPERATURE=4096, /* The value in the temperature field has been updated | */ + HIL_SENSOR_UPDATED_RESET=2147483648, /* Full reset of attitude/position/velocities/etc was performed in sim (Bit 31). | */ + HIL_SENSOR_UPDATED_FLAGS_ENUM_END=2147483649, /* | */ +} HIL_SENSOR_UPDATED_FLAGS; +#endif + +/** @brief Flags in the HIGHRES_IMU message indicate which fields have updated since the last message */ +#ifndef HAVE_ENUM_HIGHRES_IMU_UPDATED_FLAGS +#define HAVE_ENUM_HIGHRES_IMU_UPDATED_FLAGS +typedef enum HIGHRES_IMU_UPDATED_FLAGS +{ + HIGHRES_IMU_UPDATED_XACC=1, /* The value in the xacc field has been updated | */ + HIGHRES_IMU_UPDATED_YACC=2, /* The value in the yacc field has been updated | */ + HIGHRES_IMU_UPDATED_ZACC=4, /* The value in the zacc field has been updated since | */ + HIGHRES_IMU_UPDATED_XGYRO=8, /* The value in the xgyro field has been updated | */ + HIGHRES_IMU_UPDATED_YGYRO=16, /* The value in the ygyro field has been updated | */ + HIGHRES_IMU_UPDATED_ZGYRO=32, /* The value in the zgyro field has been updated | */ + HIGHRES_IMU_UPDATED_XMAG=64, /* The value in the xmag field has been updated | */ + HIGHRES_IMU_UPDATED_YMAG=128, /* The value in the ymag field has been updated | */ + HIGHRES_IMU_UPDATED_ZMAG=256, /* The value in the zmag field has been updated | */ + HIGHRES_IMU_UPDATED_ABS_PRESSURE=512, /* The value in the abs_pressure field has been updated | */ + HIGHRES_IMU_UPDATED_DIFF_PRESSURE=1024, /* The value in the diff_pressure field has been updated | */ + HIGHRES_IMU_UPDATED_PRESSURE_ALT=2048, /* The value in the pressure_alt field has been updated | */ + HIGHRES_IMU_UPDATED_TEMPERATURE=4096, /* The value in the temperature field has been updated | */ + HIGHRES_IMU_UPDATED_FLAGS_ENUM_END=4097, /* | */ +} HIGHRES_IMU_UPDATED_FLAGS; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_CAN_FILTER_OP +#define HAVE_ENUM_CAN_FILTER_OP +typedef enum CAN_FILTER_OP +{ + CAN_FILTER_REPLACE=0, /* | */ + CAN_FILTER_ADD=1, /* | */ + CAN_FILTER_REMOVE=2, /* | */ + CAN_FILTER_OP_ENUM_END=3, /* | */ +} CAN_FILTER_OP; +#endif + +/** @brief MAV FTP error codes (https://mavlink.io/en/services/ftp.html) */ +#ifndef HAVE_ENUM_MAV_FTP_ERR +#define HAVE_ENUM_MAV_FTP_ERR +typedef enum MAV_FTP_ERR +{ + MAV_FTP_ERR_NONE=0, /* None: No error | */ + MAV_FTP_ERR_FAIL=1, /* Fail: Unknown failure | */ + MAV_FTP_ERR_FAILERRNO=2, /* FailErrno: Command failed, Err number sent back in PayloadHeader.data[1]. + This is a file-system error number understood by the server operating system. | */ + MAV_FTP_ERR_INVALIDDATASIZE=3, /* InvalidDataSize: Payload size is invalid | */ + MAV_FTP_ERR_INVALIDSESSION=4, /* InvalidSession: Session is not currently open | */ + MAV_FTP_ERR_NOSESSIONSAVAILABLE=5, /* NoSessionsAvailable: All available sessions are already in use | */ + MAV_FTP_ERR_EOF=6, /* EOF: Offset past end of file for ListDirectory and ReadFile commands | */ + MAV_FTP_ERR_UNKNOWNCOMMAND=7, /* UnknownCommand: Unknown command / opcode | */ + MAV_FTP_ERR_FILEEXISTS=8, /* FileExists: File/directory already exists | */ + MAV_FTP_ERR_FILEPROTECTED=9, /* FileProtected: File/directory is write protected | */ + MAV_FTP_ERR_FILENOTFOUND=10, /* FileNotFound: File/directory not found | */ + MAV_FTP_ERR_ENUM_END=11, /* | */ +} MAV_FTP_ERR; +#endif + +/** @brief MAV FTP opcodes: https://mavlink.io/en/services/ftp.html */ +#ifndef HAVE_ENUM_MAV_FTP_OPCODE +#define HAVE_ENUM_MAV_FTP_OPCODE +typedef enum MAV_FTP_OPCODE +{ + MAV_FTP_OPCODE_NONE=0, /* None. Ignored, always ACKed | */ + MAV_FTP_OPCODE_TERMINATESESSION=1, /* TerminateSession: Terminates open Read session | */ + MAV_FTP_OPCODE_RESETSESSION=2, /* ResetSessions: Terminates all open read sessions | */ + MAV_FTP_OPCODE_LISTDIRECTORY=3, /* ListDirectory. List files and directories in path from offset | */ + MAV_FTP_OPCODE_OPENFILERO=4, /* OpenFileRO: Opens file at path for reading, returns session | */ + MAV_FTP_OPCODE_READFILE=5, /* ReadFile: Reads size bytes from offset in session | */ + MAV_FTP_OPCODE_CREATEFILE=6, /* CreateFile: Creates file at path for writing, returns session | */ + MAV_FTP_OPCODE_WRITEFILE=7, /* WriteFile: Writes size bytes to offset in session | */ + MAV_FTP_OPCODE_REMOVEFILE=8, /* RemoveFile: Remove file at path | */ + MAV_FTP_OPCODE_CREATEDIRECTORY=9, /* CreateDirectory: Creates directory at path | */ + MAV_FTP_OPCODE_REMOVEDIRECTORY=10, /* RemoveDirectory: Removes directory at path. The directory must be empty. | */ + MAV_FTP_OPCODE_OPENFILEWO=11, /* OpenFileWO: Opens file at path for writing, returns session | */ + MAV_FTP_OPCODE_TRUNCATEFILE=12, /* TruncateFile: Truncate file at path to offset length | */ + MAV_FTP_OPCODE_RENAME=13, /* Rename: Rename path1 to path2 | */ + MAV_FTP_OPCODE_CALCFILECRC=14, /* CalcFileCRC32: Calculate CRC32 for file at path | */ + MAV_FTP_OPCODE_BURSTREADFILE=15, /* BurstReadFile: Burst download session file | */ + MAV_FTP_OPCODE_ACK=128, /* ACK: ACK response | */ + MAV_FTP_OPCODE_NAK=129, /* NAK: NAK response | */ + MAV_FTP_OPCODE_ENUM_END=130, /* | */ +} MAV_FTP_OPCODE; +#endif + +/** @brief + States of the mission state machine. + Note that these states are independent of whether the mission is in a mode that can execute mission items or not (is suspended). + They may not all be relevant on all vehicles. + */ +#ifndef HAVE_ENUM_MISSION_STATE +#define HAVE_ENUM_MISSION_STATE +typedef enum MISSION_STATE +{ + MISSION_STATE_UNKNOWN=0, /* The mission status reporting is not supported. | */ + MISSION_STATE_NO_MISSION=1, /* No mission on the vehicle. | */ + MISSION_STATE_NOT_STARTED=2, /* Mission has not started. This is the case after a mission has uploaded but not yet started executing. | */ + MISSION_STATE_ACTIVE=3, /* Mission is active, and will execute mission items when in auto mode. | */ + MISSION_STATE_PAUSED=4, /* Mission is paused when in auto mode. | */ + MISSION_STATE_COMPLETE=5, /* Mission has executed all mission items. | */ + MISSION_STATE_ENUM_END=6, /* | */ +} MISSION_STATE; +#endif + +/** @brief + Possible safety switch states. + */ +#ifndef HAVE_ENUM_SAFETY_SWITCH_STATE +#define HAVE_ENUM_SAFETY_SWITCH_STATE +typedef enum SAFETY_SWITCH_STATE +{ + SAFETY_SWITCH_STATE_SAFE=0, /* Safety switch is engaged and vehicle should be safe to approach. | */ + SAFETY_SWITCH_STATE_DANGEROUS=1, /* Safety switch is NOT engaged and motors, propellers and other actuators should be considered active. | */ + SAFETY_SWITCH_STATE_ENUM_END=2, /* | */ +} SAFETY_SWITCH_STATE; +#endif + +/** @brief Modes of illuminator */ +#ifndef HAVE_ENUM_ILLUMINATOR_MODE +#define HAVE_ENUM_ILLUMINATOR_MODE +typedef enum ILLUMINATOR_MODE +{ + ILLUMINATOR_MODE_UNKNOWN=0, /* Illuminator mode is not specified/unknown | */ + ILLUMINATOR_MODE_INTERNAL_CONTROL=1, /* Illuminator behavior is controlled by MAV_CMD_DO_ILLUMINATOR_CONFIGURE settings | */ + ILLUMINATOR_MODE_EXTERNAL_SYNC=2, /* Illuminator behavior is controlled by external factors: e.g. an external hardware signal | */ + ILLUMINATOR_MODE_ENUM_END=3, /* | */ +} ILLUMINATOR_MODE; +#endif + +/** @brief Illuminator module error flags (bitmap, 0 means no error) */ +#ifndef HAVE_ENUM_ILLUMINATOR_ERROR_FLAGS +#define HAVE_ENUM_ILLUMINATOR_ERROR_FLAGS +typedef enum ILLUMINATOR_ERROR_FLAGS +{ + ILLUMINATOR_ERROR_FLAGS_THERMAL_THROTTLING=1, /* Illuminator thermal throttling error. | */ + ILLUMINATOR_ERROR_FLAGS_OVER_TEMPERATURE_SHUTDOWN=2, /* Illuminator over temperature shutdown error. | */ + ILLUMINATOR_ERROR_FLAGS_THERMISTOR_FAILURE=4, /* Illuminator thermistor failure. | */ + ILLUMINATOR_ERROR_FLAGS_ENUM_END=5, /* | */ +} ILLUMINATOR_ERROR_FLAGS; +#endif + +/** @brief Standard modes with a well understood meaning across flight stacks and vehicle types. + For example, most flight stack have the concept of a "return" or "RTL" mode that takes a vehicle to safety, even though the precise mechanics of this mode may differ. + The modes supported by a flight stack can be queried using AVAILABLE_MODES and set using MAV_CMD_DO_SET_STANDARD_MODE. + The current mode is streamed in CURRENT_MODE. + See https://mavlink.io/en/services/standard_modes.html + */ +#ifndef HAVE_ENUM_MAV_STANDARD_MODE +#define HAVE_ENUM_MAV_STANDARD_MODE +typedef enum MAV_STANDARD_MODE +{ + MAV_STANDARD_MODE_NON_STANDARD=0, /* Non standard mode. + This may be used when reporting the mode if the current flight mode is not a standard mode. + | */ + MAV_STANDARD_MODE_POSITION_HOLD=1, /* Position mode (manual). + Position-controlled and stabilized manual mode. + When sticks are released vehicles return to their level-flight orientation and hold both position and altitude against wind and external forces. + This mode can only be set by vehicles that can hold a fixed position. + Multicopter (MC) vehicles actively brake and hold both position and altitude against wind and external forces. + Hybrid MC/FW ("VTOL") vehicles first transition to multicopter mode (if needed) but otherwise behave in the same way as MC vehicles. + Fixed-wing (FW) vehicles must not support this mode. + Other vehicle types must not support this mode (this may be revisited through the PR process). + | */ + MAV_STANDARD_MODE_ORBIT=2, /* Orbit (manual). + Position-controlled and stabilized manual mode. + The vehicle circles around a fixed setpoint in the horizontal plane at a particular radius, altitude, and direction. + Flight stacks may further allow manual control over the setpoint position, radius, direction, speed, and/or altitude of the circle, but this is not mandated. + Flight stacks may support the [MAV_CMD_DO_ORBIT](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_ORBIT) for changing the orbit parameters. + MC and FW vehicles may support this mode. + Hybrid MC/FW ("VTOL") vehicles may support this mode in MC/FW or both modes; if the mode is not supported by the current configuration the vehicle should transition to the supported configuration. + Other vehicle types must not support this mode (this may be revisited through the PR process). + | */ + MAV_STANDARD_MODE_CRUISE=3, /* Cruise mode (manual). + Position-controlled and stabilized manual mode. + When sticks are released vehicles return to their level-flight orientation and hold their original track against wind and external forces. + Fixed-wing (FW) vehicles level orientation and maintain current track and altitude against wind and external forces. + Hybrid MC/FW ("VTOL") vehicles first transition to FW mode (if needed) but otherwise behave in the same way as MC vehicles. + Multicopter (MC) vehicles must not support this mode. + Other vehicle types must not support this mode (this may be revisited through the PR process). + | */ + MAV_STANDARD_MODE_ALTITUDE_HOLD=4, /* Altitude hold (manual). + Altitude-controlled and stabilized manual mode. + When sticks are released vehicles return to their level-flight orientation and hold their altitude. + MC vehicles continue with existing momentum and may move with wind (or other external forces). + FW vehicles continue with current heading, but may be moved off-track by wind. + Hybrid MC/FW ("VTOL") vehicles behave according to their current configuration/mode (FW or MC). + Other vehicle types must not support this mode (this may be revisited through the PR process). + | */ + MAV_STANDARD_MODE_SAFE_RECOVERY=5, /* Safe recovery mode (auto). + Automatic mode that takes vehicle to a predefined safe location via a safe flight path, and may also automatically land the vehicle. + This mode is more commonly referred to as RTL and/or or Smart RTL. + The precise return location, flight path, and landing behaviour depend on vehicle configuration and type. + For example, the vehicle might return to the home/launch location, a rally point, or the start of a mission landing, it might follow a direct path, mission path, or breadcrumb path, and land using a mission landing pattern or some other kind of descent. + | */ + MAV_STANDARD_MODE_MISSION=6, /* Mission mode (automatic). + Automatic mode that executes MAVLink missions. + Missions are executed from the current waypoint as soon as the mode is enabled. + | */ + MAV_STANDARD_MODE_LAND=7, /* Land mode (auto). + Automatic mode that lands the vehicle at the current location. + The precise landing behaviour depends on vehicle configuration and type. + | */ + MAV_STANDARD_MODE_TAKEOFF=8, /* Takeoff mode (auto). + Automatic takeoff mode. + The precise takeoff behaviour depends on vehicle configuration and type. + | */ + MAV_STANDARD_MODE_ENUM_END=9, /* | */ +} MAV_STANDARD_MODE; +#endif + +/** @brief Mode properties. + */ +#ifndef HAVE_ENUM_MAV_MODE_PROPERTY +#define HAVE_ENUM_MAV_MODE_PROPERTY +typedef enum MAV_MODE_PROPERTY +{ + MAV_MODE_PROPERTY_ADVANCED=1, /* If set, this mode is an advanced mode. + For example a rate-controlled manual mode might be advanced, whereas a position-controlled manual mode is not. + A GCS can optionally use this flag to configure the UI for its intended users. + | */ + MAV_MODE_PROPERTY_NOT_USER_SELECTABLE=2, /* If set, this mode should not be added to the list of selectable modes. + The mode might still be selected by the FC directly (for example as part of a failsafe). + | */ + MAV_MODE_PROPERTY_AUTO_MODE=4, /* If set, this mode is automatically controlled (it may use but does not require a manual controller). + If unset the mode is a assumed to require user input (be a manual mode). + | */ + MAV_MODE_PROPERTY_ENUM_END=5, /* | */ +} MAV_MODE_PROPERTY; +#endif + +/** @brief Flags used in HIL_ACTUATOR_CONTROLS message. */ +#ifndef HAVE_ENUM_HIL_ACTUATOR_CONTROLS_FLAGS +#define HAVE_ENUM_HIL_ACTUATOR_CONTROLS_FLAGS +typedef enum HIL_ACTUATOR_CONTROLS_FLAGS +{ + HIL_ACTUATOR_CONTROLS_FLAGS_LOCKSTEP=1, /* Simulation is using lockstep | */ + HIL_ACTUATOR_CONTROLS_FLAGS_ENUM_END=2, /* | */ +} HIL_ACTUATOR_CONTROLS_FLAGS; +#endif + // MAVLINK VERSION #ifndef MAVLINK_VERSION @@ -2211,7 +2818,6 @@ typedef enum MAG_CAL_STATUS #include "./mavlink_msg_auth_key.h" #include "./mavlink_msg_link_node_status.h" #include "./mavlink_msg_set_mode.h" -#include "./mavlink_msg_param_ack_transaction.h" #include "./mavlink_msg_param_request_read.h" #include "./mavlink_msg_param_request_list.h" #include "./mavlink_msg_param_value.h" @@ -2244,7 +2850,6 @@ typedef enum MAG_CAL_STATUS #include "./mavlink_msg_gps_global_origin.h" #include "./mavlink_msg_param_map_rc.h" #include "./mavlink_msg_mission_request_int.h" -#include "./mavlink_msg_mission_changed.h" #include "./mavlink_msg_safety_set_allowed_area.h" #include "./mavlink_msg_safety_allowed_area.h" #include "./mavlink_msg_attitude_quaternion_cov.h" @@ -2321,7 +2926,6 @@ typedef enum MAG_CAL_STATUS #include "./mavlink_msg_follow_target.h" #include "./mavlink_msg_control_system_state.h" #include "./mavlink_msg_battery_status.h" -#include "./mavlink_msg_autopilot_version.h" #include "./mavlink_msg_landing_target.h" #include "./mavlink_msg_fence_status.h" #include "./mavlink_msg_mag_cal_report.h" @@ -2364,6 +2968,7 @@ typedef enum MAG_CAL_STATUS #include "./mavlink_msg_camera_fov_status.h" #include "./mavlink_msg_camera_tracking_image_status.h" #include "./mavlink_msg_camera_tracking_geo_status.h" +#include "./mavlink_msg_camera_thermal_range.h" #include "./mavlink_msg_gimbal_manager_information.h" #include "./mavlink_msg_gimbal_manager_status.h" #include "./mavlink_msg_gimbal_manager_set_attitude.h" @@ -2396,14 +3001,29 @@ typedef enum MAG_CAL_STATUS #include "./mavlink_msg_debug_float_array.h" #include "./mavlink_msg_orbit_execution_status.h" #include "./mavlink_msg_smart_battery_info.h" +#include "./mavlink_msg_fuel_status.h" +#include "./mavlink_msg_battery_info.h" #include "./mavlink_msg_generator_status.h" #include "./mavlink_msg_actuator_output_status.h" #include "./mavlink_msg_time_estimate_to_target.h" #include "./mavlink_msg_tunnel.h" +#include "./mavlink_msg_can_frame.h" #include "./mavlink_msg_onboard_computer_status.h" #include "./mavlink_msg_component_information.h" +#include "./mavlink_msg_component_information_basic.h" +#include "./mavlink_msg_component_metadata.h" #include "./mavlink_msg_play_tune_v2.h" #include "./mavlink_msg_supported_tunes.h" +#include "./mavlink_msg_event.h" +#include "./mavlink_msg_current_event_sequence.h" +#include "./mavlink_msg_request_event.h" +#include "./mavlink_msg_response_event_error.h" +#include "./mavlink_msg_available_modes.h" +#include "./mavlink_msg_current_mode.h" +#include "./mavlink_msg_available_modes_monitor.h" +#include "./mavlink_msg_illuminator_status.h" +#include "./mavlink_msg_canfd_frame.h" +#include "./mavlink_msg_can_filter_modify.h" #include "./mavlink_msg_wheel_distance.h" #include "./mavlink_msg_winch_status.h" #include "./mavlink_msg_open_drone_id_basic_id.h" @@ -2413,16 +3033,17 @@ typedef enum MAG_CAL_STATUS #include "./mavlink_msg_open_drone_id_system.h" #include "./mavlink_msg_open_drone_id_operator_id.h" #include "./mavlink_msg_open_drone_id_message_pack.h" +#include "./mavlink_msg_open_drone_id_arm_status.h" +#include "./mavlink_msg_open_drone_id_system_update.h" +#include "./mavlink_msg_hygrometer_sensor.h" // base include -#include "../minimal/minimal.h" +#include "../standard/standard.h" -#undef MAVLINK_THIS_XML_IDX -#define MAVLINK_THIS_XML_IDX 0 -#if MAVLINK_THIS_XML_IDX == MAVLINK_PRIMARY_XML_IDX -# define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, MAVLINK_MESSAGE_INFO_LINK_NODE_STATUS, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_PARAM_ACK_TRANSACTION, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_PARAM_MAP_RC, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_INT, MAVLINK_MESSAGE_INFO_MISSION_CHANGED, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION_COV, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT_COV, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_COV, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, MAVLINK_MESSAGE_INFO_MISSION_ITEM_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_INT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, MAVLINK_MESSAGE_INFO_COMMAND_CANCEL, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_SET_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_GLOBAL_INT, MAVLINK_MESSAGE_INFO_POSITION_TARGET_GLOBAL_INT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, MAVLINK_MESSAGE_INFO_HIL_ACTUATOR_CONTROLS, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW_RAD, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_PROTOCOL, MAVLINK_MESSAGE_INFO_TIMESYNC, MAVLINK_MESSAGE_INFO_CAMERA_TRIGGER, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, MAVLINK_MESSAGE_INFO_GPS_RTK, MAVLINK_MESSAGE_INFO_GPS2_RTK, MAVLINK_MESSAGE_INFO_SCALED_IMU3, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, MAVLINK_MESSAGE_INFO_TERRAIN_REQUEST, MAVLINK_MESSAGE_INFO_TERRAIN_DATA, MAVLINK_MESSAGE_INFO_TERRAIN_CHECK, MAVLINK_MESSAGE_INFO_TERRAIN_REPORT, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE2, MAVLINK_MESSAGE_INFO_ATT_POS_MOCAP, MAVLINK_MESSAGE_INFO_SET_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ALTITUDE, MAVLINK_MESSAGE_INFO_RESOURCE_REQUEST, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE3, MAVLINK_MESSAGE_INFO_FOLLOW_TARGET, MAVLINK_MESSAGE_INFO_CONTROL_SYSTEM_STATE, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION, MAVLINK_MESSAGE_INFO_LANDING_TARGET, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT, MAVLINK_MESSAGE_INFO_EFI_STATUS, MAVLINK_MESSAGE_INFO_ESTIMATOR_STATUS, MAVLINK_MESSAGE_INFO_WIND_COV, MAVLINK_MESSAGE_INFO_GPS_INPUT, MAVLINK_MESSAGE_INFO_GPS_RTCM_DATA, MAVLINK_MESSAGE_INFO_HIGH_LATENCY, MAVLINK_MESSAGE_INFO_HIGH_LATENCY2, MAVLINK_MESSAGE_INFO_VIBRATION, MAVLINK_MESSAGE_INFO_HOME_POSITION, MAVLINK_MESSAGE_INFO_SET_HOME_POSITION, MAVLINK_MESSAGE_INFO_MESSAGE_INTERVAL, MAVLINK_MESSAGE_INFO_EXTENDED_SYS_STATE, MAVLINK_MESSAGE_INFO_ADSB_VEHICLE, MAVLINK_MESSAGE_INFO_COLLISION, MAVLINK_MESSAGE_INFO_V2_EXTENSION, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, MAVLINK_MESSAGE_INFO_SETUP_SIGNING, MAVLINK_MESSAGE_INFO_BUTTON_CHANGE, MAVLINK_MESSAGE_INFO_PLAY_TUNE, MAVLINK_MESSAGE_INFO_CAMERA_INFORMATION, MAVLINK_MESSAGE_INFO_CAMERA_SETTINGS, MAVLINK_MESSAGE_INFO_STORAGE_INFORMATION, MAVLINK_MESSAGE_INFO_CAMERA_CAPTURE_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_IMAGE_CAPTURED, MAVLINK_MESSAGE_INFO_FLIGHT_INFORMATION, MAVLINK_MESSAGE_INFO_MOUNT_ORIENTATION, MAVLINK_MESSAGE_INFO_LOGGING_DATA, MAVLINK_MESSAGE_INFO_LOGGING_DATA_ACKED, MAVLINK_MESSAGE_INFO_LOGGING_ACK, MAVLINK_MESSAGE_INFO_VIDEO_STREAM_INFORMATION, MAVLINK_MESSAGE_INFO_VIDEO_STREAM_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_FOV_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_IMAGE_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_GEO_STATUS, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_INFORMATION, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_STATUS, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_ATTITUDE, MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_INFORMATION, MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_SET_ATTITUDE, MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_ATTITUDE_STATUS, MAVLINK_MESSAGE_INFO_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_PITCHYAW, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_ESC_INFO, MAVLINK_MESSAGE_INFO_ESC_STATUS, MAVLINK_MESSAGE_INFO_WIFI_CONFIG_AP, MAVLINK_MESSAGE_INFO_PROTOCOL_VERSION, MAVLINK_MESSAGE_INFO_AIS_VESSEL, MAVLINK_MESSAGE_INFO_UAVCAN_NODE_STATUS, MAVLINK_MESSAGE_INFO_UAVCAN_NODE_INFO, MAVLINK_MESSAGE_INFO_PARAM_EXT_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_EXT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_EXT_VALUE, MAVLINK_MESSAGE_INFO_PARAM_EXT_SET, MAVLINK_MESSAGE_INFO_PARAM_EXT_ACK, MAVLINK_MESSAGE_INFO_OBSTACLE_DISTANCE, MAVLINK_MESSAGE_INFO_ODOMETRY, MAVLINK_MESSAGE_INFO_TRAJECTORY_REPRESENTATION_WAYPOINTS, MAVLINK_MESSAGE_INFO_TRAJECTORY_REPRESENTATION_BEZIER, MAVLINK_MESSAGE_INFO_CELLULAR_STATUS, MAVLINK_MESSAGE_INFO_ISBD_LINK_STATUS, MAVLINK_MESSAGE_INFO_CELLULAR_CONFIG, MAVLINK_MESSAGE_INFO_RAW_RPM, MAVLINK_MESSAGE_INFO_UTM_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_DEBUG_FLOAT_ARRAY, MAVLINK_MESSAGE_INFO_ORBIT_EXECUTION_STATUS, MAVLINK_MESSAGE_INFO_SMART_BATTERY_INFO, MAVLINK_MESSAGE_INFO_GENERATOR_STATUS, MAVLINK_MESSAGE_INFO_ACTUATOR_OUTPUT_STATUS, MAVLINK_MESSAGE_INFO_TIME_ESTIMATE_TO_TARGET, MAVLINK_MESSAGE_INFO_TUNNEL, MAVLINK_MESSAGE_INFO_ONBOARD_COMPUTER_STATUS, MAVLINK_MESSAGE_INFO_COMPONENT_INFORMATION, MAVLINK_MESSAGE_INFO_PLAY_TUNE_V2, MAVLINK_MESSAGE_INFO_SUPPORTED_TUNES, MAVLINK_MESSAGE_INFO_WHEEL_DISTANCE, MAVLINK_MESSAGE_INFO_WINCH_STATUS, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_BASIC_ID, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_LOCATION, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_AUTHENTICATION, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SELF_ID, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SYSTEM, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_OPERATOR_ID, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_MESSAGE_PACK} -# define MAVLINK_MESSAGE_NAMES {{ "ACTUATOR_CONTROL_TARGET", 140 }, { "ACTUATOR_OUTPUT_STATUS", 375 }, { "ADSB_VEHICLE", 246 }, { "AIS_VESSEL", 301 }, { "ALTITUDE", 141 }, { "ATTITUDE", 30 }, { "ATTITUDE_QUATERNION", 31 }, { "ATTITUDE_QUATERNION_COV", 61 }, { "ATTITUDE_TARGET", 83 }, { "ATT_POS_MOCAP", 138 }, { "AUTH_KEY", 7 }, { "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE", 286 }, { "AUTOPILOT_VERSION", 148 }, { "BATTERY_STATUS", 147 }, { "BUTTON_CHANGE", 257 }, { "CAMERA_CAPTURE_STATUS", 262 }, { "CAMERA_FOV_STATUS", 271 }, { "CAMERA_IMAGE_CAPTURED", 263 }, { "CAMERA_INFORMATION", 259 }, { "CAMERA_SETTINGS", 260 }, { "CAMERA_TRACKING_GEO_STATUS", 276 }, { "CAMERA_TRACKING_IMAGE_STATUS", 275 }, { "CAMERA_TRIGGER", 112 }, { "CELLULAR_CONFIG", 336 }, { "CELLULAR_STATUS", 334 }, { "CHANGE_OPERATOR_CONTROL", 5 }, { "CHANGE_OPERATOR_CONTROL_ACK", 6 }, { "COLLISION", 247 }, { "COMMAND_ACK", 77 }, { "COMMAND_CANCEL", 80 }, { "COMMAND_INT", 75 }, { "COMMAND_LONG", 76 }, { "COMPONENT_INFORMATION", 395 }, { "CONTROL_SYSTEM_STATE", 146 }, { "DATA_STREAM", 67 }, { "DATA_TRANSMISSION_HANDSHAKE", 130 }, { "DEBUG", 254 }, { "DEBUG_FLOAT_ARRAY", 350 }, { "DEBUG_VECT", 250 }, { "DISTANCE_SENSOR", 132 }, { "EFI_STATUS", 225 }, { "ENCAPSULATED_DATA", 131 }, { "ESC_INFO", 290 }, { "ESC_STATUS", 291 }, { "ESTIMATOR_STATUS", 230 }, { "EXTENDED_SYS_STATE", 245 }, { "FENCE_STATUS", 162 }, { "FILE_TRANSFER_PROTOCOL", 110 }, { "FLIGHT_INFORMATION", 264 }, { "FOLLOW_TARGET", 144 }, { "GENERATOR_STATUS", 373 }, { "GIMBAL_DEVICE_ATTITUDE_STATUS", 285 }, { "GIMBAL_DEVICE_INFORMATION", 283 }, { "GIMBAL_DEVICE_SET_ATTITUDE", 284 }, { "GIMBAL_MANAGER_INFORMATION", 280 }, { "GIMBAL_MANAGER_SET_ATTITUDE", 282 }, { "GIMBAL_MANAGER_SET_MANUAL_CONTROL", 288 }, { "GIMBAL_MANAGER_SET_PITCHYAW", 287 }, { "GIMBAL_MANAGER_STATUS", 281 }, { "GLOBAL_POSITION_INT", 33 }, { "GLOBAL_POSITION_INT_COV", 63 }, { "GLOBAL_VISION_POSITION_ESTIMATE", 101 }, { "GPS2_RAW", 124 }, { "GPS2_RTK", 128 }, { "GPS_GLOBAL_ORIGIN", 49 }, { "GPS_INJECT_DATA", 123 }, { "GPS_INPUT", 232 }, { "GPS_RAW_INT", 24 }, { "GPS_RTCM_DATA", 233 }, { "GPS_RTK", 127 }, { "GPS_STATUS", 25 }, { "HEARTBEAT", 0 }, { "HIGHRES_IMU", 105 }, { "HIGH_LATENCY", 234 }, { "HIGH_LATENCY2", 235 }, { "HIL_ACTUATOR_CONTROLS", 93 }, { "HIL_CONTROLS", 91 }, { "HIL_GPS", 113 }, { "HIL_OPTICAL_FLOW", 114 }, { "HIL_RC_INPUTS_RAW", 92 }, { "HIL_SENSOR", 107 }, { "HIL_STATE", 90 }, { "HIL_STATE_QUATERNION", 115 }, { "HOME_POSITION", 242 }, { "ISBD_LINK_STATUS", 335 }, { "LANDING_TARGET", 149 }, { "LINK_NODE_STATUS", 8 }, { "LOCAL_POSITION_NED", 32 }, { "LOCAL_POSITION_NED_COV", 64 }, { "LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET", 89 }, { "LOGGING_ACK", 268 }, { "LOGGING_DATA", 266 }, { "LOGGING_DATA_ACKED", 267 }, { "LOG_DATA", 120 }, { "LOG_ENTRY", 118 }, { "LOG_ERASE", 121 }, { "LOG_REQUEST_DATA", 119 }, { "LOG_REQUEST_END", 122 }, { "LOG_REQUEST_LIST", 117 }, { "MAG_CAL_REPORT", 192 }, { "MANUAL_CONTROL", 69 }, { "MANUAL_SETPOINT", 81 }, { "MEMORY_VECT", 249 }, { "MESSAGE_INTERVAL", 244 }, { "MISSION_ACK", 47 }, { "MISSION_CHANGED", 52 }, { "MISSION_CLEAR_ALL", 45 }, { "MISSION_COUNT", 44 }, { "MISSION_CURRENT", 42 }, { "MISSION_ITEM", 39 }, { "MISSION_ITEM_INT", 73 }, { "MISSION_ITEM_REACHED", 46 }, { "MISSION_REQUEST", 40 }, { "MISSION_REQUEST_INT", 51 }, { "MISSION_REQUEST_LIST", 43 }, { "MISSION_REQUEST_PARTIAL_LIST", 37 }, { "MISSION_SET_CURRENT", 41 }, { "MISSION_WRITE_PARTIAL_LIST", 38 }, { "MOUNT_ORIENTATION", 265 }, { "NAMED_VALUE_FLOAT", 251 }, { "NAMED_VALUE_INT", 252 }, { "NAV_CONTROLLER_OUTPUT", 62 }, { "OBSTACLE_DISTANCE", 330 }, { "ODOMETRY", 331 }, { "ONBOARD_COMPUTER_STATUS", 390 }, { "OPEN_DRONE_ID_AUTHENTICATION", 12902 }, { "OPEN_DRONE_ID_BASIC_ID", 12900 }, { "OPEN_DRONE_ID_LOCATION", 12901 }, { "OPEN_DRONE_ID_MESSAGE_PACK", 12915 }, { "OPEN_DRONE_ID_OPERATOR_ID", 12905 }, { "OPEN_DRONE_ID_SELF_ID", 12903 }, { "OPEN_DRONE_ID_SYSTEM", 12904 }, { "OPTICAL_FLOW", 100 }, { "OPTICAL_FLOW_RAD", 106 }, { "ORBIT_EXECUTION_STATUS", 360 }, { "PARAM_ACK_TRANSACTION", 19 }, { "PARAM_EXT_ACK", 324 }, { "PARAM_EXT_REQUEST_LIST", 321 }, { "PARAM_EXT_REQUEST_READ", 320 }, { "PARAM_EXT_SET", 323 }, { "PARAM_EXT_VALUE", 322 }, { "PARAM_MAP_RC", 50 }, { "PARAM_REQUEST_LIST", 21 }, { "PARAM_REQUEST_READ", 20 }, { "PARAM_SET", 23 }, { "PARAM_VALUE", 22 }, { "PING", 4 }, { "PLAY_TUNE", 258 }, { "PLAY_TUNE_V2", 400 }, { "POSITION_TARGET_GLOBAL_INT", 87 }, { "POSITION_TARGET_LOCAL_NED", 85 }, { "POWER_STATUS", 125 }, { "PROTOCOL_VERSION", 300 }, { "RADIO_STATUS", 109 }, { "RAW_IMU", 27 }, { "RAW_PRESSURE", 28 }, { "RAW_RPM", 339 }, { "RC_CHANNELS", 65 }, { "RC_CHANNELS_OVERRIDE", 70 }, { "RC_CHANNELS_RAW", 35 }, { "RC_CHANNELS_SCALED", 34 }, { "REQUEST_DATA_STREAM", 66 }, { "RESOURCE_REQUEST", 142 }, { "SAFETY_ALLOWED_AREA", 55 }, { "SAFETY_SET_ALLOWED_AREA", 54 }, { "SCALED_IMU", 26 }, { "SCALED_IMU2", 116 }, { "SCALED_IMU3", 129 }, { "SCALED_PRESSURE", 29 }, { "SCALED_PRESSURE2", 137 }, { "SCALED_PRESSURE3", 143 }, { "SERIAL_CONTROL", 126 }, { "SERVO_OUTPUT_RAW", 36 }, { "SETUP_SIGNING", 256 }, { "SET_ACTUATOR_CONTROL_TARGET", 139 }, { "SET_ATTITUDE_TARGET", 82 }, { "SET_GPS_GLOBAL_ORIGIN", 48 }, { "SET_HOME_POSITION", 243 }, { "SET_MODE", 11 }, { "SET_POSITION_TARGET_GLOBAL_INT", 86 }, { "SET_POSITION_TARGET_LOCAL_NED", 84 }, { "SIM_STATE", 108 }, { "SMART_BATTERY_INFO", 370 }, { "STATUSTEXT", 253 }, { "STORAGE_INFORMATION", 261 }, { "SUPPORTED_TUNES", 401 }, { "SYSTEM_TIME", 2 }, { "SYS_STATUS", 1 }, { "TERRAIN_CHECK", 135 }, { "TERRAIN_DATA", 134 }, { "TERRAIN_REPORT", 136 }, { "TERRAIN_REQUEST", 133 }, { "TIMESYNC", 111 }, { "TIME_ESTIMATE_TO_TARGET", 380 }, { "TRAJECTORY_REPRESENTATION_BEZIER", 333 }, { "TRAJECTORY_REPRESENTATION_WAYPOINTS", 332 }, { "TUNNEL", 385 }, { "UAVCAN_NODE_INFO", 311 }, { "UAVCAN_NODE_STATUS", 310 }, { "UTM_GLOBAL_POSITION", 340 }, { "V2_EXTENSION", 248 }, { "VFR_HUD", 74 }, { "VIBRATION", 241 }, { "VICON_POSITION_ESTIMATE", 104 }, { "VIDEO_STREAM_INFORMATION", 269 }, { "VIDEO_STREAM_STATUS", 270 }, { "VISION_POSITION_ESTIMATE", 102 }, { "VISION_SPEED_ESTIMATE", 103 }, { "WHEEL_DISTANCE", 9000 }, { "WIFI_CONFIG_AP", 299 }, { "WINCH_STATUS", 9005 }, { "WIND_COV", 231 }} +#if MAVLINK_COMMON_XML_HASH == MAVLINK_PRIMARY_XML_HASH +# define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, MAVLINK_MESSAGE_INFO_LINK_NODE_STATUS, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_PARAM_MAP_RC, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION_COV, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT_COV, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_COV, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, MAVLINK_MESSAGE_INFO_MISSION_ITEM_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_INT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, MAVLINK_MESSAGE_INFO_COMMAND_CANCEL, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_SET_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_GLOBAL_INT, MAVLINK_MESSAGE_INFO_POSITION_TARGET_GLOBAL_INT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, MAVLINK_MESSAGE_INFO_HIL_ACTUATOR_CONTROLS, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW_RAD, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_PROTOCOL, MAVLINK_MESSAGE_INFO_TIMESYNC, MAVLINK_MESSAGE_INFO_CAMERA_TRIGGER, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, MAVLINK_MESSAGE_INFO_GPS_RTK, MAVLINK_MESSAGE_INFO_GPS2_RTK, MAVLINK_MESSAGE_INFO_SCALED_IMU3, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, MAVLINK_MESSAGE_INFO_TERRAIN_REQUEST, MAVLINK_MESSAGE_INFO_TERRAIN_DATA, MAVLINK_MESSAGE_INFO_TERRAIN_CHECK, MAVLINK_MESSAGE_INFO_TERRAIN_REPORT, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE2, MAVLINK_MESSAGE_INFO_ATT_POS_MOCAP, MAVLINK_MESSAGE_INFO_SET_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ALTITUDE, MAVLINK_MESSAGE_INFO_RESOURCE_REQUEST, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE3, MAVLINK_MESSAGE_INFO_FOLLOW_TARGET, MAVLINK_MESSAGE_INFO_CONTROL_SYSTEM_STATE, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION, MAVLINK_MESSAGE_INFO_LANDING_TARGET, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT, MAVLINK_MESSAGE_INFO_EFI_STATUS, MAVLINK_MESSAGE_INFO_ESTIMATOR_STATUS, MAVLINK_MESSAGE_INFO_WIND_COV, MAVLINK_MESSAGE_INFO_GPS_INPUT, MAVLINK_MESSAGE_INFO_GPS_RTCM_DATA, MAVLINK_MESSAGE_INFO_HIGH_LATENCY, MAVLINK_MESSAGE_INFO_HIGH_LATENCY2, MAVLINK_MESSAGE_INFO_VIBRATION, MAVLINK_MESSAGE_INFO_HOME_POSITION, MAVLINK_MESSAGE_INFO_SET_HOME_POSITION, MAVLINK_MESSAGE_INFO_MESSAGE_INTERVAL, MAVLINK_MESSAGE_INFO_EXTENDED_SYS_STATE, MAVLINK_MESSAGE_INFO_ADSB_VEHICLE, MAVLINK_MESSAGE_INFO_COLLISION, MAVLINK_MESSAGE_INFO_V2_EXTENSION, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, MAVLINK_MESSAGE_INFO_SETUP_SIGNING, MAVLINK_MESSAGE_INFO_BUTTON_CHANGE, MAVLINK_MESSAGE_INFO_PLAY_TUNE, MAVLINK_MESSAGE_INFO_CAMERA_INFORMATION, MAVLINK_MESSAGE_INFO_CAMERA_SETTINGS, MAVLINK_MESSAGE_INFO_STORAGE_INFORMATION, MAVLINK_MESSAGE_INFO_CAMERA_CAPTURE_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_IMAGE_CAPTURED, MAVLINK_MESSAGE_INFO_FLIGHT_INFORMATION, MAVLINK_MESSAGE_INFO_MOUNT_ORIENTATION, MAVLINK_MESSAGE_INFO_LOGGING_DATA, MAVLINK_MESSAGE_INFO_LOGGING_DATA_ACKED, MAVLINK_MESSAGE_INFO_LOGGING_ACK, MAVLINK_MESSAGE_INFO_VIDEO_STREAM_INFORMATION, MAVLINK_MESSAGE_INFO_VIDEO_STREAM_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_FOV_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_IMAGE_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_GEO_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_THERMAL_RANGE, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_INFORMATION, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_STATUS, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_ATTITUDE, MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_INFORMATION, MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_SET_ATTITUDE, MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_ATTITUDE_STATUS, MAVLINK_MESSAGE_INFO_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_PITCHYAW, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_ESC_INFO, MAVLINK_MESSAGE_INFO_ESC_STATUS, MAVLINK_MESSAGE_INFO_WIFI_CONFIG_AP, MAVLINK_MESSAGE_INFO_PROTOCOL_VERSION, MAVLINK_MESSAGE_INFO_AIS_VESSEL, MAVLINK_MESSAGE_INFO_UAVCAN_NODE_STATUS, MAVLINK_MESSAGE_INFO_UAVCAN_NODE_INFO, MAVLINK_MESSAGE_INFO_PARAM_EXT_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_EXT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_EXT_VALUE, MAVLINK_MESSAGE_INFO_PARAM_EXT_SET, MAVLINK_MESSAGE_INFO_PARAM_EXT_ACK, MAVLINK_MESSAGE_INFO_OBSTACLE_DISTANCE, MAVLINK_MESSAGE_INFO_ODOMETRY, MAVLINK_MESSAGE_INFO_TRAJECTORY_REPRESENTATION_WAYPOINTS, MAVLINK_MESSAGE_INFO_TRAJECTORY_REPRESENTATION_BEZIER, MAVLINK_MESSAGE_INFO_CELLULAR_STATUS, MAVLINK_MESSAGE_INFO_ISBD_LINK_STATUS, MAVLINK_MESSAGE_INFO_CELLULAR_CONFIG, MAVLINK_MESSAGE_INFO_RAW_RPM, MAVLINK_MESSAGE_INFO_UTM_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_DEBUG_FLOAT_ARRAY, MAVLINK_MESSAGE_INFO_ORBIT_EXECUTION_STATUS, MAVLINK_MESSAGE_INFO_SMART_BATTERY_INFO, MAVLINK_MESSAGE_INFO_FUEL_STATUS, MAVLINK_MESSAGE_INFO_BATTERY_INFO, MAVLINK_MESSAGE_INFO_GENERATOR_STATUS, MAVLINK_MESSAGE_INFO_ACTUATOR_OUTPUT_STATUS, MAVLINK_MESSAGE_INFO_TIME_ESTIMATE_TO_TARGET, MAVLINK_MESSAGE_INFO_TUNNEL, MAVLINK_MESSAGE_INFO_CAN_FRAME, MAVLINK_MESSAGE_INFO_CANFD_FRAME, MAVLINK_MESSAGE_INFO_CAN_FILTER_MODIFY, MAVLINK_MESSAGE_INFO_ONBOARD_COMPUTER_STATUS, MAVLINK_MESSAGE_INFO_COMPONENT_INFORMATION, MAVLINK_MESSAGE_INFO_COMPONENT_INFORMATION_BASIC, MAVLINK_MESSAGE_INFO_COMPONENT_METADATA, MAVLINK_MESSAGE_INFO_PLAY_TUNE_V2, MAVLINK_MESSAGE_INFO_SUPPORTED_TUNES, MAVLINK_MESSAGE_INFO_EVENT, MAVLINK_MESSAGE_INFO_CURRENT_EVENT_SEQUENCE, MAVLINK_MESSAGE_INFO_REQUEST_EVENT, MAVLINK_MESSAGE_INFO_RESPONSE_EVENT_ERROR, MAVLINK_MESSAGE_INFO_AVAILABLE_MODES, MAVLINK_MESSAGE_INFO_CURRENT_MODE, MAVLINK_MESSAGE_INFO_AVAILABLE_MODES_MONITOR, MAVLINK_MESSAGE_INFO_ILLUMINATOR_STATUS, MAVLINK_MESSAGE_INFO_WHEEL_DISTANCE, MAVLINK_MESSAGE_INFO_WINCH_STATUS, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_BASIC_ID, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_LOCATION, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_AUTHENTICATION, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SELF_ID, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SYSTEM, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_OPERATOR_ID, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_MESSAGE_PACK, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_ARM_STATUS, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SYSTEM_UPDATE, MAVLINK_MESSAGE_INFO_HYGROMETER_SENSOR} +# define MAVLINK_MESSAGE_NAMES {{ "ACTUATOR_CONTROL_TARGET", 140 }, { "ACTUATOR_OUTPUT_STATUS", 375 }, { "ADSB_VEHICLE", 246 }, { "AIS_VESSEL", 301 }, { "ALTITUDE", 141 }, { "ATTITUDE", 30 }, { "ATTITUDE_QUATERNION", 31 }, { "ATTITUDE_QUATERNION_COV", 61 }, { "ATTITUDE_TARGET", 83 }, { "ATT_POS_MOCAP", 138 }, { "AUTH_KEY", 7 }, { "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE", 286 }, { "AUTOPILOT_VERSION", 148 }, { "AVAILABLE_MODES", 435 }, { "AVAILABLE_MODES_MONITOR", 437 }, { "BATTERY_INFO", 372 }, { "BATTERY_STATUS", 147 }, { "BUTTON_CHANGE", 257 }, { "CAMERA_CAPTURE_STATUS", 262 }, { "CAMERA_FOV_STATUS", 271 }, { "CAMERA_IMAGE_CAPTURED", 263 }, { "CAMERA_INFORMATION", 259 }, { "CAMERA_SETTINGS", 260 }, { "CAMERA_THERMAL_RANGE", 277 }, { "CAMERA_TRACKING_GEO_STATUS", 276 }, { "CAMERA_TRACKING_IMAGE_STATUS", 275 }, { "CAMERA_TRIGGER", 112 }, { "CANFD_FRAME", 387 }, { "CAN_FILTER_MODIFY", 388 }, { "CAN_FRAME", 386 }, { "CELLULAR_CONFIG", 336 }, { "CELLULAR_STATUS", 334 }, { "CHANGE_OPERATOR_CONTROL", 5 }, { "CHANGE_OPERATOR_CONTROL_ACK", 6 }, { "COLLISION", 247 }, { "COMMAND_ACK", 77 }, { "COMMAND_CANCEL", 80 }, { "COMMAND_INT", 75 }, { "COMMAND_LONG", 76 }, { "COMPONENT_INFORMATION", 395 }, { "COMPONENT_INFORMATION_BASIC", 396 }, { "COMPONENT_METADATA", 397 }, { "CONTROL_SYSTEM_STATE", 146 }, { "CURRENT_EVENT_SEQUENCE", 411 }, { "CURRENT_MODE", 436 }, { "DATA_STREAM", 67 }, { "DATA_TRANSMISSION_HANDSHAKE", 130 }, { "DEBUG", 254 }, { "DEBUG_FLOAT_ARRAY", 350 }, { "DEBUG_VECT", 250 }, { "DISTANCE_SENSOR", 132 }, { "EFI_STATUS", 225 }, { "ENCAPSULATED_DATA", 131 }, { "ESC_INFO", 290 }, { "ESC_STATUS", 291 }, { "ESTIMATOR_STATUS", 230 }, { "EVENT", 410 }, { "EXTENDED_SYS_STATE", 245 }, { "FENCE_STATUS", 162 }, { "FILE_TRANSFER_PROTOCOL", 110 }, { "FLIGHT_INFORMATION", 264 }, { "FOLLOW_TARGET", 144 }, { "FUEL_STATUS", 371 }, { "GENERATOR_STATUS", 373 }, { "GIMBAL_DEVICE_ATTITUDE_STATUS", 285 }, { "GIMBAL_DEVICE_INFORMATION", 283 }, { "GIMBAL_DEVICE_SET_ATTITUDE", 284 }, { "GIMBAL_MANAGER_INFORMATION", 280 }, { "GIMBAL_MANAGER_SET_ATTITUDE", 282 }, { "GIMBAL_MANAGER_SET_MANUAL_CONTROL", 288 }, { "GIMBAL_MANAGER_SET_PITCHYAW", 287 }, { "GIMBAL_MANAGER_STATUS", 281 }, { "GLOBAL_POSITION_INT", 33 }, { "GLOBAL_POSITION_INT_COV", 63 }, { "GLOBAL_VISION_POSITION_ESTIMATE", 101 }, { "GPS2_RAW", 124 }, { "GPS2_RTK", 128 }, { "GPS_GLOBAL_ORIGIN", 49 }, { "GPS_INJECT_DATA", 123 }, { "GPS_INPUT", 232 }, { "GPS_RAW_INT", 24 }, { "GPS_RTCM_DATA", 233 }, { "GPS_RTK", 127 }, { "GPS_STATUS", 25 }, { "HEARTBEAT", 0 }, { "HIGHRES_IMU", 105 }, { "HIGH_LATENCY", 234 }, { "HIGH_LATENCY2", 235 }, { "HIL_ACTUATOR_CONTROLS", 93 }, { "HIL_CONTROLS", 91 }, { "HIL_GPS", 113 }, { "HIL_OPTICAL_FLOW", 114 }, { "HIL_RC_INPUTS_RAW", 92 }, { "HIL_SENSOR", 107 }, { "HIL_STATE", 90 }, { "HIL_STATE_QUATERNION", 115 }, { "HOME_POSITION", 242 }, { "HYGROMETER_SENSOR", 12920 }, { "ILLUMINATOR_STATUS", 440 }, { "ISBD_LINK_STATUS", 335 }, { "LANDING_TARGET", 149 }, { "LINK_NODE_STATUS", 8 }, { "LOCAL_POSITION_NED", 32 }, { "LOCAL_POSITION_NED_COV", 64 }, { "LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET", 89 }, { "LOGGING_ACK", 268 }, { "LOGGING_DATA", 266 }, { "LOGGING_DATA_ACKED", 267 }, { "LOG_DATA", 120 }, { "LOG_ENTRY", 118 }, { "LOG_ERASE", 121 }, { "LOG_REQUEST_DATA", 119 }, { "LOG_REQUEST_END", 122 }, { "LOG_REQUEST_LIST", 117 }, { "MAG_CAL_REPORT", 192 }, { "MANUAL_CONTROL", 69 }, { "MANUAL_SETPOINT", 81 }, { "MEMORY_VECT", 249 }, { "MESSAGE_INTERVAL", 244 }, { "MISSION_ACK", 47 }, { "MISSION_CLEAR_ALL", 45 }, { "MISSION_COUNT", 44 }, { "MISSION_CURRENT", 42 }, { "MISSION_ITEM", 39 }, { "MISSION_ITEM_INT", 73 }, { "MISSION_ITEM_REACHED", 46 }, { "MISSION_REQUEST", 40 }, { "MISSION_REQUEST_INT", 51 }, { "MISSION_REQUEST_LIST", 43 }, { "MISSION_REQUEST_PARTIAL_LIST", 37 }, { "MISSION_SET_CURRENT", 41 }, { "MISSION_WRITE_PARTIAL_LIST", 38 }, { "MOUNT_ORIENTATION", 265 }, { "NAMED_VALUE_FLOAT", 251 }, { "NAMED_VALUE_INT", 252 }, { "NAV_CONTROLLER_OUTPUT", 62 }, { "OBSTACLE_DISTANCE", 330 }, { "ODOMETRY", 331 }, { "ONBOARD_COMPUTER_STATUS", 390 }, { "OPEN_DRONE_ID_ARM_STATUS", 12918 }, { "OPEN_DRONE_ID_AUTHENTICATION", 12902 }, { "OPEN_DRONE_ID_BASIC_ID", 12900 }, { "OPEN_DRONE_ID_LOCATION", 12901 }, { "OPEN_DRONE_ID_MESSAGE_PACK", 12915 }, { "OPEN_DRONE_ID_OPERATOR_ID", 12905 }, { "OPEN_DRONE_ID_SELF_ID", 12903 }, { "OPEN_DRONE_ID_SYSTEM", 12904 }, { "OPEN_DRONE_ID_SYSTEM_UPDATE", 12919 }, { "OPTICAL_FLOW", 100 }, { "OPTICAL_FLOW_RAD", 106 }, { "ORBIT_EXECUTION_STATUS", 360 }, { "PARAM_EXT_ACK", 324 }, { "PARAM_EXT_REQUEST_LIST", 321 }, { "PARAM_EXT_REQUEST_READ", 320 }, { "PARAM_EXT_SET", 323 }, { "PARAM_EXT_VALUE", 322 }, { "PARAM_MAP_RC", 50 }, { "PARAM_REQUEST_LIST", 21 }, { "PARAM_REQUEST_READ", 20 }, { "PARAM_SET", 23 }, { "PARAM_VALUE", 22 }, { "PING", 4 }, { "PLAY_TUNE", 258 }, { "PLAY_TUNE_V2", 400 }, { "POSITION_TARGET_GLOBAL_INT", 87 }, { "POSITION_TARGET_LOCAL_NED", 85 }, { "POWER_STATUS", 125 }, { "PROTOCOL_VERSION", 300 }, { "RADIO_STATUS", 109 }, { "RAW_IMU", 27 }, { "RAW_PRESSURE", 28 }, { "RAW_RPM", 339 }, { "RC_CHANNELS", 65 }, { "RC_CHANNELS_OVERRIDE", 70 }, { "RC_CHANNELS_RAW", 35 }, { "RC_CHANNELS_SCALED", 34 }, { "REQUEST_DATA_STREAM", 66 }, { "REQUEST_EVENT", 412 }, { "RESOURCE_REQUEST", 142 }, { "RESPONSE_EVENT_ERROR", 413 }, { "SAFETY_ALLOWED_AREA", 55 }, { "SAFETY_SET_ALLOWED_AREA", 54 }, { "SCALED_IMU", 26 }, { "SCALED_IMU2", 116 }, { "SCALED_IMU3", 129 }, { "SCALED_PRESSURE", 29 }, { "SCALED_PRESSURE2", 137 }, { "SCALED_PRESSURE3", 143 }, { "SERIAL_CONTROL", 126 }, { "SERVO_OUTPUT_RAW", 36 }, { "SETUP_SIGNING", 256 }, { "SET_ACTUATOR_CONTROL_TARGET", 139 }, { "SET_ATTITUDE_TARGET", 82 }, { "SET_GPS_GLOBAL_ORIGIN", 48 }, { "SET_HOME_POSITION", 243 }, { "SET_MODE", 11 }, { "SET_POSITION_TARGET_GLOBAL_INT", 86 }, { "SET_POSITION_TARGET_LOCAL_NED", 84 }, { "SIM_STATE", 108 }, { "SMART_BATTERY_INFO", 370 }, { "STATUSTEXT", 253 }, { "STORAGE_INFORMATION", 261 }, { "SUPPORTED_TUNES", 401 }, { "SYSTEM_TIME", 2 }, { "SYS_STATUS", 1 }, { "TERRAIN_CHECK", 135 }, { "TERRAIN_DATA", 134 }, { "TERRAIN_REPORT", 136 }, { "TERRAIN_REQUEST", 133 }, { "TIMESYNC", 111 }, { "TIME_ESTIMATE_TO_TARGET", 380 }, { "TRAJECTORY_REPRESENTATION_BEZIER", 333 }, { "TRAJECTORY_REPRESENTATION_WAYPOINTS", 332 }, { "TUNNEL", 385 }, { "UAVCAN_NODE_INFO", 311 }, { "UAVCAN_NODE_STATUS", 310 }, { "UTM_GLOBAL_POSITION", 340 }, { "V2_EXTENSION", 248 }, { "VFR_HUD", 74 }, { "VIBRATION", 241 }, { "VICON_POSITION_ESTIMATE", 104 }, { "VIDEO_STREAM_INFORMATION", 269 }, { "VIDEO_STREAM_STATUS", 270 }, { "VISION_POSITION_ESTIMATE", 102 }, { "VISION_SPEED_ESTIMATE", 103 }, { "WHEEL_DISTANCE", 9000 }, { "WIFI_CONFIG_AP", 299 }, { "WINCH_STATUS", 9005 }, { "WIND_COV", 231 }} # if MAVLINK_COMMAND_24BIT # include "../mavlink_get_info.h" # endif diff --git a/lib/main/MAVLink/common/mavlink.h b/lib/main/MAVLink/common/mavlink.h index 39d85d2d5f8..c4350341463 100755 --- a/lib/main/MAVLink/common/mavlink.h +++ b/lib/main/MAVLink/common/mavlink.h @@ -6,7 +6,7 @@ #ifndef MAVLINK_H #define MAVLINK_H -#define MAVLINK_PRIMARY_XML_IDX 0 +#define MAVLINK_PRIMARY_XML_HASH 1337309235272790416 #ifndef MAVLINK_STX #define MAVLINK_STX 253 diff --git a/lib/main/MAVLink/common/mavlink_msg_actuator_control_target.h b/lib/main/MAVLink/common/mavlink_msg_actuator_control_target.h index 31e307cb5a4..c03a6a7c76c 100755 --- a/lib/main/MAVLink/common/mavlink_msg_actuator_control_target.h +++ b/lib/main/MAVLink/common/mavlink_msg_actuator_control_target.h @@ -65,7 +65,7 @@ static inline uint16_t mavlink_msg_actuator_control_target_pack(uint8_t system_i mavlink_actuator_control_target_t packet; packet.time_usec = time_usec; packet.group_mlx = group_mlx; - mav_array_memcpy(packet.controls, controls, sizeof(float)*8); + mav_array_assign_float(packet.controls, controls, 8); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN); #endif @@ -73,6 +73,43 @@ static inline uint16_t mavlink_msg_actuator_control_target_pack(uint8_t system_i return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_CRC); } +/** + * @brief Pack a actuator_control_target message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param group_mlx Actuator group. The "_mlx" indicates this is a multi-instance message and a MAVLink parser should use this field to difference between instances. + * @param controls Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for single rotation direction motors is 0..1, negative range for reverse direction. Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through mixer to repurpose them as generic outputs. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_actuator_control_target_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint8_t group_mlx, const float *controls) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 40, group_mlx); + _mav_put_float_array(buf, 8, controls, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN); +#else + mavlink_actuator_control_target_t packet; + packet.time_usec = time_usec; + packet.group_mlx = group_mlx; + mav_array_memcpy(packet.controls, controls, sizeof(float)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN); +#endif +} + /** * @brief Pack a actuator_control_target message on a channel * @param system_id ID of this system @@ -98,7 +135,7 @@ static inline uint16_t mavlink_msg_actuator_control_target_pack_chan(uint8_t sys mavlink_actuator_control_target_t packet; packet.time_usec = time_usec; packet.group_mlx = group_mlx; - mav_array_memcpy(packet.controls, controls, sizeof(float)*8); + mav_array_assign_float(packet.controls, controls, 8); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN); #endif @@ -133,6 +170,20 @@ static inline uint16_t mavlink_msg_actuator_control_target_encode_chan(uint8_t s return mavlink_msg_actuator_control_target_pack_chan(system_id, component_id, chan, msg, actuator_control_target->time_usec, actuator_control_target->group_mlx, actuator_control_target->controls); } +/** + * @brief Encode a actuator_control_target struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param actuator_control_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_actuator_control_target_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_actuator_control_target_t* actuator_control_target) +{ + return mavlink_msg_actuator_control_target_pack_status(system_id, component_id, _status, msg, actuator_control_target->time_usec, actuator_control_target->group_mlx, actuator_control_target->controls); +} + /** * @brief Send a actuator_control_target message * @param chan MAVLink channel to send the message @@ -155,7 +206,7 @@ static inline void mavlink_msg_actuator_control_target_send(mavlink_channel_t ch mavlink_actuator_control_target_t packet; packet.time_usec = time_usec; packet.group_mlx = group_mlx; - mav_array_memcpy(packet.controls, controls, sizeof(float)*8); + mav_array_assign_float(packet.controls, controls, 8); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET, (const char *)&packet, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_CRC); #endif } @@ -176,7 +227,7 @@ static inline void mavlink_msg_actuator_control_target_send_struct(mavlink_chann #if MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -194,7 +245,7 @@ static inline void mavlink_msg_actuator_control_target_send_buf(mavlink_message_ mavlink_actuator_control_target_t *packet = (mavlink_actuator_control_target_t *)msgbuf; packet->time_usec = time_usec; packet->group_mlx = group_mlx; - mav_array_memcpy(packet->controls, controls, sizeof(float)*8); + mav_array_assign_float(packet->controls, controls, 8); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET, (const char *)packet, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_actuator_output_status.h b/lib/main/MAVLink/common/mavlink_msg_actuator_output_status.h index 52ee04df757..413235dc5d9 100644 --- a/lib/main/MAVLink/common/mavlink_msg_actuator_output_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_actuator_output_status.h @@ -65,7 +65,7 @@ static inline uint16_t mavlink_msg_actuator_output_status_pack(uint8_t system_id mavlink_actuator_output_status_t packet; packet.time_usec = time_usec; packet.active = active; - mav_array_memcpy(packet.actuator, actuator, sizeof(float)*32); + mav_array_assign_float(packet.actuator, actuator, 32); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN); #endif @@ -73,6 +73,43 @@ static inline uint16_t mavlink_msg_actuator_output_status_pack(uint8_t system_id return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC); } +/** + * @brief Pack a actuator_output_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (since system boot). + * @param active Active outputs + * @param actuator Servo / motor output array values. Zero values indicate unused channels. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_actuator_output_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint32_t active, const float *actuator) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, active); + _mav_put_float_array(buf, 12, actuator, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN); +#else + mavlink_actuator_output_status_t packet; + packet.time_usec = time_usec; + packet.active = active; + mav_array_memcpy(packet.actuator, actuator, sizeof(float)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN); +#endif +} + /** * @brief Pack a actuator_output_status message on a channel * @param system_id ID of this system @@ -98,7 +135,7 @@ static inline uint16_t mavlink_msg_actuator_output_status_pack_chan(uint8_t syst mavlink_actuator_output_status_t packet; packet.time_usec = time_usec; packet.active = active; - mav_array_memcpy(packet.actuator, actuator, sizeof(float)*32); + mav_array_assign_float(packet.actuator, actuator, 32); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN); #endif @@ -133,6 +170,20 @@ static inline uint16_t mavlink_msg_actuator_output_status_encode_chan(uint8_t sy return mavlink_msg_actuator_output_status_pack_chan(system_id, component_id, chan, msg, actuator_output_status->time_usec, actuator_output_status->active, actuator_output_status->actuator); } +/** + * @brief Encode a actuator_output_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param actuator_output_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_actuator_output_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_actuator_output_status_t* actuator_output_status) +{ + return mavlink_msg_actuator_output_status_pack_status(system_id, component_id, _status, msg, actuator_output_status->time_usec, actuator_output_status->active, actuator_output_status->actuator); +} + /** * @brief Send a actuator_output_status message * @param chan MAVLink channel to send the message @@ -155,7 +206,7 @@ static inline void mavlink_msg_actuator_output_status_send(mavlink_channel_t cha mavlink_actuator_output_status_t packet; packet.time_usec = time_usec; packet.active = active; - mav_array_memcpy(packet.actuator, actuator, sizeof(float)*32); + mav_array_assign_float(packet.actuator, actuator, 32); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC); #endif } @@ -176,7 +227,7 @@ static inline void mavlink_msg_actuator_output_status_send_struct(mavlink_channe #if MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -194,7 +245,7 @@ static inline void mavlink_msg_actuator_output_status_send_buf(mavlink_message_t mavlink_actuator_output_status_t *packet = (mavlink_actuator_output_status_t *)msgbuf; packet->time_usec = time_usec; packet->active = active; - mav_array_memcpy(packet->actuator, actuator, sizeof(float)*32); + mav_array_assign_float(packet->actuator, actuator, 32); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS, (const char *)packet, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_adsb_vehicle.h b/lib/main/MAVLink/common/mavlink_msg_adsb_vehicle.h index 7296c22aa5a..fcdb9257ed6 100755 --- a/lib/main/MAVLink/common/mavlink_msg_adsb_vehicle.h +++ b/lib/main/MAVLink/common/mavlink_msg_adsb_vehicle.h @@ -13,7 +13,7 @@ typedef struct __mavlink_adsb_vehicle_t { uint16_t hor_velocity; /*< [cm/s] The horizontal velocity*/ int16_t ver_velocity; /*< [cm/s] The vertical velocity. Positive is up*/ uint16_t flags; /*< Bitmap to indicate various statuses including valid data fields*/ - uint16_t squawk; /*< Squawk code*/ + uint16_t squawk; /*< Squawk code. Note that the code is in decimal: e.g. 7700 (general emergency) is encoded as binary 0b0001_1110_0001_0100, not(!) as 0b0000_111_111_000_000*/ uint8_t altitude_type; /*< ADSB altitude type.*/ char callsign[9]; /*< The callsign, 8+null*/ uint8_t emitter_type; /*< ADSB emitter type.*/ @@ -89,7 +89,7 @@ typedef struct __mavlink_adsb_vehicle_t { * @param emitter_type ADSB emitter type. * @param tslc [s] Time since last communication in seconds * @param flags Bitmap to indicate various statuses including valid data fields - * @param squawk Squawk code + * @param squawk Squawk code. Note that the code is in decimal: e.g. 7700 (general emergency) is encoded as binary 0b0001_1110_0001_0100, not(!) as 0b0000_111_111_000_000 * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_adsb_vehicle_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -125,7 +125,7 @@ static inline uint16_t mavlink_msg_adsb_vehicle_pack(uint8_t system_id, uint8_t packet.altitude_type = altitude_type; packet.emitter_type = emitter_type; packet.tslc = tslc; - mav_array_memcpy(packet.callsign, callsign, sizeof(char)*9); + mav_array_assign_char(packet.callsign, callsign, 9); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ADSB_VEHICLE_LEN); #endif @@ -133,6 +133,73 @@ static inline uint16_t mavlink_msg_adsb_vehicle_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ADSB_VEHICLE_MIN_LEN, MAVLINK_MSG_ID_ADSB_VEHICLE_LEN, MAVLINK_MSG_ID_ADSB_VEHICLE_CRC); } +/** + * @brief Pack a adsb_vehicle message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param ICAO_address ICAO address + * @param lat [degE7] Latitude + * @param lon [degE7] Longitude + * @param altitude_type ADSB altitude type. + * @param altitude [mm] Altitude(ASL) + * @param heading [cdeg] Course over ground + * @param hor_velocity [cm/s] The horizontal velocity + * @param ver_velocity [cm/s] The vertical velocity. Positive is up + * @param callsign The callsign, 8+null + * @param emitter_type ADSB emitter type. + * @param tslc [s] Time since last communication in seconds + * @param flags Bitmap to indicate various statuses including valid data fields + * @param squawk Squawk code. Note that the code is in decimal: e.g. 7700 (general emergency) is encoded as binary 0b0001_1110_0001_0100, not(!) as 0b0000_111_111_000_000 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_adsb_vehicle_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t ICAO_address, int32_t lat, int32_t lon, uint8_t altitude_type, int32_t altitude, uint16_t heading, uint16_t hor_velocity, int16_t ver_velocity, const char *callsign, uint8_t emitter_type, uint8_t tslc, uint16_t flags, uint16_t squawk) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ADSB_VEHICLE_LEN]; + _mav_put_uint32_t(buf, 0, ICAO_address); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_int32_t(buf, 12, altitude); + _mav_put_uint16_t(buf, 16, heading); + _mav_put_uint16_t(buf, 18, hor_velocity); + _mav_put_int16_t(buf, 20, ver_velocity); + _mav_put_uint16_t(buf, 22, flags); + _mav_put_uint16_t(buf, 24, squawk); + _mav_put_uint8_t(buf, 26, altitude_type); + _mav_put_uint8_t(buf, 36, emitter_type); + _mav_put_uint8_t(buf, 37, tslc); + _mav_put_char_array(buf, 27, callsign, 9); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ADSB_VEHICLE_LEN); +#else + mavlink_adsb_vehicle_t packet; + packet.ICAO_address = ICAO_address; + packet.lat = lat; + packet.lon = lon; + packet.altitude = altitude; + packet.heading = heading; + packet.hor_velocity = hor_velocity; + packet.ver_velocity = ver_velocity; + packet.flags = flags; + packet.squawk = squawk; + packet.altitude_type = altitude_type; + packet.emitter_type = emitter_type; + packet.tslc = tslc; + mav_array_memcpy(packet.callsign, callsign, sizeof(char)*9); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ADSB_VEHICLE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ADSB_VEHICLE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ADSB_VEHICLE_MIN_LEN, MAVLINK_MSG_ID_ADSB_VEHICLE_LEN, MAVLINK_MSG_ID_ADSB_VEHICLE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ADSB_VEHICLE_MIN_LEN, MAVLINK_MSG_ID_ADSB_VEHICLE_LEN); +#endif +} + /** * @brief Pack a adsb_vehicle message on a channel * @param system_id ID of this system @@ -151,7 +218,7 @@ static inline uint16_t mavlink_msg_adsb_vehicle_pack(uint8_t system_id, uint8_t * @param emitter_type ADSB emitter type. * @param tslc [s] Time since last communication in seconds * @param flags Bitmap to indicate various statuses including valid data fields - * @param squawk Squawk code + * @param squawk Squawk code. Note that the code is in decimal: e.g. 7700 (general emergency) is encoded as binary 0b0001_1110_0001_0100, not(!) as 0b0000_111_111_000_000 * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_adsb_vehicle_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -188,7 +255,7 @@ static inline uint16_t mavlink_msg_adsb_vehicle_pack_chan(uint8_t system_id, uin packet.altitude_type = altitude_type; packet.emitter_type = emitter_type; packet.tslc = tslc; - mav_array_memcpy(packet.callsign, callsign, sizeof(char)*9); + mav_array_assign_char(packet.callsign, callsign, 9); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ADSB_VEHICLE_LEN); #endif @@ -223,6 +290,20 @@ static inline uint16_t mavlink_msg_adsb_vehicle_encode_chan(uint8_t system_id, u return mavlink_msg_adsb_vehicle_pack_chan(system_id, component_id, chan, msg, adsb_vehicle->ICAO_address, adsb_vehicle->lat, adsb_vehicle->lon, adsb_vehicle->altitude_type, adsb_vehicle->altitude, adsb_vehicle->heading, adsb_vehicle->hor_velocity, adsb_vehicle->ver_velocity, adsb_vehicle->callsign, adsb_vehicle->emitter_type, adsb_vehicle->tslc, adsb_vehicle->flags, adsb_vehicle->squawk); } +/** + * @brief Encode a adsb_vehicle struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param adsb_vehicle C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_adsb_vehicle_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_adsb_vehicle_t* adsb_vehicle) +{ + return mavlink_msg_adsb_vehicle_pack_status(system_id, component_id, _status, msg, adsb_vehicle->ICAO_address, adsb_vehicle->lat, adsb_vehicle->lon, adsb_vehicle->altitude_type, adsb_vehicle->altitude, adsb_vehicle->heading, adsb_vehicle->hor_velocity, adsb_vehicle->ver_velocity, adsb_vehicle->callsign, adsb_vehicle->emitter_type, adsb_vehicle->tslc, adsb_vehicle->flags, adsb_vehicle->squawk); +} + /** * @brief Send a adsb_vehicle message * @param chan MAVLink channel to send the message @@ -239,7 +320,7 @@ static inline uint16_t mavlink_msg_adsb_vehicle_encode_chan(uint8_t system_id, u * @param emitter_type ADSB emitter type. * @param tslc [s] Time since last communication in seconds * @param flags Bitmap to indicate various statuses including valid data fields - * @param squawk Squawk code + * @param squawk Squawk code. Note that the code is in decimal: e.g. 7700 (general emergency) is encoded as binary 0b0001_1110_0001_0100, not(!) as 0b0000_111_111_000_000 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -275,7 +356,7 @@ static inline void mavlink_msg_adsb_vehicle_send(mavlink_channel_t chan, uint32_ packet.altitude_type = altitude_type; packet.emitter_type = emitter_type; packet.tslc = tslc; - mav_array_memcpy(packet.callsign, callsign, sizeof(char)*9); + mav_array_assign_char(packet.callsign, callsign, 9); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ADSB_VEHICLE, (const char *)&packet, MAVLINK_MSG_ID_ADSB_VEHICLE_MIN_LEN, MAVLINK_MSG_ID_ADSB_VEHICLE_LEN, MAVLINK_MSG_ID_ADSB_VEHICLE_CRC); #endif } @@ -296,7 +377,7 @@ static inline void mavlink_msg_adsb_vehicle_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_ADSB_VEHICLE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -334,7 +415,7 @@ static inline void mavlink_msg_adsb_vehicle_send_buf(mavlink_message_t *msgbuf, packet->altitude_type = altitude_type; packet->emitter_type = emitter_type; packet->tslc = tslc; - mav_array_memcpy(packet->callsign, callsign, sizeof(char)*9); + mav_array_assign_char(packet->callsign, callsign, 9); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ADSB_VEHICLE, (const char *)packet, MAVLINK_MSG_ID_ADSB_VEHICLE_MIN_LEN, MAVLINK_MSG_ID_ADSB_VEHICLE_LEN, MAVLINK_MSG_ID_ADSB_VEHICLE_CRC); #endif } @@ -468,7 +549,7 @@ static inline uint16_t mavlink_msg_adsb_vehicle_get_flags(const mavlink_message_ /** * @brief Get field squawk from adsb_vehicle message * - * @return Squawk code + * @return Squawk code. Note that the code is in decimal: e.g. 7700 (general emergency) is encoded as binary 0b0001_1110_0001_0100, not(!) as 0b0000_111_111_000_000 */ static inline uint16_t mavlink_msg_adsb_vehicle_get_squawk(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_ais_vessel.h b/lib/main/MAVLink/common/mavlink_msg_ais_vessel.h index e97fe42bcdd..1fd7e6091a7 100644 --- a/lib/main/MAVLink/common/mavlink_msg_ais_vessel.h +++ b/lib/main/MAVLink/common/mavlink_msg_ais_vessel.h @@ -15,7 +15,7 @@ typedef struct __mavlink_ais_vessel_t { uint16_t dimension_stern; /*< [m] Distance from lat/lon location to stern*/ uint16_t tslc; /*< [s] Time since last communication in seconds*/ uint16_t flags; /*< Bitmask to indicate various statuses including valid data fields*/ - int8_t turn_rate; /*< [cdeg/s] Turn rate*/ + int8_t turn_rate; /*< [ddeg/s] Turn rate, 0.1 degrees per second*/ uint8_t navigational_status; /*< Navigational status*/ uint8_t type; /*< Type of vessels*/ uint8_t dimension_port; /*< [m] Distance from lat/lon location to port side*/ @@ -96,7 +96,7 @@ typedef struct __mavlink_ais_vessel_t { * @param COG [cdeg] Course over ground * @param heading [cdeg] True heading * @param velocity [cm/s] Speed over ground - * @param turn_rate [cdeg/s] Turn rate + * @param turn_rate [ddeg/s] Turn rate, 0.1 degrees per second * @param navigational_status Navigational status * @param type Type of vessels * @param dimension_bow [m] Distance from lat/lon location to bow @@ -112,6 +112,81 @@ typedef struct __mavlink_ais_vessel_t { static inline uint16_t mavlink_msg_ais_vessel_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t MMSI, int32_t lat, int32_t lon, uint16_t COG, uint16_t heading, uint16_t velocity, int8_t turn_rate, uint8_t navigational_status, uint8_t type, uint16_t dimension_bow, uint16_t dimension_stern, uint8_t dimension_port, uint8_t dimension_starboard, const char *callsign, const char *name, uint16_t tslc, uint16_t flags) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIS_VESSEL_LEN]; + _mav_put_uint32_t(buf, 0, MMSI); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_uint16_t(buf, 12, COG); + _mav_put_uint16_t(buf, 14, heading); + _mav_put_uint16_t(buf, 16, velocity); + _mav_put_uint16_t(buf, 18, dimension_bow); + _mav_put_uint16_t(buf, 20, dimension_stern); + _mav_put_uint16_t(buf, 22, tslc); + _mav_put_uint16_t(buf, 24, flags); + _mav_put_int8_t(buf, 26, turn_rate); + _mav_put_uint8_t(buf, 27, navigational_status); + _mav_put_uint8_t(buf, 28, type); + _mav_put_uint8_t(buf, 29, dimension_port); + _mav_put_uint8_t(buf, 30, dimension_starboard); + _mav_put_char_array(buf, 31, callsign, 7); + _mav_put_char_array(buf, 38, name, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIS_VESSEL_LEN); +#else + mavlink_ais_vessel_t packet; + packet.MMSI = MMSI; + packet.lat = lat; + packet.lon = lon; + packet.COG = COG; + packet.heading = heading; + packet.velocity = velocity; + packet.dimension_bow = dimension_bow; + packet.dimension_stern = dimension_stern; + packet.tslc = tslc; + packet.flags = flags; + packet.turn_rate = turn_rate; + packet.navigational_status = navigational_status; + packet.type = type; + packet.dimension_port = dimension_port; + packet.dimension_starboard = dimension_starboard; + mav_array_assign_char(packet.callsign, callsign, 7); + mav_array_assign_char(packet.name, name, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIS_VESSEL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AIS_VESSEL; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); +} + +/** + * @brief Pack a ais_vessel message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param MMSI Mobile Marine Service Identifier, 9 decimal digits + * @param lat [degE7] Latitude + * @param lon [degE7] Longitude + * @param COG [cdeg] Course over ground + * @param heading [cdeg] True heading + * @param velocity [cm/s] Speed over ground + * @param turn_rate [ddeg/s] Turn rate, 0.1 degrees per second + * @param navigational_status Navigational status + * @param type Type of vessels + * @param dimension_bow [m] Distance from lat/lon location to bow + * @param dimension_stern [m] Distance from lat/lon location to stern + * @param dimension_port [m] Distance from lat/lon location to port side + * @param dimension_starboard [m] Distance from lat/lon location to starboard side + * @param callsign The vessel callsign + * @param name The vessel name + * @param tslc [s] Time since last communication in seconds + * @param flags Bitmask to indicate various statuses including valid data fields + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ais_vessel_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t MMSI, int32_t lat, int32_t lon, uint16_t COG, uint16_t heading, uint16_t velocity, int8_t turn_rate, uint8_t navigational_status, uint8_t type, uint16_t dimension_bow, uint16_t dimension_stern, uint8_t dimension_port, uint8_t dimension_starboard, const char *callsign, const char *name, uint16_t tslc, uint16_t flags) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_AIS_VESSEL_LEN]; _mav_put_uint32_t(buf, 0, MMSI); @@ -155,7 +230,11 @@ static inline uint16_t mavlink_msg_ais_vessel_pack(uint8_t system_id, uint8_t co #endif msg->msgid = MAVLINK_MSG_ID_AIS_VESSEL; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN); +#endif } /** @@ -170,7 +249,7 @@ static inline uint16_t mavlink_msg_ais_vessel_pack(uint8_t system_id, uint8_t co * @param COG [cdeg] Course over ground * @param heading [cdeg] True heading * @param velocity [cm/s] Speed over ground - * @param turn_rate [cdeg/s] Turn rate + * @param turn_rate [ddeg/s] Turn rate, 0.1 degrees per second * @param navigational_status Navigational status * @param type Type of vessels * @param dimension_bow [m] Distance from lat/lon location to bow @@ -224,8 +303,8 @@ static inline uint16_t mavlink_msg_ais_vessel_pack_chan(uint8_t system_id, uint8 packet.type = type; packet.dimension_port = dimension_port; packet.dimension_starboard = dimension_starboard; - mav_array_memcpy(packet.callsign, callsign, sizeof(char)*7); - mav_array_memcpy(packet.name, name, sizeof(char)*20); + mav_array_assign_char(packet.callsign, callsign, 7); + mav_array_assign_char(packet.name, name, 20); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIS_VESSEL_LEN); #endif @@ -260,6 +339,20 @@ static inline uint16_t mavlink_msg_ais_vessel_encode_chan(uint8_t system_id, uin return mavlink_msg_ais_vessel_pack_chan(system_id, component_id, chan, msg, ais_vessel->MMSI, ais_vessel->lat, ais_vessel->lon, ais_vessel->COG, ais_vessel->heading, ais_vessel->velocity, ais_vessel->turn_rate, ais_vessel->navigational_status, ais_vessel->type, ais_vessel->dimension_bow, ais_vessel->dimension_stern, ais_vessel->dimension_port, ais_vessel->dimension_starboard, ais_vessel->callsign, ais_vessel->name, ais_vessel->tslc, ais_vessel->flags); } +/** + * @brief Encode a ais_vessel struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param ais_vessel C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ais_vessel_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_ais_vessel_t* ais_vessel) +{ + return mavlink_msg_ais_vessel_pack_status(system_id, component_id, _status, msg, ais_vessel->MMSI, ais_vessel->lat, ais_vessel->lon, ais_vessel->COG, ais_vessel->heading, ais_vessel->velocity, ais_vessel->turn_rate, ais_vessel->navigational_status, ais_vessel->type, ais_vessel->dimension_bow, ais_vessel->dimension_stern, ais_vessel->dimension_port, ais_vessel->dimension_starboard, ais_vessel->callsign, ais_vessel->name, ais_vessel->tslc, ais_vessel->flags); +} + /** * @brief Send a ais_vessel message * @param chan MAVLink channel to send the message @@ -270,7 +363,7 @@ static inline uint16_t mavlink_msg_ais_vessel_encode_chan(uint8_t system_id, uin * @param COG [cdeg] Course over ground * @param heading [cdeg] True heading * @param velocity [cm/s] Speed over ground - * @param turn_rate [cdeg/s] Turn rate + * @param turn_rate [ddeg/s] Turn rate, 0.1 degrees per second * @param navigational_status Navigational status * @param type Type of vessels * @param dimension_bow [m] Distance from lat/lon location to bow @@ -323,8 +416,8 @@ static inline void mavlink_msg_ais_vessel_send(mavlink_channel_t chan, uint32_t packet.type = type; packet.dimension_port = dimension_port; packet.dimension_starboard = dimension_starboard; - mav_array_memcpy(packet.callsign, callsign, sizeof(char)*7); - mav_array_memcpy(packet.name, name, sizeof(char)*20); + mav_array_assign_char(packet.callsign, callsign, 7); + mav_array_assign_char(packet.name, name, 20); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIS_VESSEL, (const char *)&packet, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); #endif } @@ -345,7 +438,7 @@ static inline void mavlink_msg_ais_vessel_send_struct(mavlink_channel_t chan, co #if MAVLINK_MSG_ID_AIS_VESSEL_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -390,8 +483,8 @@ static inline void mavlink_msg_ais_vessel_send_buf(mavlink_message_t *msgbuf, ma packet->type = type; packet->dimension_port = dimension_port; packet->dimension_starboard = dimension_starboard; - mav_array_memcpy(packet->callsign, callsign, sizeof(char)*7); - mav_array_memcpy(packet->name, name, sizeof(char)*20); + mav_array_assign_char(packet->callsign, callsign, 7); + mav_array_assign_char(packet->name, name, 20); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIS_VESSEL, (const char *)packet, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); #endif } @@ -465,7 +558,7 @@ static inline uint16_t mavlink_msg_ais_vessel_get_velocity(const mavlink_message /** * @brief Get field turn_rate from ais_vessel message * - * @return [cdeg/s] Turn rate + * @return [ddeg/s] Turn rate, 0.1 degrees per second */ static inline int8_t mavlink_msg_ais_vessel_get_turn_rate(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_altitude.h b/lib/main/MAVLink/common/mavlink_msg_altitude.h index 1424f0ab9dd..8e44e6c7a1f 100755 --- a/lib/main/MAVLink/common/mavlink_msg_altitude.h +++ b/lib/main/MAVLink/common/mavlink_msg_altitude.h @@ -99,6 +99,57 @@ static inline uint16_t mavlink_msg_altitude_pack(uint8_t system_id, uint8_t comp return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ALTITUDE_MIN_LEN, MAVLINK_MSG_ID_ALTITUDE_LEN, MAVLINK_MSG_ID_ALTITUDE_CRC); } +/** + * @brief Pack a altitude message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param altitude_monotonic [m] This altitude measure is initialized on system boot and monotonic (it is never reset, but represents the local altitude change). The only guarantee on this field is that it will never be reset and is consistent within a flight. The recommended value for this field is the uncorrected barometric altitude at boot time. This altitude will also drift and vary between flights. + * @param altitude_amsl [m] This altitude measure is strictly above mean sea level and might be non-monotonic (it might reset on events like GPS lock or when a new QNH value is set). It should be the altitude to which global altitude waypoints are compared to. Note that it is *not* the GPS altitude, however, most GPS modules already output MSL by default and not the WGS84 altitude. + * @param altitude_local [m] This is the local altitude in the local coordinate frame. It is not the altitude above home, but in reference to the coordinate origin (0, 0, 0). It is up-positive. + * @param altitude_relative [m] This is the altitude above the home position. It resets on each change of the current home position. + * @param altitude_terrain [m] This is the altitude above terrain. It might be fed by a terrain database or an altimeter. Values smaller than -1000 should be interpreted as unknown. + * @param bottom_clearance [m] This is not the altitude, but the clear space below the system according to the fused clearance estimate. It generally should max out at the maximum range of e.g. the laser altimeter. It is generally a moving target. A negative value indicates no measurement available. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_altitude_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, float altitude_monotonic, float altitude_amsl, float altitude_local, float altitude_relative, float altitude_terrain, float bottom_clearance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ALTITUDE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, altitude_monotonic); + _mav_put_float(buf, 12, altitude_amsl); + _mav_put_float(buf, 16, altitude_local); + _mav_put_float(buf, 20, altitude_relative); + _mav_put_float(buf, 24, altitude_terrain); + _mav_put_float(buf, 28, bottom_clearance); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ALTITUDE_LEN); +#else + mavlink_altitude_t packet; + packet.time_usec = time_usec; + packet.altitude_monotonic = altitude_monotonic; + packet.altitude_amsl = altitude_amsl; + packet.altitude_local = altitude_local; + packet.altitude_relative = altitude_relative; + packet.altitude_terrain = altitude_terrain; + packet.bottom_clearance = bottom_clearance; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ALTITUDE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ALTITUDE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ALTITUDE_MIN_LEN, MAVLINK_MSG_ID_ALTITUDE_LEN, MAVLINK_MSG_ID_ALTITUDE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ALTITUDE_MIN_LEN, MAVLINK_MSG_ID_ALTITUDE_LEN); +#endif +} + /** * @brief Pack a altitude message on a channel * @param system_id ID of this system @@ -173,6 +224,20 @@ static inline uint16_t mavlink_msg_altitude_encode_chan(uint8_t system_id, uint8 return mavlink_msg_altitude_pack_chan(system_id, component_id, chan, msg, altitude->time_usec, altitude->altitude_monotonic, altitude->altitude_amsl, altitude->altitude_local, altitude->altitude_relative, altitude->altitude_terrain, altitude->bottom_clearance); } +/** + * @brief Encode a altitude struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param altitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_altitude_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_altitude_t* altitude) +{ + return mavlink_msg_altitude_pack_status(system_id, component_id, _status, msg, altitude->time_usec, altitude->altitude_monotonic, altitude->altitude_amsl, altitude->altitude_local, altitude->altitude_relative, altitude->altitude_terrain, altitude->bottom_clearance); +} + /** * @brief Send a altitude message * @param chan MAVLink channel to send the message @@ -230,7 +295,7 @@ static inline void mavlink_msg_altitude_send_struct(mavlink_channel_t chan, cons #if MAVLINK_MSG_ID_ALTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_att_pos_mocap.h b/lib/main/MAVLink/common/mavlink_msg_att_pos_mocap.h index 2bf0bfd1173..3d0ea12bf74 100755 --- a/lib/main/MAVLink/common/mavlink_msg_att_pos_mocap.h +++ b/lib/main/MAVLink/common/mavlink_msg_att_pos_mocap.h @@ -68,6 +68,48 @@ typedef struct __mavlink_att_pos_mocap_t { static inline uint16_t mavlink_msg_att_pos_mocap_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_usec, const float *q, float x, float y, float z, const float *covariance) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 24, x); + _mav_put_float(buf, 28, y); + _mav_put_float(buf, 32, z); + _mav_put_float_array(buf, 8, q, 4); + _mav_put_float_array(buf, 36, covariance, 21); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); +#else + mavlink_att_pos_mocap_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + mav_array_assign_float(packet.q, q, 4); + mav_array_assign_float(packet.covariance, covariance, 21); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATT_POS_MOCAP; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); +} + +/** + * @brief Pack a att_pos_mocap message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param x [m] X position (NED) + * @param y [m] Y position (NED) + * @param z [m] Z position (NED) + * @param covariance Row-major representation of a pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_att_pos_mocap_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, const float *q, float x, float y, float z, const float *covariance) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN]; _mav_put_uint64_t(buf, 0, time_usec); @@ -89,7 +131,11 @@ static inline uint16_t mavlink_msg_att_pos_mocap_pack(uint8_t system_id, uint8_t #endif msg->msgid = MAVLINK_MSG_ID_ATT_POS_MOCAP; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); +#endif } /** @@ -125,8 +171,8 @@ static inline uint16_t mavlink_msg_att_pos_mocap_pack_chan(uint8_t system_id, ui packet.x = x; packet.y = y; packet.z = z; - mav_array_memcpy(packet.q, q, sizeof(float)*4); - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); + mav_array_assign_float(packet.q, q, 4); + mav_array_assign_float(packet.covariance, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); #endif @@ -161,6 +207,20 @@ static inline uint16_t mavlink_msg_att_pos_mocap_encode_chan(uint8_t system_id, return mavlink_msg_att_pos_mocap_pack_chan(system_id, component_id, chan, msg, att_pos_mocap->time_usec, att_pos_mocap->q, att_pos_mocap->x, att_pos_mocap->y, att_pos_mocap->z, att_pos_mocap->covariance); } +/** + * @brief Encode a att_pos_mocap struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param att_pos_mocap C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_att_pos_mocap_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_att_pos_mocap_t* att_pos_mocap) +{ + return mavlink_msg_att_pos_mocap_pack_status(system_id, component_id, _status, msg, att_pos_mocap->time_usec, att_pos_mocap->q, att_pos_mocap->x, att_pos_mocap->y, att_pos_mocap->z, att_pos_mocap->covariance); +} + /** * @brief Send a att_pos_mocap message * @param chan MAVLink channel to send the message @@ -191,8 +251,8 @@ static inline void mavlink_msg_att_pos_mocap_send(mavlink_channel_t chan, uint64 packet.x = x; packet.y = y; packet.z = z; - mav_array_memcpy(packet.q, q, sizeof(float)*4); - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); + mav_array_assign_float(packet.q, q, 4); + mav_array_assign_float(packet.covariance, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, (const char *)&packet, MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); #endif } @@ -213,7 +273,7 @@ static inline void mavlink_msg_att_pos_mocap_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -236,8 +296,8 @@ static inline void mavlink_msg_att_pos_mocap_send_buf(mavlink_message_t *msgbuf, packet->x = x; packet->y = y; packet->z = z; - mav_array_memcpy(packet->q, q, sizeof(float)*4); - mav_array_memcpy(packet->covariance, covariance, sizeof(float)*21); + mav_array_assign_float(packet->q, q, 4); + mav_array_assign_float(packet->covariance, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, (const char *)packet, MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_attitude.h b/lib/main/MAVLink/common/mavlink_msg_attitude.h index 6a8237c92c4..c260822bfb9 100755 --- a/lib/main/MAVLink/common/mavlink_msg_attitude.h +++ b/lib/main/MAVLink/common/mavlink_msg_attitude.h @@ -99,6 +99,57 @@ static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t comp return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); } +/** + * @brief Pack a attitude message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param roll [rad] Roll angle (-pi..+pi) + * @param pitch [rad] Pitch angle (-pi..+pi) + * @param yaw [rad] Yaw angle (-pi..+pi) + * @param rollspeed [rad/s] Roll angular speed + * @param pitchspeed [rad/s] Pitch angular speed + * @param yawspeed [rad/s] Yaw angular speed + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, rollspeed); + _mav_put_float(buf, 20, pitchspeed); + _mav_put_float(buf, 24, yawspeed); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_LEN); +#else + mavlink_attitude_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_LEN); +#endif +} + /** * @brief Pack a attitude message on a channel * @param system_id ID of this system @@ -173,6 +224,20 @@ static inline uint16_t mavlink_msg_attitude_encode_chan(uint8_t system_id, uint8 return mavlink_msg_attitude_pack_chan(system_id, component_id, chan, msg, attitude->time_boot_ms, attitude->roll, attitude->pitch, attitude->yaw, attitude->rollspeed, attitude->pitchspeed, attitude->yawspeed); } +/** + * @brief Encode a attitude struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_attitude_t* attitude) +{ + return mavlink_msg_attitude_pack_status(system_id, component_id, _status, msg, attitude->time_boot_ms, attitude->roll, attitude->pitch, attitude->yaw, attitude->rollspeed, attitude->pitchspeed, attitude->yawspeed); +} + /** * @brief Send a attitude message * @param chan MAVLink channel to send the message @@ -230,7 +295,7 @@ static inline void mavlink_msg_attitude_send_struct(mavlink_channel_t chan, cons #if MAVLINK_MSG_ID_ATTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_attitude_quaternion.h b/lib/main/MAVLink/common/mavlink_msg_attitude_quaternion.h index a6138162216..a2f65fcbd1d 100755 --- a/lib/main/MAVLink/common/mavlink_msg_attitude_quaternion.h +++ b/lib/main/MAVLink/common/mavlink_msg_attitude_quaternion.h @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, u packet.rollspeed = rollspeed; packet.pitchspeed = pitchspeed; packet.yawspeed = yawspeed; - mav_array_memcpy(packet.repr_offset_q, repr_offset_q, sizeof(float)*4); + mav_array_assign_float(packet.repr_offset_q, repr_offset_q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); #endif @@ -109,6 +109,61 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, u return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); } +/** + * @brief Pack a attitude_quaternion message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param q1 Quaternion component 1, w (1 in null-rotation) + * @param q2 Quaternion component 2, x (0 in null-rotation) + * @param q3 Quaternion component 3, y (0 in null-rotation) + * @param q4 Quaternion component 4, z (0 in null-rotation) + * @param rollspeed [rad/s] Roll angular speed + * @param pitchspeed [rad/s] Pitch angular speed + * @param yawspeed [rad/s] Yaw angular speed + * @param repr_offset_q Rotation offset by which the attitude quaternion and angular speed vector should be rotated for user display (quaternion with [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], send [0, 0, 0, 0] if field not supported). This field is intended for systems in which the reference attitude may change during flight. For example, tailsitters VTOLs rotate their reference attitude by 90 degrees between hover mode and fixed wing mode, thus repr_offset_q is equal to [1, 0, 0, 0] in hover mode and equal to [0.7071, 0, 0.7071, 0] in fixed wing mode. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_quaternion_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed, const float *repr_offset_q) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, q1); + _mav_put_float(buf, 8, q2); + _mav_put_float(buf, 12, q3); + _mav_put_float(buf, 16, q4); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_float_array(buf, 32, repr_offset_q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#else + mavlink_attitude_quaternion_t packet; + packet.time_boot_ms = time_boot_ms; + packet.q1 = q1; + packet.q2 = q2; + packet.q3 = q3; + packet.q4 = q4; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + mav_array_memcpy(packet.repr_offset_q, repr_offset_q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#endif +} + /** * @brief Pack a attitude_quaternion message on a channel * @param system_id ID of this system @@ -152,7 +207,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_ packet.rollspeed = rollspeed; packet.pitchspeed = pitchspeed; packet.yawspeed = yawspeed; - mav_array_memcpy(packet.repr_offset_q, repr_offset_q, sizeof(float)*4); + mav_array_assign_float(packet.repr_offset_q, repr_offset_q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); #endif @@ -187,6 +242,20 @@ static inline uint16_t mavlink_msg_attitude_quaternion_encode_chan(uint8_t syste return mavlink_msg_attitude_quaternion_pack_chan(system_id, component_id, chan, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed, attitude_quaternion->repr_offset_q); } +/** + * @brief Encode a attitude_quaternion struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param attitude_quaternion C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_quaternion_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_attitude_quaternion_t* attitude_quaternion) +{ + return mavlink_msg_attitude_quaternion_pack_status(system_id, component_id, _status, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed, attitude_quaternion->repr_offset_q); +} + /** * @brief Send a attitude_quaternion message * @param chan MAVLink channel to send the message @@ -227,7 +296,7 @@ static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan, packet.rollspeed = rollspeed; packet.pitchspeed = pitchspeed; packet.yawspeed = yawspeed; - mav_array_memcpy(packet.repr_offset_q, repr_offset_q, sizeof(float)*4); + mav_array_assign_float(packet.repr_offset_q, repr_offset_q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); #endif } @@ -248,7 +317,7 @@ static inline void mavlink_msg_attitude_quaternion_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -278,7 +347,7 @@ static inline void mavlink_msg_attitude_quaternion_send_buf(mavlink_message_t *m packet->rollspeed = rollspeed; packet->pitchspeed = pitchspeed; packet->yawspeed = yawspeed; - mav_array_memcpy(packet->repr_offset_q, repr_offset_q, sizeof(float)*4); + mav_array_assign_float(packet->repr_offset_q, repr_offset_q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_attitude_quaternion_cov.h b/lib/main/MAVLink/common/mavlink_msg_attitude_quaternion_cov.h index bc2351aa3cf..86457f398be 100755 --- a/lib/main/MAVLink/common/mavlink_msg_attitude_quaternion_cov.h +++ b/lib/main/MAVLink/common/mavlink_msg_attitude_quaternion_cov.h @@ -68,6 +68,48 @@ typedef struct __mavlink_attitude_quaternion_cov_t { static inline uint16_t mavlink_msg_attitude_quaternion_cov_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_usec, const float *q, float rollspeed, float pitchspeed, float yawspeed, const float *covariance) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 24, rollspeed); + _mav_put_float(buf, 28, pitchspeed); + _mav_put_float(buf, 32, yawspeed); + _mav_put_float_array(buf, 8, q, 4); + _mav_put_float_array(buf, 36, covariance, 9); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#else + mavlink_attitude_quaternion_cov_t packet; + packet.time_usec = time_usec; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + mav_array_assign_float(packet.q, q, 4); + mav_array_assign_float(packet.covariance, covariance, 9); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC); +} + +/** + * @brief Pack a attitude_quaternion_cov message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + * @param rollspeed [rad/s] Roll angular speed + * @param pitchspeed [rad/s] Pitch angular speed + * @param yawspeed [rad/s] Yaw angular speed + * @param covariance Row-major representation of a 3x3 attitude covariance matrix (states: roll, pitch, yaw; first three entries are the first ROW, next three entries are the second row, etc.). If unknown, assign NaN value to first element in the array. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_quaternion_cov_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, const float *q, float rollspeed, float pitchspeed, float yawspeed, const float *covariance) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN]; _mav_put_uint64_t(buf, 0, time_usec); @@ -89,7 +131,11 @@ static inline uint16_t mavlink_msg_attitude_quaternion_cov_pack(uint8_t system_i #endif msg->msgid = MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); +#endif } /** @@ -125,8 +171,8 @@ static inline uint16_t mavlink_msg_attitude_quaternion_cov_pack_chan(uint8_t sys packet.rollspeed = rollspeed; packet.pitchspeed = pitchspeed; packet.yawspeed = yawspeed; - mav_array_memcpy(packet.q, q, sizeof(float)*4); - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*9); + mav_array_assign_float(packet.q, q, 4); + mav_array_assign_float(packet.covariance, covariance, 9); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN); #endif @@ -161,6 +207,20 @@ static inline uint16_t mavlink_msg_attitude_quaternion_cov_encode_chan(uint8_t s return mavlink_msg_attitude_quaternion_cov_pack_chan(system_id, component_id, chan, msg, attitude_quaternion_cov->time_usec, attitude_quaternion_cov->q, attitude_quaternion_cov->rollspeed, attitude_quaternion_cov->pitchspeed, attitude_quaternion_cov->yawspeed, attitude_quaternion_cov->covariance); } +/** + * @brief Encode a attitude_quaternion_cov struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param attitude_quaternion_cov C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_quaternion_cov_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_attitude_quaternion_cov_t* attitude_quaternion_cov) +{ + return mavlink_msg_attitude_quaternion_cov_pack_status(system_id, component_id, _status, msg, attitude_quaternion_cov->time_usec, attitude_quaternion_cov->q, attitude_quaternion_cov->rollspeed, attitude_quaternion_cov->pitchspeed, attitude_quaternion_cov->yawspeed, attitude_quaternion_cov->covariance); +} + /** * @brief Send a attitude_quaternion_cov message * @param chan MAVLink channel to send the message @@ -191,8 +251,8 @@ static inline void mavlink_msg_attitude_quaternion_cov_send(mavlink_channel_t ch packet.rollspeed = rollspeed; packet.pitchspeed = pitchspeed; packet.yawspeed = yawspeed; - mav_array_memcpy(packet.q, q, sizeof(float)*4); - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*9); + mav_array_assign_float(packet.q, q, 4); + mav_array_assign_float(packet.covariance, covariance, 9); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC); #endif } @@ -213,7 +273,7 @@ static inline void mavlink_msg_attitude_quaternion_cov_send_struct(mavlink_chann #if MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -236,8 +296,8 @@ static inline void mavlink_msg_attitude_quaternion_cov_send_buf(mavlink_message_ packet->rollspeed = rollspeed; packet->pitchspeed = pitchspeed; packet->yawspeed = yawspeed; - mav_array_memcpy(packet->q, q, sizeof(float)*4); - mav_array_memcpy(packet->covariance, covariance, sizeof(float)*9); + mav_array_assign_float(packet->q, q, 4); + mav_array_assign_float(packet->covariance, covariance, 9); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_attitude_target.h b/lib/main/MAVLink/common/mavlink_msg_attitude_target.h index bedbb93e9fb..89de26e1f77 100755 --- a/lib/main/MAVLink/common/mavlink_msg_attitude_target.h +++ b/lib/main/MAVLink/common/mavlink_msg_attitude_target.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_attitude_target_pack(uint8_t system_id, uint8 packet.body_yaw_rate = body_yaw_rate; packet.thrust = thrust; packet.type_mask = type_mask; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); #endif @@ -97,6 +97,55 @@ static inline uint16_t mavlink_msg_attitude_target_pack(uint8_t system_id, uint8 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_CRC); } +/** + * @brief Pack a attitude_target message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param body_roll_rate [rad/s] Body roll rate + * @param body_pitch_rate [rad/s] Body pitch rate + * @param body_yaw_rate [rad/s] Body yaw rate + * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_target_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, type_mask); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#else + mavlink_attitude_target_t packet; + packet.time_boot_ms = time_boot_ms; + packet.body_roll_rate = body_roll_rate; + packet.body_pitch_rate = body_pitch_rate; + packet.body_yaw_rate = body_yaw_rate; + packet.thrust = thrust; + packet.type_mask = type_mask; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); +#endif +} + /** * @brief Pack a attitude_target message on a channel * @param system_id ID of this system @@ -134,7 +183,7 @@ static inline uint16_t mavlink_msg_attitude_target_pack_chan(uint8_t system_id, packet.body_yaw_rate = body_yaw_rate; packet.thrust = thrust; packet.type_mask = type_mask; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN); #endif @@ -169,6 +218,20 @@ static inline uint16_t mavlink_msg_attitude_target_encode_chan(uint8_t system_id return mavlink_msg_attitude_target_pack_chan(system_id, component_id, chan, msg, attitude_target->time_boot_ms, attitude_target->type_mask, attitude_target->q, attitude_target->body_roll_rate, attitude_target->body_pitch_rate, attitude_target->body_yaw_rate, attitude_target->thrust); } +/** + * @brief Encode a attitude_target struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param attitude_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_target_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_attitude_target_t* attitude_target) +{ + return mavlink_msg_attitude_target_pack_status(system_id, component_id, _status, msg, attitude_target->time_boot_ms, attitude_target->type_mask, attitude_target->q, attitude_target->body_roll_rate, attitude_target->body_pitch_rate, attitude_target->body_yaw_rate, attitude_target->thrust); +} + /** * @brief Send a attitude_target message * @param chan MAVLink channel to send the message @@ -203,7 +266,7 @@ static inline void mavlink_msg_attitude_target_send(mavlink_channel_t chan, uint packet.body_yaw_rate = body_yaw_rate; packet.thrust = thrust; packet.type_mask = type_mask; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_TARGET, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_CRC); #endif } @@ -224,7 +287,7 @@ static inline void mavlink_msg_attitude_target_send_struct(mavlink_channel_t cha #if MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -250,7 +313,7 @@ static inline void mavlink_msg_attitude_target_send_buf(mavlink_message_t *msgbu packet->body_yaw_rate = body_yaw_rate; packet->thrust = thrust; packet->type_mask = type_mask; - mav_array_memcpy(packet->q, q, sizeof(float)*4); + mav_array_assign_float(packet->q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_TARGET, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_ATTITUDE_TARGET_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_auth_key.h b/lib/main/MAVLink/common/mavlink_msg_auth_key.h index 57cd6916ab6..827544a6953 100755 --- a/lib/main/MAVLink/common/mavlink_msg_auth_key.h +++ b/lib/main/MAVLink/common/mavlink_msg_auth_key.h @@ -55,7 +55,7 @@ static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t comp #else mavlink_auth_key_t packet; - mav_array_memcpy(packet.key, key, sizeof(char)*32); + mav_array_assign_char(packet.key, key, 32); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTH_KEY_LEN); #endif @@ -63,6 +63,39 @@ static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t comp return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTH_KEY_MIN_LEN, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); } +/** + * @brief Pack a auth_key message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param key key + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_auth_key_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + const char *key) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTH_KEY_LEN]; + + _mav_put_char_array(buf, 0, key, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#else + mavlink_auth_key_t packet; + + mav_array_memcpy(packet.key, key, sizeof(char)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_AUTH_KEY_MIN_LEN, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_AUTH_KEY_MIN_LEN, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#endif +} + /** * @brief Pack a auth_key message on a channel * @param system_id ID of this system @@ -84,7 +117,7 @@ static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t #else mavlink_auth_key_t packet; - mav_array_memcpy(packet.key, key, sizeof(char)*32); + mav_array_assign_char(packet.key, key, 32); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTH_KEY_LEN); #endif @@ -119,6 +152,20 @@ static inline uint16_t mavlink_msg_auth_key_encode_chan(uint8_t system_id, uint8 return mavlink_msg_auth_key_pack_chan(system_id, component_id, chan, msg, auth_key->key); } +/** + * @brief Encode a auth_key struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param auth_key C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_auth_key_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_auth_key_t* auth_key) +{ + return mavlink_msg_auth_key_pack_status(system_id, component_id, _status, msg, auth_key->key); +} + /** * @brief Send a auth_key message * @param chan MAVLink channel to send the message @@ -137,7 +184,7 @@ static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char #else mavlink_auth_key_t packet; - mav_array_memcpy(packet.key, key, sizeof(char)*32); + mav_array_assign_char(packet.key, key, 32); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)&packet, MAVLINK_MSG_ID_AUTH_KEY_MIN_LEN, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); #endif } @@ -158,7 +205,7 @@ static inline void mavlink_msg_auth_key_send_struct(mavlink_channel_t chan, cons #if MAVLINK_MSG_ID_AUTH_KEY_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -174,7 +221,7 @@ static inline void mavlink_msg_auth_key_send_buf(mavlink_message_t *msgbuf, mavl #else mavlink_auth_key_t *packet = (mavlink_auth_key_t *)msgbuf; - mav_array_memcpy(packet->key, key, sizeof(char)*32); + mav_array_assign_char(packet->key, key, 32); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)packet, MAVLINK_MSG_ID_AUTH_KEY_MIN_LEN, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_autopilot_state_for_gimbal_device.h b/lib/main/MAVLink/common/mavlink_msg_autopilot_state_for_gimbal_device.h index c9cdce8d542..e15fa6d45f0 100644 --- a/lib/main/MAVLink/common/mavlink_msg_autopilot_state_for_gimbal_device.h +++ b/lib/main/MAVLink/common/mavlink_msg_autopilot_state_for_gimbal_device.h @@ -3,25 +3,26 @@ #define MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE 286 - +MAVPACKED( typedef struct __mavlink_autopilot_state_for_gimbal_device_t { uint64_t time_boot_us; /*< [us] Timestamp (time since system boot).*/ float q[4]; /*< Quaternion components of autopilot attitude: w, x, y, z (1 0 0 0 is the null-rotation, Hamilton convention).*/ - uint32_t q_estimated_delay_us; /*< [us] Estimated delay of the attitude data.*/ - float vx; /*< [m/s] X Speed in NED (North, East, Down).*/ - float vy; /*< [m/s] Y Speed in NED (North, East, Down).*/ - float vz; /*< [m/s] Z Speed in NED (North, East, Down).*/ - uint32_t v_estimated_delay_us; /*< [us] Estimated delay of the speed data.*/ - float feed_forward_angular_velocity_z; /*< [rad/s] Feed forward Z component of angular velocity, positive is yawing to the right, NaN to be ignored. This is to indicate if the autopilot is actively yawing.*/ + uint32_t q_estimated_delay_us; /*< [us] Estimated delay of the attitude data. 0 if unknown.*/ + float vx; /*< [m/s] X Speed in NED (North, East, Down). NAN if unknown.*/ + float vy; /*< [m/s] Y Speed in NED (North, East, Down). NAN if unknown.*/ + float vz; /*< [m/s] Z Speed in NED (North, East, Down). NAN if unknown.*/ + uint32_t v_estimated_delay_us; /*< [us] Estimated delay of the speed data. 0 if unknown.*/ + float feed_forward_angular_velocity_z; /*< [rad/s] Feed forward Z component of angular velocity (positive: yawing to the right). NaN to be ignored. This is to indicate if the autopilot is actively yawing.*/ uint16_t estimator_status; /*< Bitmap indicating which estimator outputs are valid.*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ uint8_t landed_state; /*< The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown.*/ -} mavlink_autopilot_state_for_gimbal_device_t; + float angular_velocity_z; /*< [rad/s] Z component of angular velocity in NED (North, East, Down). NaN if unknown.*/ +}) mavlink_autopilot_state_for_gimbal_device_t; -#define MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN 53 +#define MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN 57 #define MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN 53 -#define MAVLINK_MSG_ID_286_LEN 53 +#define MAVLINK_MSG_ID_286_LEN 57 #define MAVLINK_MSG_ID_286_MIN_LEN 53 #define MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC 210 @@ -33,7 +34,7 @@ typedef struct __mavlink_autopilot_state_for_gimbal_device_t { #define MAVLINK_MESSAGE_INFO_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE { \ 286, \ "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE", \ - 12, \ + 13, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 50, offsetof(mavlink_autopilot_state_for_gimbal_device_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 51, offsetof(mavlink_autopilot_state_for_gimbal_device_t, target_component) }, \ { "time_boot_us", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_autopilot_state_for_gimbal_device_t, time_boot_us) }, \ @@ -46,12 +47,13 @@ typedef struct __mavlink_autopilot_state_for_gimbal_device_t { { "feed_forward_angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_autopilot_state_for_gimbal_device_t, feed_forward_angular_velocity_z) }, \ { "estimator_status", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_autopilot_state_for_gimbal_device_t, estimator_status) }, \ { "landed_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_autopilot_state_for_gimbal_device_t, landed_state) }, \ + { "angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 53, offsetof(mavlink_autopilot_state_for_gimbal_device_t, angular_velocity_z) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE { \ "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE", \ - 12, \ + 13, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 50, offsetof(mavlink_autopilot_state_for_gimbal_device_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 51, offsetof(mavlink_autopilot_state_for_gimbal_device_t, target_component) }, \ { "time_boot_us", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_autopilot_state_for_gimbal_device_t, time_boot_us) }, \ @@ -64,6 +66,7 @@ typedef struct __mavlink_autopilot_state_for_gimbal_device_t { { "feed_forward_angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_autopilot_state_for_gimbal_device_t, feed_forward_angular_velocity_z) }, \ { "estimator_status", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_autopilot_state_for_gimbal_device_t, estimator_status) }, \ { "landed_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_autopilot_state_for_gimbal_device_t, landed_state) }, \ + { "angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 53, offsetof(mavlink_autopilot_state_for_gimbal_device_t, angular_velocity_z) }, \ } \ } #endif @@ -78,18 +81,19 @@ typedef struct __mavlink_autopilot_state_for_gimbal_device_t { * @param target_component Component ID * @param time_boot_us [us] Timestamp (time since system boot). * @param q Quaternion components of autopilot attitude: w, x, y, z (1 0 0 0 is the null-rotation, Hamilton convention). - * @param q_estimated_delay_us [us] Estimated delay of the attitude data. - * @param vx [m/s] X Speed in NED (North, East, Down). - * @param vy [m/s] Y Speed in NED (North, East, Down). - * @param vz [m/s] Z Speed in NED (North, East, Down). - * @param v_estimated_delay_us [us] Estimated delay of the speed data. - * @param feed_forward_angular_velocity_z [rad/s] Feed forward Z component of angular velocity, positive is yawing to the right, NaN to be ignored. This is to indicate if the autopilot is actively yawing. + * @param q_estimated_delay_us [us] Estimated delay of the attitude data. 0 if unknown. + * @param vx [m/s] X Speed in NED (North, East, Down). NAN if unknown. + * @param vy [m/s] Y Speed in NED (North, East, Down). NAN if unknown. + * @param vz [m/s] Z Speed in NED (North, East, Down). NAN if unknown. + * @param v_estimated_delay_us [us] Estimated delay of the speed data. 0 if unknown. + * @param feed_forward_angular_velocity_z [rad/s] Feed forward Z component of angular velocity (positive: yawing to the right). NaN to be ignored. This is to indicate if the autopilot is actively yawing. * @param estimator_status Bitmap indicating which estimator outputs are valid. * @param landed_state The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + * @param angular_velocity_z [rad/s] Z component of angular velocity in NED (North, East, Down). NaN if unknown. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint64_t time_boot_us, const float *q, uint32_t q_estimated_delay_us, float vx, float vy, float vz, uint32_t v_estimated_delay_us, float feed_forward_angular_velocity_z, uint16_t estimator_status, uint8_t landed_state) + uint8_t target_system, uint8_t target_component, uint64_t time_boot_us, const float *q, uint32_t q_estimated_delay_us, float vx, float vy, float vz, uint32_t v_estimated_delay_us, float feed_forward_angular_velocity_z, uint16_t estimator_status, uint8_t landed_state, float angular_velocity_z) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN]; @@ -104,6 +108,7 @@ static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_pack(uint8_ _mav_put_uint8_t(buf, 50, target_system); _mav_put_uint8_t(buf, 51, target_component); _mav_put_uint8_t(buf, 52, landed_state); + _mav_put_float(buf, 53, angular_velocity_z); _mav_put_float_array(buf, 8, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN); #else @@ -119,7 +124,8 @@ static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_pack(uint8_ packet.target_system = target_system; packet.target_component = target_component; packet.landed_state = landed_state; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + packet.angular_velocity_z = angular_velocity_z; + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN); #endif @@ -127,6 +133,73 @@ static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_pack(uint8_ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); } +/** + * @brief Pack a autopilot_state_for_gimbal_device message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param time_boot_us [us] Timestamp (time since system boot). + * @param q Quaternion components of autopilot attitude: w, x, y, z (1 0 0 0 is the null-rotation, Hamilton convention). + * @param q_estimated_delay_us [us] Estimated delay of the attitude data. 0 if unknown. + * @param vx [m/s] X Speed in NED (North, East, Down). NAN if unknown. + * @param vy [m/s] Y Speed in NED (North, East, Down). NAN if unknown. + * @param vz [m/s] Z Speed in NED (North, East, Down). NAN if unknown. + * @param v_estimated_delay_us [us] Estimated delay of the speed data. 0 if unknown. + * @param feed_forward_angular_velocity_z [rad/s] Feed forward Z component of angular velocity (positive: yawing to the right). NaN to be ignored. This is to indicate if the autopilot is actively yawing. + * @param estimator_status Bitmap indicating which estimator outputs are valid. + * @param landed_state The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + * @param angular_velocity_z [rad/s] Z component of angular velocity in NED (North, East, Down). NaN if unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint64_t time_boot_us, const float *q, uint32_t q_estimated_delay_us, float vx, float vy, float vz, uint32_t v_estimated_delay_us, float feed_forward_angular_velocity_z, uint16_t estimator_status, uint8_t landed_state, float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN]; + _mav_put_uint64_t(buf, 0, time_boot_us); + _mav_put_uint32_t(buf, 24, q_estimated_delay_us); + _mav_put_float(buf, 28, vx); + _mav_put_float(buf, 32, vy); + _mav_put_float(buf, 36, vz); + _mav_put_uint32_t(buf, 40, v_estimated_delay_us); + _mav_put_float(buf, 44, feed_forward_angular_velocity_z); + _mav_put_uint16_t(buf, 48, estimator_status); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, landed_state); + _mav_put_float(buf, 53, angular_velocity_z); + _mav_put_float_array(buf, 8, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN); +#else + mavlink_autopilot_state_for_gimbal_device_t packet; + packet.time_boot_us = time_boot_us; + packet.q_estimated_delay_us = q_estimated_delay_us; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.v_estimated_delay_us = v_estimated_delay_us; + packet.feed_forward_angular_velocity_z = feed_forward_angular_velocity_z; + packet.estimator_status = estimator_status; + packet.target_system = target_system; + packet.target_component = target_component; + packet.landed_state = landed_state; + packet.angular_velocity_z = angular_velocity_z; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN); +#endif +} + /** * @brief Pack a autopilot_state_for_gimbal_device message on a channel * @param system_id ID of this system @@ -137,19 +210,20 @@ static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_pack(uint8_ * @param target_component Component ID * @param time_boot_us [us] Timestamp (time since system boot). * @param q Quaternion components of autopilot attitude: w, x, y, z (1 0 0 0 is the null-rotation, Hamilton convention). - * @param q_estimated_delay_us [us] Estimated delay of the attitude data. - * @param vx [m/s] X Speed in NED (North, East, Down). - * @param vy [m/s] Y Speed in NED (North, East, Down). - * @param vz [m/s] Z Speed in NED (North, East, Down). - * @param v_estimated_delay_us [us] Estimated delay of the speed data. - * @param feed_forward_angular_velocity_z [rad/s] Feed forward Z component of angular velocity, positive is yawing to the right, NaN to be ignored. This is to indicate if the autopilot is actively yawing. + * @param q_estimated_delay_us [us] Estimated delay of the attitude data. 0 if unknown. + * @param vx [m/s] X Speed in NED (North, East, Down). NAN if unknown. + * @param vy [m/s] Y Speed in NED (North, East, Down). NAN if unknown. + * @param vz [m/s] Z Speed in NED (North, East, Down). NAN if unknown. + * @param v_estimated_delay_us [us] Estimated delay of the speed data. 0 if unknown. + * @param feed_forward_angular_velocity_z [rad/s] Feed forward Z component of angular velocity (positive: yawing to the right). NaN to be ignored. This is to indicate if the autopilot is actively yawing. * @param estimator_status Bitmap indicating which estimator outputs are valid. * @param landed_state The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + * @param angular_velocity_z [rad/s] Z component of angular velocity in NED (North, East, Down). NaN if unknown. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint64_t time_boot_us,const float *q,uint32_t q_estimated_delay_us,float vx,float vy,float vz,uint32_t v_estimated_delay_us,float feed_forward_angular_velocity_z,uint16_t estimator_status,uint8_t landed_state) + uint8_t target_system,uint8_t target_component,uint64_t time_boot_us,const float *q,uint32_t q_estimated_delay_us,float vx,float vy,float vz,uint32_t v_estimated_delay_us,float feed_forward_angular_velocity_z,uint16_t estimator_status,uint8_t landed_state,float angular_velocity_z) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN]; @@ -164,6 +238,7 @@ static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_pack_chan(u _mav_put_uint8_t(buf, 50, target_system); _mav_put_uint8_t(buf, 51, target_component); _mav_put_uint8_t(buf, 52, landed_state); + _mav_put_float(buf, 53, angular_velocity_z); _mav_put_float_array(buf, 8, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN); #else @@ -179,7 +254,8 @@ static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_pack_chan(u packet.target_system = target_system; packet.target_component = target_component; packet.landed_state = landed_state; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + packet.angular_velocity_z = angular_velocity_z; + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN); #endif @@ -197,7 +273,7 @@ static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_pack_chan(u */ static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_autopilot_state_for_gimbal_device_t* autopilot_state_for_gimbal_device) { - return mavlink_msg_autopilot_state_for_gimbal_device_pack(system_id, component_id, msg, autopilot_state_for_gimbal_device->target_system, autopilot_state_for_gimbal_device->target_component, autopilot_state_for_gimbal_device->time_boot_us, autopilot_state_for_gimbal_device->q, autopilot_state_for_gimbal_device->q_estimated_delay_us, autopilot_state_for_gimbal_device->vx, autopilot_state_for_gimbal_device->vy, autopilot_state_for_gimbal_device->vz, autopilot_state_for_gimbal_device->v_estimated_delay_us, autopilot_state_for_gimbal_device->feed_forward_angular_velocity_z, autopilot_state_for_gimbal_device->estimator_status, autopilot_state_for_gimbal_device->landed_state); + return mavlink_msg_autopilot_state_for_gimbal_device_pack(system_id, component_id, msg, autopilot_state_for_gimbal_device->target_system, autopilot_state_for_gimbal_device->target_component, autopilot_state_for_gimbal_device->time_boot_us, autopilot_state_for_gimbal_device->q, autopilot_state_for_gimbal_device->q_estimated_delay_us, autopilot_state_for_gimbal_device->vx, autopilot_state_for_gimbal_device->vy, autopilot_state_for_gimbal_device->vz, autopilot_state_for_gimbal_device->v_estimated_delay_us, autopilot_state_for_gimbal_device->feed_forward_angular_velocity_z, autopilot_state_for_gimbal_device->estimator_status, autopilot_state_for_gimbal_device->landed_state, autopilot_state_for_gimbal_device->angular_velocity_z); } /** @@ -211,7 +287,21 @@ static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_encode(uint */ static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_autopilot_state_for_gimbal_device_t* autopilot_state_for_gimbal_device) { - return mavlink_msg_autopilot_state_for_gimbal_device_pack_chan(system_id, component_id, chan, msg, autopilot_state_for_gimbal_device->target_system, autopilot_state_for_gimbal_device->target_component, autopilot_state_for_gimbal_device->time_boot_us, autopilot_state_for_gimbal_device->q, autopilot_state_for_gimbal_device->q_estimated_delay_us, autopilot_state_for_gimbal_device->vx, autopilot_state_for_gimbal_device->vy, autopilot_state_for_gimbal_device->vz, autopilot_state_for_gimbal_device->v_estimated_delay_us, autopilot_state_for_gimbal_device->feed_forward_angular_velocity_z, autopilot_state_for_gimbal_device->estimator_status, autopilot_state_for_gimbal_device->landed_state); + return mavlink_msg_autopilot_state_for_gimbal_device_pack_chan(system_id, component_id, chan, msg, autopilot_state_for_gimbal_device->target_system, autopilot_state_for_gimbal_device->target_component, autopilot_state_for_gimbal_device->time_boot_us, autopilot_state_for_gimbal_device->q, autopilot_state_for_gimbal_device->q_estimated_delay_us, autopilot_state_for_gimbal_device->vx, autopilot_state_for_gimbal_device->vy, autopilot_state_for_gimbal_device->vz, autopilot_state_for_gimbal_device->v_estimated_delay_us, autopilot_state_for_gimbal_device->feed_forward_angular_velocity_z, autopilot_state_for_gimbal_device->estimator_status, autopilot_state_for_gimbal_device->landed_state, autopilot_state_for_gimbal_device->angular_velocity_z); +} + +/** + * @brief Encode a autopilot_state_for_gimbal_device struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param autopilot_state_for_gimbal_device C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_autopilot_state_for_gimbal_device_t* autopilot_state_for_gimbal_device) +{ + return mavlink_msg_autopilot_state_for_gimbal_device_pack_status(system_id, component_id, _status, msg, autopilot_state_for_gimbal_device->target_system, autopilot_state_for_gimbal_device->target_component, autopilot_state_for_gimbal_device->time_boot_us, autopilot_state_for_gimbal_device->q, autopilot_state_for_gimbal_device->q_estimated_delay_us, autopilot_state_for_gimbal_device->vx, autopilot_state_for_gimbal_device->vy, autopilot_state_for_gimbal_device->vz, autopilot_state_for_gimbal_device->v_estimated_delay_us, autopilot_state_for_gimbal_device->feed_forward_angular_velocity_z, autopilot_state_for_gimbal_device->estimator_status, autopilot_state_for_gimbal_device->landed_state, autopilot_state_for_gimbal_device->angular_velocity_z); } /** @@ -222,18 +312,19 @@ static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_encode_chan * @param target_component Component ID * @param time_boot_us [us] Timestamp (time since system boot). * @param q Quaternion components of autopilot attitude: w, x, y, z (1 0 0 0 is the null-rotation, Hamilton convention). - * @param q_estimated_delay_us [us] Estimated delay of the attitude data. - * @param vx [m/s] X Speed in NED (North, East, Down). - * @param vy [m/s] Y Speed in NED (North, East, Down). - * @param vz [m/s] Z Speed in NED (North, East, Down). - * @param v_estimated_delay_us [us] Estimated delay of the speed data. - * @param feed_forward_angular_velocity_z [rad/s] Feed forward Z component of angular velocity, positive is yawing to the right, NaN to be ignored. This is to indicate if the autopilot is actively yawing. + * @param q_estimated_delay_us [us] Estimated delay of the attitude data. 0 if unknown. + * @param vx [m/s] X Speed in NED (North, East, Down). NAN if unknown. + * @param vy [m/s] Y Speed in NED (North, East, Down). NAN if unknown. + * @param vz [m/s] Z Speed in NED (North, East, Down). NAN if unknown. + * @param v_estimated_delay_us [us] Estimated delay of the speed data. 0 if unknown. + * @param feed_forward_angular_velocity_z [rad/s] Feed forward Z component of angular velocity (positive: yawing to the right). NaN to be ignored. This is to indicate if the autopilot is actively yawing. * @param estimator_status Bitmap indicating which estimator outputs are valid. * @param landed_state The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + * @param angular_velocity_z [rad/s] Z component of angular velocity in NED (North, East, Down). NaN if unknown. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_autopilot_state_for_gimbal_device_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint64_t time_boot_us, const float *q, uint32_t q_estimated_delay_us, float vx, float vy, float vz, uint32_t v_estimated_delay_us, float feed_forward_angular_velocity_z, uint16_t estimator_status, uint8_t landed_state) +static inline void mavlink_msg_autopilot_state_for_gimbal_device_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint64_t time_boot_us, const float *q, uint32_t q_estimated_delay_us, float vx, float vy, float vz, uint32_t v_estimated_delay_us, float feed_forward_angular_velocity_z, uint16_t estimator_status, uint8_t landed_state, float angular_velocity_z) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN]; @@ -248,6 +339,7 @@ static inline void mavlink_msg_autopilot_state_for_gimbal_device_send(mavlink_ch _mav_put_uint8_t(buf, 50, target_system); _mav_put_uint8_t(buf, 51, target_component); _mav_put_uint8_t(buf, 52, landed_state); + _mav_put_float(buf, 53, angular_velocity_z); _mav_put_float_array(buf, 8, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, buf, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); #else @@ -263,7 +355,8 @@ static inline void mavlink_msg_autopilot_state_for_gimbal_device_send(mavlink_ch packet.target_system = target_system; packet.target_component = target_component; packet.landed_state = landed_state; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + packet.angular_velocity_z = angular_velocity_z; + mav_array_assign_float(packet.q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, (const char *)&packet, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); #endif } @@ -276,7 +369,7 @@ static inline void mavlink_msg_autopilot_state_for_gimbal_device_send(mavlink_ch static inline void mavlink_msg_autopilot_state_for_gimbal_device_send_struct(mavlink_channel_t chan, const mavlink_autopilot_state_for_gimbal_device_t* autopilot_state_for_gimbal_device) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_autopilot_state_for_gimbal_device_send(chan, autopilot_state_for_gimbal_device->target_system, autopilot_state_for_gimbal_device->target_component, autopilot_state_for_gimbal_device->time_boot_us, autopilot_state_for_gimbal_device->q, autopilot_state_for_gimbal_device->q_estimated_delay_us, autopilot_state_for_gimbal_device->vx, autopilot_state_for_gimbal_device->vy, autopilot_state_for_gimbal_device->vz, autopilot_state_for_gimbal_device->v_estimated_delay_us, autopilot_state_for_gimbal_device->feed_forward_angular_velocity_z, autopilot_state_for_gimbal_device->estimator_status, autopilot_state_for_gimbal_device->landed_state); + mavlink_msg_autopilot_state_for_gimbal_device_send(chan, autopilot_state_for_gimbal_device->target_system, autopilot_state_for_gimbal_device->target_component, autopilot_state_for_gimbal_device->time_boot_us, autopilot_state_for_gimbal_device->q, autopilot_state_for_gimbal_device->q_estimated_delay_us, autopilot_state_for_gimbal_device->vx, autopilot_state_for_gimbal_device->vy, autopilot_state_for_gimbal_device->vz, autopilot_state_for_gimbal_device->v_estimated_delay_us, autopilot_state_for_gimbal_device->feed_forward_angular_velocity_z, autopilot_state_for_gimbal_device->estimator_status, autopilot_state_for_gimbal_device->landed_state, autopilot_state_for_gimbal_device->angular_velocity_z); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, (const char *)autopilot_state_for_gimbal_device, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); #endif @@ -284,13 +377,13 @@ static inline void mavlink_msg_autopilot_state_for_gimbal_device_send_struct(mav #if MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_autopilot_state_for_gimbal_device_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint64_t time_boot_us, const float *q, uint32_t q_estimated_delay_us, float vx, float vy, float vz, uint32_t v_estimated_delay_us, float feed_forward_angular_velocity_z, uint16_t estimator_status, uint8_t landed_state) +static inline void mavlink_msg_autopilot_state_for_gimbal_device_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint64_t time_boot_us, const float *q, uint32_t q_estimated_delay_us, float vx, float vy, float vz, uint32_t v_estimated_delay_us, float feed_forward_angular_velocity_z, uint16_t estimator_status, uint8_t landed_state, float angular_velocity_z) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -305,6 +398,7 @@ static inline void mavlink_msg_autopilot_state_for_gimbal_device_send_buf(mavlin _mav_put_uint8_t(buf, 50, target_system); _mav_put_uint8_t(buf, 51, target_component); _mav_put_uint8_t(buf, 52, landed_state); + _mav_put_float(buf, 53, angular_velocity_z); _mav_put_float_array(buf, 8, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, buf, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); #else @@ -320,7 +414,8 @@ static inline void mavlink_msg_autopilot_state_for_gimbal_device_send_buf(mavlin packet->target_system = target_system; packet->target_component = target_component; packet->landed_state = landed_state; - mav_array_memcpy(packet->q, q, sizeof(float)*4); + packet->angular_velocity_z = angular_velocity_z; + mav_array_assign_float(packet->q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, (const char *)packet, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); #endif } @@ -374,7 +469,7 @@ static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_get_q(const /** * @brief Get field q_estimated_delay_us from autopilot_state_for_gimbal_device message * - * @return [us] Estimated delay of the attitude data. + * @return [us] Estimated delay of the attitude data. 0 if unknown. */ static inline uint32_t mavlink_msg_autopilot_state_for_gimbal_device_get_q_estimated_delay_us(const mavlink_message_t* msg) { @@ -384,7 +479,7 @@ static inline uint32_t mavlink_msg_autopilot_state_for_gimbal_device_get_q_estim /** * @brief Get field vx from autopilot_state_for_gimbal_device message * - * @return [m/s] X Speed in NED (North, East, Down). + * @return [m/s] X Speed in NED (North, East, Down). NAN if unknown. */ static inline float mavlink_msg_autopilot_state_for_gimbal_device_get_vx(const mavlink_message_t* msg) { @@ -394,7 +489,7 @@ static inline float mavlink_msg_autopilot_state_for_gimbal_device_get_vx(const m /** * @brief Get field vy from autopilot_state_for_gimbal_device message * - * @return [m/s] Y Speed in NED (North, East, Down). + * @return [m/s] Y Speed in NED (North, East, Down). NAN if unknown. */ static inline float mavlink_msg_autopilot_state_for_gimbal_device_get_vy(const mavlink_message_t* msg) { @@ -404,7 +499,7 @@ static inline float mavlink_msg_autopilot_state_for_gimbal_device_get_vy(const m /** * @brief Get field vz from autopilot_state_for_gimbal_device message * - * @return [m/s] Z Speed in NED (North, East, Down). + * @return [m/s] Z Speed in NED (North, East, Down). NAN if unknown. */ static inline float mavlink_msg_autopilot_state_for_gimbal_device_get_vz(const mavlink_message_t* msg) { @@ -414,7 +509,7 @@ static inline float mavlink_msg_autopilot_state_for_gimbal_device_get_vz(const m /** * @brief Get field v_estimated_delay_us from autopilot_state_for_gimbal_device message * - * @return [us] Estimated delay of the speed data. + * @return [us] Estimated delay of the speed data. 0 if unknown. */ static inline uint32_t mavlink_msg_autopilot_state_for_gimbal_device_get_v_estimated_delay_us(const mavlink_message_t* msg) { @@ -424,7 +519,7 @@ static inline uint32_t mavlink_msg_autopilot_state_for_gimbal_device_get_v_estim /** * @brief Get field feed_forward_angular_velocity_z from autopilot_state_for_gimbal_device message * - * @return [rad/s] Feed forward Z component of angular velocity, positive is yawing to the right, NaN to be ignored. This is to indicate if the autopilot is actively yawing. + * @return [rad/s] Feed forward Z component of angular velocity (positive: yawing to the right). NaN to be ignored. This is to indicate if the autopilot is actively yawing. */ static inline float mavlink_msg_autopilot_state_for_gimbal_device_get_feed_forward_angular_velocity_z(const mavlink_message_t* msg) { @@ -451,6 +546,16 @@ static inline uint8_t mavlink_msg_autopilot_state_for_gimbal_device_get_landed_s return _MAV_RETURN_uint8_t(msg, 52); } +/** + * @brief Get field angular_velocity_z from autopilot_state_for_gimbal_device message + * + * @return [rad/s] Z component of angular velocity in NED (North, East, Down). NaN if unknown. + */ +static inline float mavlink_msg_autopilot_state_for_gimbal_device_get_angular_velocity_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 53); +} + /** * @brief Decode a autopilot_state_for_gimbal_device message into a struct * @@ -472,6 +577,7 @@ static inline void mavlink_msg_autopilot_state_for_gimbal_device_decode(const ma autopilot_state_for_gimbal_device->target_system = mavlink_msg_autopilot_state_for_gimbal_device_get_target_system(msg); autopilot_state_for_gimbal_device->target_component = mavlink_msg_autopilot_state_for_gimbal_device_get_target_component(msg); autopilot_state_for_gimbal_device->landed_state = mavlink_msg_autopilot_state_for_gimbal_device_get_landed_state(msg); + autopilot_state_for_gimbal_device->angular_velocity_z = mavlink_msg_autopilot_state_for_gimbal_device_get_angular_velocity_z(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN? msg->len : MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN; memset(autopilot_state_for_gimbal_device, 0, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_available_modes.h b/lib/main/MAVLink/common/mavlink_msg_available_modes.h new file mode 100644 index 00000000000..a316e271fe6 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_available_modes.h @@ -0,0 +1,390 @@ +#pragma once +// MESSAGE AVAILABLE_MODES PACKING + +#define MAVLINK_MSG_ID_AVAILABLE_MODES 435 + + +typedef struct __mavlink_available_modes_t { + uint32_t custom_mode; /*< A bitfield for use for autopilot-specific flags*/ + uint32_t properties; /*< Mode properties.*/ + uint8_t number_modes; /*< The total number of available modes for the current vehicle type.*/ + uint8_t mode_index; /*< The current mode index within number_modes, indexed from 1. The index is not guaranteed to be persistent, and may change between reboots or if the set of modes change.*/ + uint8_t standard_mode; /*< Standard mode.*/ + char mode_name[35]; /*< Name of custom mode, with null termination character. Should be omitted for standard modes.*/ +} mavlink_available_modes_t; + +#define MAVLINK_MSG_ID_AVAILABLE_MODES_LEN 46 +#define MAVLINK_MSG_ID_AVAILABLE_MODES_MIN_LEN 46 +#define MAVLINK_MSG_ID_435_LEN 46 +#define MAVLINK_MSG_ID_435_MIN_LEN 46 + +#define MAVLINK_MSG_ID_AVAILABLE_MODES_CRC 134 +#define MAVLINK_MSG_ID_435_CRC 134 + +#define MAVLINK_MSG_AVAILABLE_MODES_FIELD_MODE_NAME_LEN 35 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_AVAILABLE_MODES { \ + 435, \ + "AVAILABLE_MODES", \ + 6, \ + { { "number_modes", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_available_modes_t, number_modes) }, \ + { "mode_index", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_available_modes_t, mode_index) }, \ + { "standard_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_available_modes_t, standard_mode) }, \ + { "custom_mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_available_modes_t, custom_mode) }, \ + { "properties", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_available_modes_t, properties) }, \ + { "mode_name", NULL, MAVLINK_TYPE_CHAR, 35, 11, offsetof(mavlink_available_modes_t, mode_name) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_AVAILABLE_MODES { \ + "AVAILABLE_MODES", \ + 6, \ + { { "number_modes", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_available_modes_t, number_modes) }, \ + { "mode_index", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_available_modes_t, mode_index) }, \ + { "standard_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_available_modes_t, standard_mode) }, \ + { "custom_mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_available_modes_t, custom_mode) }, \ + { "properties", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_available_modes_t, properties) }, \ + { "mode_name", NULL, MAVLINK_TYPE_CHAR, 35, 11, offsetof(mavlink_available_modes_t, mode_name) }, \ + } \ +} +#endif + +/** + * @brief Pack a available_modes message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param number_modes The total number of available modes for the current vehicle type. + * @param mode_index The current mode index within number_modes, indexed from 1. The index is not guaranteed to be persistent, and may change between reboots or if the set of modes change. + * @param standard_mode Standard mode. + * @param custom_mode A bitfield for use for autopilot-specific flags + * @param properties Mode properties. + * @param mode_name Name of custom mode, with null termination character. Should be omitted for standard modes. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_available_modes_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t number_modes, uint8_t mode_index, uint8_t standard_mode, uint32_t custom_mode, uint32_t properties, const char *mode_name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AVAILABLE_MODES_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint32_t(buf, 4, properties); + _mav_put_uint8_t(buf, 8, number_modes); + _mav_put_uint8_t(buf, 9, mode_index); + _mav_put_uint8_t(buf, 10, standard_mode); + _mav_put_char_array(buf, 11, mode_name, 35); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AVAILABLE_MODES_LEN); +#else + mavlink_available_modes_t packet; + packet.custom_mode = custom_mode; + packet.properties = properties; + packet.number_modes = number_modes; + packet.mode_index = mode_index; + packet.standard_mode = standard_mode; + mav_array_assign_char(packet.mode_name, mode_name, 35); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AVAILABLE_MODES_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AVAILABLE_MODES; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AVAILABLE_MODES_MIN_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_CRC); +} + +/** + * @brief Pack a available_modes message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param number_modes The total number of available modes for the current vehicle type. + * @param mode_index The current mode index within number_modes, indexed from 1. The index is not guaranteed to be persistent, and may change between reboots or if the set of modes change. + * @param standard_mode Standard mode. + * @param custom_mode A bitfield for use for autopilot-specific flags + * @param properties Mode properties. + * @param mode_name Name of custom mode, with null termination character. Should be omitted for standard modes. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_available_modes_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t number_modes, uint8_t mode_index, uint8_t standard_mode, uint32_t custom_mode, uint32_t properties, const char *mode_name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AVAILABLE_MODES_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint32_t(buf, 4, properties); + _mav_put_uint8_t(buf, 8, number_modes); + _mav_put_uint8_t(buf, 9, mode_index); + _mav_put_uint8_t(buf, 10, standard_mode); + _mav_put_char_array(buf, 11, mode_name, 35); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AVAILABLE_MODES_LEN); +#else + mavlink_available_modes_t packet; + packet.custom_mode = custom_mode; + packet.properties = properties; + packet.number_modes = number_modes; + packet.mode_index = mode_index; + packet.standard_mode = standard_mode; + mav_array_memcpy(packet.mode_name, mode_name, sizeof(char)*35); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AVAILABLE_MODES_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AVAILABLE_MODES; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_AVAILABLE_MODES_MIN_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_AVAILABLE_MODES_MIN_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_LEN); +#endif +} + +/** + * @brief Pack a available_modes message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param number_modes The total number of available modes for the current vehicle type. + * @param mode_index The current mode index within number_modes, indexed from 1. The index is not guaranteed to be persistent, and may change between reboots or if the set of modes change. + * @param standard_mode Standard mode. + * @param custom_mode A bitfield for use for autopilot-specific flags + * @param properties Mode properties. + * @param mode_name Name of custom mode, with null termination character. Should be omitted for standard modes. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_available_modes_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t number_modes,uint8_t mode_index,uint8_t standard_mode,uint32_t custom_mode,uint32_t properties,const char *mode_name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AVAILABLE_MODES_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint32_t(buf, 4, properties); + _mav_put_uint8_t(buf, 8, number_modes); + _mav_put_uint8_t(buf, 9, mode_index); + _mav_put_uint8_t(buf, 10, standard_mode); + _mav_put_char_array(buf, 11, mode_name, 35); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AVAILABLE_MODES_LEN); +#else + mavlink_available_modes_t packet; + packet.custom_mode = custom_mode; + packet.properties = properties; + packet.number_modes = number_modes; + packet.mode_index = mode_index; + packet.standard_mode = standard_mode; + mav_array_assign_char(packet.mode_name, mode_name, 35); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AVAILABLE_MODES_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AVAILABLE_MODES; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AVAILABLE_MODES_MIN_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_CRC); +} + +/** + * @brief Encode a available_modes struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param available_modes C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_available_modes_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_available_modes_t* available_modes) +{ + return mavlink_msg_available_modes_pack(system_id, component_id, msg, available_modes->number_modes, available_modes->mode_index, available_modes->standard_mode, available_modes->custom_mode, available_modes->properties, available_modes->mode_name); +} + +/** + * @brief Encode a available_modes struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param available_modes C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_available_modes_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_available_modes_t* available_modes) +{ + return mavlink_msg_available_modes_pack_chan(system_id, component_id, chan, msg, available_modes->number_modes, available_modes->mode_index, available_modes->standard_mode, available_modes->custom_mode, available_modes->properties, available_modes->mode_name); +} + +/** + * @brief Encode a available_modes struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param available_modes C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_available_modes_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_available_modes_t* available_modes) +{ + return mavlink_msg_available_modes_pack_status(system_id, component_id, _status, msg, available_modes->number_modes, available_modes->mode_index, available_modes->standard_mode, available_modes->custom_mode, available_modes->properties, available_modes->mode_name); +} + +/** + * @brief Send a available_modes message + * @param chan MAVLink channel to send the message + * + * @param number_modes The total number of available modes for the current vehicle type. + * @param mode_index The current mode index within number_modes, indexed from 1. The index is not guaranteed to be persistent, and may change between reboots or if the set of modes change. + * @param standard_mode Standard mode. + * @param custom_mode A bitfield for use for autopilot-specific flags + * @param properties Mode properties. + * @param mode_name Name of custom mode, with null termination character. Should be omitted for standard modes. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_available_modes_send(mavlink_channel_t chan, uint8_t number_modes, uint8_t mode_index, uint8_t standard_mode, uint32_t custom_mode, uint32_t properties, const char *mode_name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AVAILABLE_MODES_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint32_t(buf, 4, properties); + _mav_put_uint8_t(buf, 8, number_modes); + _mav_put_uint8_t(buf, 9, mode_index); + _mav_put_uint8_t(buf, 10, standard_mode); + _mav_put_char_array(buf, 11, mode_name, 35); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AVAILABLE_MODES, buf, MAVLINK_MSG_ID_AVAILABLE_MODES_MIN_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_CRC); +#else + mavlink_available_modes_t packet; + packet.custom_mode = custom_mode; + packet.properties = properties; + packet.number_modes = number_modes; + packet.mode_index = mode_index; + packet.standard_mode = standard_mode; + mav_array_assign_char(packet.mode_name, mode_name, 35); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AVAILABLE_MODES, (const char *)&packet, MAVLINK_MSG_ID_AVAILABLE_MODES_MIN_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_CRC); +#endif +} + +/** + * @brief Send a available_modes message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_available_modes_send_struct(mavlink_channel_t chan, const mavlink_available_modes_t* available_modes) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_available_modes_send(chan, available_modes->number_modes, available_modes->mode_index, available_modes->standard_mode, available_modes->custom_mode, available_modes->properties, available_modes->mode_name); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AVAILABLE_MODES, (const char *)available_modes, MAVLINK_MSG_ID_AVAILABLE_MODES_MIN_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_CRC); +#endif +} + +#if MAVLINK_MSG_ID_AVAILABLE_MODES_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_available_modes_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t number_modes, uint8_t mode_index, uint8_t standard_mode, uint32_t custom_mode, uint32_t properties, const char *mode_name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint32_t(buf, 4, properties); + _mav_put_uint8_t(buf, 8, number_modes); + _mav_put_uint8_t(buf, 9, mode_index); + _mav_put_uint8_t(buf, 10, standard_mode); + _mav_put_char_array(buf, 11, mode_name, 35); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AVAILABLE_MODES, buf, MAVLINK_MSG_ID_AVAILABLE_MODES_MIN_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_CRC); +#else + mavlink_available_modes_t *packet = (mavlink_available_modes_t *)msgbuf; + packet->custom_mode = custom_mode; + packet->properties = properties; + packet->number_modes = number_modes; + packet->mode_index = mode_index; + packet->standard_mode = standard_mode; + mav_array_assign_char(packet->mode_name, mode_name, 35); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AVAILABLE_MODES, (const char *)packet, MAVLINK_MSG_ID_AVAILABLE_MODES_MIN_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_CRC); +#endif +} +#endif + +#endif + +// MESSAGE AVAILABLE_MODES UNPACKING + + +/** + * @brief Get field number_modes from available_modes message + * + * @return The total number of available modes for the current vehicle type. + */ +static inline uint8_t mavlink_msg_available_modes_get_number_modes(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field mode_index from available_modes message + * + * @return The current mode index within number_modes, indexed from 1. The index is not guaranteed to be persistent, and may change between reboots or if the set of modes change. + */ +static inline uint8_t mavlink_msg_available_modes_get_mode_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 9); +} + +/** + * @brief Get field standard_mode from available_modes message + * + * @return Standard mode. + */ +static inline uint8_t mavlink_msg_available_modes_get_standard_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 10); +} + +/** + * @brief Get field custom_mode from available_modes message + * + * @return A bitfield for use for autopilot-specific flags + */ +static inline uint32_t mavlink_msg_available_modes_get_custom_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field properties from available_modes message + * + * @return Mode properties. + */ +static inline uint32_t mavlink_msg_available_modes_get_properties(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field mode_name from available_modes message + * + * @return Name of custom mode, with null termination character. Should be omitted for standard modes. + */ +static inline uint16_t mavlink_msg_available_modes_get_mode_name(const mavlink_message_t* msg, char *mode_name) +{ + return _MAV_RETURN_char_array(msg, mode_name, 35, 11); +} + +/** + * @brief Decode a available_modes message into a struct + * + * @param msg The message to decode + * @param available_modes C-struct to decode the message contents into + */ +static inline void mavlink_msg_available_modes_decode(const mavlink_message_t* msg, mavlink_available_modes_t* available_modes) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + available_modes->custom_mode = mavlink_msg_available_modes_get_custom_mode(msg); + available_modes->properties = mavlink_msg_available_modes_get_properties(msg); + available_modes->number_modes = mavlink_msg_available_modes_get_number_modes(msg); + available_modes->mode_index = mavlink_msg_available_modes_get_mode_index(msg); + available_modes->standard_mode = mavlink_msg_available_modes_get_standard_mode(msg); + mavlink_msg_available_modes_get_mode_name(msg, available_modes->mode_name); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_AVAILABLE_MODES_LEN? msg->len : MAVLINK_MSG_ID_AVAILABLE_MODES_LEN; + memset(available_modes, 0, MAVLINK_MSG_ID_AVAILABLE_MODES_LEN); + memcpy(available_modes, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_available_modes_monitor.h b/lib/main/MAVLink/common/mavlink_msg_available_modes_monitor.h new file mode 100644 index 00000000000..e3c902f4ada --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_available_modes_monitor.h @@ -0,0 +1,260 @@ +#pragma once +// MESSAGE AVAILABLE_MODES_MONITOR PACKING + +#define MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR 437 + + +typedef struct __mavlink_available_modes_monitor_t { + uint8_t seq; /*< Sequence number. The value iterates sequentially whenever AVAILABLE_MODES changes (e.g. support for a new mode is added/removed dynamically).*/ +} mavlink_available_modes_monitor_t; + +#define MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN 1 +#define MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_MIN_LEN 1 +#define MAVLINK_MSG_ID_437_LEN 1 +#define MAVLINK_MSG_ID_437_MIN_LEN 1 + +#define MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_CRC 30 +#define MAVLINK_MSG_ID_437_CRC 30 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_AVAILABLE_MODES_MONITOR { \ + 437, \ + "AVAILABLE_MODES_MONITOR", \ + 1, \ + { { "seq", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_available_modes_monitor_t, seq) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_AVAILABLE_MODES_MONITOR { \ + "AVAILABLE_MODES_MONITOR", \ + 1, \ + { { "seq", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_available_modes_monitor_t, seq) }, \ + } \ +} +#endif + +/** + * @brief Pack a available_modes_monitor message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param seq Sequence number. The value iterates sequentially whenever AVAILABLE_MODES changes (e.g. support for a new mode is added/removed dynamically). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_available_modes_monitor_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN]; + _mav_put_uint8_t(buf, 0, seq); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN); +#else + mavlink_available_modes_monitor_t packet; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_MIN_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_CRC); +} + +/** + * @brief Pack a available_modes_monitor message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param seq Sequence number. The value iterates sequentially whenever AVAILABLE_MODES changes (e.g. support for a new mode is added/removed dynamically). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_available_modes_monitor_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN]; + _mav_put_uint8_t(buf, 0, seq); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN); +#else + mavlink_available_modes_monitor_t packet; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_MIN_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_MIN_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN); +#endif +} + +/** + * @brief Pack a available_modes_monitor message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param seq Sequence number. The value iterates sequentially whenever AVAILABLE_MODES changes (e.g. support for a new mode is added/removed dynamically). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_available_modes_monitor_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN]; + _mav_put_uint8_t(buf, 0, seq); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN); +#else + mavlink_available_modes_monitor_t packet; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_MIN_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_CRC); +} + +/** + * @brief Encode a available_modes_monitor struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param available_modes_monitor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_available_modes_monitor_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_available_modes_monitor_t* available_modes_monitor) +{ + return mavlink_msg_available_modes_monitor_pack(system_id, component_id, msg, available_modes_monitor->seq); +} + +/** + * @brief Encode a available_modes_monitor struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param available_modes_monitor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_available_modes_monitor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_available_modes_monitor_t* available_modes_monitor) +{ + return mavlink_msg_available_modes_monitor_pack_chan(system_id, component_id, chan, msg, available_modes_monitor->seq); +} + +/** + * @brief Encode a available_modes_monitor struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param available_modes_monitor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_available_modes_monitor_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_available_modes_monitor_t* available_modes_monitor) +{ + return mavlink_msg_available_modes_monitor_pack_status(system_id, component_id, _status, msg, available_modes_monitor->seq); +} + +/** + * @brief Send a available_modes_monitor message + * @param chan MAVLink channel to send the message + * + * @param seq Sequence number. The value iterates sequentially whenever AVAILABLE_MODES changes (e.g. support for a new mode is added/removed dynamically). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_available_modes_monitor_send(mavlink_channel_t chan, uint8_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN]; + _mav_put_uint8_t(buf, 0, seq); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR, buf, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_MIN_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_CRC); +#else + mavlink_available_modes_monitor_t packet; + packet.seq = seq; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR, (const char *)&packet, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_MIN_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_CRC); +#endif +} + +/** + * @brief Send a available_modes_monitor message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_available_modes_monitor_send_struct(mavlink_channel_t chan, const mavlink_available_modes_monitor_t* available_modes_monitor) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_available_modes_monitor_send(chan, available_modes_monitor->seq); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR, (const char *)available_modes_monitor, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_MIN_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_CRC); +#endif +} + +#if MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_available_modes_monitor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, seq); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR, buf, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_MIN_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_CRC); +#else + mavlink_available_modes_monitor_t *packet = (mavlink_available_modes_monitor_t *)msgbuf; + packet->seq = seq; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR, (const char *)packet, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_MIN_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_CRC); +#endif +} +#endif + +#endif + +// MESSAGE AVAILABLE_MODES_MONITOR UNPACKING + + +/** + * @brief Get field seq from available_modes_monitor message + * + * @return Sequence number. The value iterates sequentially whenever AVAILABLE_MODES changes (e.g. support for a new mode is added/removed dynamically). + */ +static inline uint8_t mavlink_msg_available_modes_monitor_get_seq(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Decode a available_modes_monitor message into a struct + * + * @param msg The message to decode + * @param available_modes_monitor C-struct to decode the message contents into + */ +static inline void mavlink_msg_available_modes_monitor_decode(const mavlink_message_t* msg, mavlink_available_modes_monitor_t* available_modes_monitor) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + available_modes_monitor->seq = mavlink_msg_available_modes_monitor_get_seq(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN? msg->len : MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN; + memset(available_modes_monitor, 0, MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN); + memcpy(available_modes_monitor, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_battery_info.h b/lib/main/MAVLink/common/mavlink_msg_battery_info.h new file mode 100644 index 00000000000..19e29f36ce3 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_battery_info.h @@ -0,0 +1,784 @@ +#pragma once +// MESSAGE BATTERY_INFO PACKING + +#define MAVLINK_MSG_ID_BATTERY_INFO 372 + + +typedef struct __mavlink_battery_info_t { + float discharge_minimum_voltage; /*< [V] Minimum per-cell voltage when discharging. 0: field not provided.*/ + float charging_minimum_voltage; /*< [V] Minimum per-cell voltage when charging. 0: field not provided.*/ + float resting_minimum_voltage; /*< [V] Minimum per-cell voltage when resting. 0: field not provided.*/ + float charging_maximum_voltage; /*< [V] Maximum per-cell voltage when charged. 0: field not provided.*/ + float charging_maximum_current; /*< [A] Maximum pack continuous charge current. 0: field not provided.*/ + float nominal_voltage; /*< [V] Battery nominal voltage. Used for conversion between Wh and Ah. 0: field not provided.*/ + float discharge_maximum_current; /*< [A] Maximum pack discharge current. 0: field not provided.*/ + float discharge_maximum_burst_current; /*< [A] Maximum pack discharge burst current. 0: field not provided.*/ + float design_capacity; /*< [Ah] Fully charged design capacity. 0: field not provided.*/ + float full_charge_capacity; /*< [Ah] Predicted battery capacity when fully charged (accounting for battery degradation). NAN: field not provided.*/ + uint16_t cycle_count; /*< Lifetime count of the number of charge/discharge cycles (https://en.wikipedia.org/wiki/Charge_cycle). UINT16_MAX: field not provided.*/ + uint16_t weight; /*< [g] Battery weight. 0: field not provided.*/ + uint8_t id; /*< Battery ID*/ + uint8_t battery_function; /*< Function of the battery.*/ + uint8_t type; /*< Type (chemistry) of the battery.*/ + uint8_t state_of_health; /*< [%] State of Health (SOH) estimate. Typically 100% at the time of manufacture and will decrease over time and use. -1: field not provided.*/ + uint8_t cells_in_series; /*< Number of battery cells in series. 0: field not provided.*/ + char manufacture_date[9]; /*< Manufacture date (DDMMYYYY) in ASCII characters, 0 terminated. All 0: field not provided.*/ + char serial_number[32]; /*< Serial number in ASCII characters, 0 terminated. All 0: field not provided.*/ + char name[50]; /*< Battery device name. Formatted as manufacturer name then product name, separated with an underscore (in ASCII characters), 0 terminated. All 0: field not provided.*/ +} mavlink_battery_info_t; + +#define MAVLINK_MSG_ID_BATTERY_INFO_LEN 140 +#define MAVLINK_MSG_ID_BATTERY_INFO_MIN_LEN 140 +#define MAVLINK_MSG_ID_372_LEN 140 +#define MAVLINK_MSG_ID_372_MIN_LEN 140 + +#define MAVLINK_MSG_ID_BATTERY_INFO_CRC 26 +#define MAVLINK_MSG_ID_372_CRC 26 + +#define MAVLINK_MSG_BATTERY_INFO_FIELD_MANUFACTURE_DATE_LEN 9 +#define MAVLINK_MSG_BATTERY_INFO_FIELD_SERIAL_NUMBER_LEN 32 +#define MAVLINK_MSG_BATTERY_INFO_FIELD_NAME_LEN 50 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_BATTERY_INFO { \ + 372, \ + "BATTERY_INFO", \ + 20, \ + { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 44, offsetof(mavlink_battery_info_t, id) }, \ + { "battery_function", NULL, MAVLINK_TYPE_UINT8_T, 0, 45, offsetof(mavlink_battery_info_t, battery_function) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 46, offsetof(mavlink_battery_info_t, type) }, \ + { "state_of_health", NULL, MAVLINK_TYPE_UINT8_T, 0, 47, offsetof(mavlink_battery_info_t, state_of_health) }, \ + { "cells_in_series", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_battery_info_t, cells_in_series) }, \ + { "cycle_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 40, offsetof(mavlink_battery_info_t, cycle_count) }, \ + { "weight", NULL, MAVLINK_TYPE_UINT16_T, 0, 42, offsetof(mavlink_battery_info_t, weight) }, \ + { "discharge_minimum_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_battery_info_t, discharge_minimum_voltage) }, \ + { "charging_minimum_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_battery_info_t, charging_minimum_voltage) }, \ + { "resting_minimum_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_battery_info_t, resting_minimum_voltage) }, \ + { "charging_maximum_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_battery_info_t, charging_maximum_voltage) }, \ + { "charging_maximum_current", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_battery_info_t, charging_maximum_current) }, \ + { "nominal_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_battery_info_t, nominal_voltage) }, \ + { "discharge_maximum_current", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_battery_info_t, discharge_maximum_current) }, \ + { "discharge_maximum_burst_current", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_battery_info_t, discharge_maximum_burst_current) }, \ + { "design_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_battery_info_t, design_capacity) }, \ + { "full_charge_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_battery_info_t, full_charge_capacity) }, \ + { "manufacture_date", NULL, MAVLINK_TYPE_CHAR, 9, 49, offsetof(mavlink_battery_info_t, manufacture_date) }, \ + { "serial_number", NULL, MAVLINK_TYPE_CHAR, 32, 58, offsetof(mavlink_battery_info_t, serial_number) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 50, 90, offsetof(mavlink_battery_info_t, name) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_BATTERY_INFO { \ + "BATTERY_INFO", \ + 20, \ + { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 44, offsetof(mavlink_battery_info_t, id) }, \ + { "battery_function", NULL, MAVLINK_TYPE_UINT8_T, 0, 45, offsetof(mavlink_battery_info_t, battery_function) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 46, offsetof(mavlink_battery_info_t, type) }, \ + { "state_of_health", NULL, MAVLINK_TYPE_UINT8_T, 0, 47, offsetof(mavlink_battery_info_t, state_of_health) }, \ + { "cells_in_series", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_battery_info_t, cells_in_series) }, \ + { "cycle_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 40, offsetof(mavlink_battery_info_t, cycle_count) }, \ + { "weight", NULL, MAVLINK_TYPE_UINT16_T, 0, 42, offsetof(mavlink_battery_info_t, weight) }, \ + { "discharge_minimum_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_battery_info_t, discharge_minimum_voltage) }, \ + { "charging_minimum_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_battery_info_t, charging_minimum_voltage) }, \ + { "resting_minimum_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_battery_info_t, resting_minimum_voltage) }, \ + { "charging_maximum_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_battery_info_t, charging_maximum_voltage) }, \ + { "charging_maximum_current", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_battery_info_t, charging_maximum_current) }, \ + { "nominal_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_battery_info_t, nominal_voltage) }, \ + { "discharge_maximum_current", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_battery_info_t, discharge_maximum_current) }, \ + { "discharge_maximum_burst_current", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_battery_info_t, discharge_maximum_burst_current) }, \ + { "design_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_battery_info_t, design_capacity) }, \ + { "full_charge_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_battery_info_t, full_charge_capacity) }, \ + { "manufacture_date", NULL, MAVLINK_TYPE_CHAR, 9, 49, offsetof(mavlink_battery_info_t, manufacture_date) }, \ + { "serial_number", NULL, MAVLINK_TYPE_CHAR, 32, 58, offsetof(mavlink_battery_info_t, serial_number) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 50, 90, offsetof(mavlink_battery_info_t, name) }, \ + } \ +} +#endif + +/** + * @brief Pack a battery_info message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param id Battery ID + * @param battery_function Function of the battery. + * @param type Type (chemistry) of the battery. + * @param state_of_health [%] State of Health (SOH) estimate. Typically 100% at the time of manufacture and will decrease over time and use. -1: field not provided. + * @param cells_in_series Number of battery cells in series. 0: field not provided. + * @param cycle_count Lifetime count of the number of charge/discharge cycles (https://en.wikipedia.org/wiki/Charge_cycle). UINT16_MAX: field not provided. + * @param weight [g] Battery weight. 0: field not provided. + * @param discharge_minimum_voltage [V] Minimum per-cell voltage when discharging. 0: field not provided. + * @param charging_minimum_voltage [V] Minimum per-cell voltage when charging. 0: field not provided. + * @param resting_minimum_voltage [V] Minimum per-cell voltage when resting. 0: field not provided. + * @param charging_maximum_voltage [V] Maximum per-cell voltage when charged. 0: field not provided. + * @param charging_maximum_current [A] Maximum pack continuous charge current. 0: field not provided. + * @param nominal_voltage [V] Battery nominal voltage. Used for conversion between Wh and Ah. 0: field not provided. + * @param discharge_maximum_current [A] Maximum pack discharge current. 0: field not provided. + * @param discharge_maximum_burst_current [A] Maximum pack discharge burst current. 0: field not provided. + * @param design_capacity [Ah] Fully charged design capacity. 0: field not provided. + * @param full_charge_capacity [Ah] Predicted battery capacity when fully charged (accounting for battery degradation). NAN: field not provided. + * @param manufacture_date Manufacture date (DDMMYYYY) in ASCII characters, 0 terminated. All 0: field not provided. + * @param serial_number Serial number in ASCII characters, 0 terminated. All 0: field not provided. + * @param name Battery device name. Formatted as manufacturer name then product name, separated with an underscore (in ASCII characters), 0 terminated. All 0: field not provided. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_battery_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t id, uint8_t battery_function, uint8_t type, uint8_t state_of_health, uint8_t cells_in_series, uint16_t cycle_count, uint16_t weight, float discharge_minimum_voltage, float charging_minimum_voltage, float resting_minimum_voltage, float charging_maximum_voltage, float charging_maximum_current, float nominal_voltage, float discharge_maximum_current, float discharge_maximum_burst_current, float design_capacity, float full_charge_capacity, const char *manufacture_date, const char *serial_number, const char *name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_BATTERY_INFO_LEN]; + _mav_put_float(buf, 0, discharge_minimum_voltage); + _mav_put_float(buf, 4, charging_minimum_voltage); + _mav_put_float(buf, 8, resting_minimum_voltage); + _mav_put_float(buf, 12, charging_maximum_voltage); + _mav_put_float(buf, 16, charging_maximum_current); + _mav_put_float(buf, 20, nominal_voltage); + _mav_put_float(buf, 24, discharge_maximum_current); + _mav_put_float(buf, 28, discharge_maximum_burst_current); + _mav_put_float(buf, 32, design_capacity); + _mav_put_float(buf, 36, full_charge_capacity); + _mav_put_uint16_t(buf, 40, cycle_count); + _mav_put_uint16_t(buf, 42, weight); + _mav_put_uint8_t(buf, 44, id); + _mav_put_uint8_t(buf, 45, battery_function); + _mav_put_uint8_t(buf, 46, type); + _mav_put_uint8_t(buf, 47, state_of_health); + _mav_put_uint8_t(buf, 48, cells_in_series); + _mav_put_char_array(buf, 49, manufacture_date, 9); + _mav_put_char_array(buf, 58, serial_number, 32); + _mav_put_char_array(buf, 90, name, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY_INFO_LEN); +#else + mavlink_battery_info_t packet; + packet.discharge_minimum_voltage = discharge_minimum_voltage; + packet.charging_minimum_voltage = charging_minimum_voltage; + packet.resting_minimum_voltage = resting_minimum_voltage; + packet.charging_maximum_voltage = charging_maximum_voltage; + packet.charging_maximum_current = charging_maximum_current; + packet.nominal_voltage = nominal_voltage; + packet.discharge_maximum_current = discharge_maximum_current; + packet.discharge_maximum_burst_current = discharge_maximum_burst_current; + packet.design_capacity = design_capacity; + packet.full_charge_capacity = full_charge_capacity; + packet.cycle_count = cycle_count; + packet.weight = weight; + packet.id = id; + packet.battery_function = battery_function; + packet.type = type; + packet.state_of_health = state_of_health; + packet.cells_in_series = cells_in_series; + mav_array_assign_char(packet.manufacture_date, manufacture_date, 9); + mav_array_assign_char(packet.serial_number, serial_number, 32); + mav_array_assign_char(packet.name, name, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_INFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_BATTERY_INFO; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_BATTERY_INFO_LEN, MAVLINK_MSG_ID_BATTERY_INFO_CRC); +} + +/** + * @brief Pack a battery_info message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param id Battery ID + * @param battery_function Function of the battery. + * @param type Type (chemistry) of the battery. + * @param state_of_health [%] State of Health (SOH) estimate. Typically 100% at the time of manufacture and will decrease over time and use. -1: field not provided. + * @param cells_in_series Number of battery cells in series. 0: field not provided. + * @param cycle_count Lifetime count of the number of charge/discharge cycles (https://en.wikipedia.org/wiki/Charge_cycle). UINT16_MAX: field not provided. + * @param weight [g] Battery weight. 0: field not provided. + * @param discharge_minimum_voltage [V] Minimum per-cell voltage when discharging. 0: field not provided. + * @param charging_minimum_voltage [V] Minimum per-cell voltage when charging. 0: field not provided. + * @param resting_minimum_voltage [V] Minimum per-cell voltage when resting. 0: field not provided. + * @param charging_maximum_voltage [V] Maximum per-cell voltage when charged. 0: field not provided. + * @param charging_maximum_current [A] Maximum pack continuous charge current. 0: field not provided. + * @param nominal_voltage [V] Battery nominal voltage. Used for conversion between Wh and Ah. 0: field not provided. + * @param discharge_maximum_current [A] Maximum pack discharge current. 0: field not provided. + * @param discharge_maximum_burst_current [A] Maximum pack discharge burst current. 0: field not provided. + * @param design_capacity [Ah] Fully charged design capacity. 0: field not provided. + * @param full_charge_capacity [Ah] Predicted battery capacity when fully charged (accounting for battery degradation). NAN: field not provided. + * @param manufacture_date Manufacture date (DDMMYYYY) in ASCII characters, 0 terminated. All 0: field not provided. + * @param serial_number Serial number in ASCII characters, 0 terminated. All 0: field not provided. + * @param name Battery device name. Formatted as manufacturer name then product name, separated with an underscore (in ASCII characters), 0 terminated. All 0: field not provided. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_battery_info_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t id, uint8_t battery_function, uint8_t type, uint8_t state_of_health, uint8_t cells_in_series, uint16_t cycle_count, uint16_t weight, float discharge_minimum_voltage, float charging_minimum_voltage, float resting_minimum_voltage, float charging_maximum_voltage, float charging_maximum_current, float nominal_voltage, float discharge_maximum_current, float discharge_maximum_burst_current, float design_capacity, float full_charge_capacity, const char *manufacture_date, const char *serial_number, const char *name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_BATTERY_INFO_LEN]; + _mav_put_float(buf, 0, discharge_minimum_voltage); + _mav_put_float(buf, 4, charging_minimum_voltage); + _mav_put_float(buf, 8, resting_minimum_voltage); + _mav_put_float(buf, 12, charging_maximum_voltage); + _mav_put_float(buf, 16, charging_maximum_current); + _mav_put_float(buf, 20, nominal_voltage); + _mav_put_float(buf, 24, discharge_maximum_current); + _mav_put_float(buf, 28, discharge_maximum_burst_current); + _mav_put_float(buf, 32, design_capacity); + _mav_put_float(buf, 36, full_charge_capacity); + _mav_put_uint16_t(buf, 40, cycle_count); + _mav_put_uint16_t(buf, 42, weight); + _mav_put_uint8_t(buf, 44, id); + _mav_put_uint8_t(buf, 45, battery_function); + _mav_put_uint8_t(buf, 46, type); + _mav_put_uint8_t(buf, 47, state_of_health); + _mav_put_uint8_t(buf, 48, cells_in_series); + _mav_put_char_array(buf, 49, manufacture_date, 9); + _mav_put_char_array(buf, 58, serial_number, 32); + _mav_put_char_array(buf, 90, name, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY_INFO_LEN); +#else + mavlink_battery_info_t packet; + packet.discharge_minimum_voltage = discharge_minimum_voltage; + packet.charging_minimum_voltage = charging_minimum_voltage; + packet.resting_minimum_voltage = resting_minimum_voltage; + packet.charging_maximum_voltage = charging_maximum_voltage; + packet.charging_maximum_current = charging_maximum_current; + packet.nominal_voltage = nominal_voltage; + packet.discharge_maximum_current = discharge_maximum_current; + packet.discharge_maximum_burst_current = discharge_maximum_burst_current; + packet.design_capacity = design_capacity; + packet.full_charge_capacity = full_charge_capacity; + packet.cycle_count = cycle_count; + packet.weight = weight; + packet.id = id; + packet.battery_function = battery_function; + packet.type = type; + packet.state_of_health = state_of_health; + packet.cells_in_series = cells_in_series; + mav_array_memcpy(packet.manufacture_date, manufacture_date, sizeof(char)*9); + mav_array_memcpy(packet.serial_number, serial_number, sizeof(char)*32); + mav_array_memcpy(packet.name, name, sizeof(char)*50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_INFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_BATTERY_INFO; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_BATTERY_INFO_LEN, MAVLINK_MSG_ID_BATTERY_INFO_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_BATTERY_INFO_LEN); +#endif +} + +/** + * @brief Pack a battery_info message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param id Battery ID + * @param battery_function Function of the battery. + * @param type Type (chemistry) of the battery. + * @param state_of_health [%] State of Health (SOH) estimate. Typically 100% at the time of manufacture and will decrease over time and use. -1: field not provided. + * @param cells_in_series Number of battery cells in series. 0: field not provided. + * @param cycle_count Lifetime count of the number of charge/discharge cycles (https://en.wikipedia.org/wiki/Charge_cycle). UINT16_MAX: field not provided. + * @param weight [g] Battery weight. 0: field not provided. + * @param discharge_minimum_voltage [V] Minimum per-cell voltage when discharging. 0: field not provided. + * @param charging_minimum_voltage [V] Minimum per-cell voltage when charging. 0: field not provided. + * @param resting_minimum_voltage [V] Minimum per-cell voltage when resting. 0: field not provided. + * @param charging_maximum_voltage [V] Maximum per-cell voltage when charged. 0: field not provided. + * @param charging_maximum_current [A] Maximum pack continuous charge current. 0: field not provided. + * @param nominal_voltage [V] Battery nominal voltage. Used for conversion between Wh and Ah. 0: field not provided. + * @param discharge_maximum_current [A] Maximum pack discharge current. 0: field not provided. + * @param discharge_maximum_burst_current [A] Maximum pack discharge burst current. 0: field not provided. + * @param design_capacity [Ah] Fully charged design capacity. 0: field not provided. + * @param full_charge_capacity [Ah] Predicted battery capacity when fully charged (accounting for battery degradation). NAN: field not provided. + * @param manufacture_date Manufacture date (DDMMYYYY) in ASCII characters, 0 terminated. All 0: field not provided. + * @param serial_number Serial number in ASCII characters, 0 terminated. All 0: field not provided. + * @param name Battery device name. Formatted as manufacturer name then product name, separated with an underscore (in ASCII characters), 0 terminated. All 0: field not provided. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_battery_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t id,uint8_t battery_function,uint8_t type,uint8_t state_of_health,uint8_t cells_in_series,uint16_t cycle_count,uint16_t weight,float discharge_minimum_voltage,float charging_minimum_voltage,float resting_minimum_voltage,float charging_maximum_voltage,float charging_maximum_current,float nominal_voltage,float discharge_maximum_current,float discharge_maximum_burst_current,float design_capacity,float full_charge_capacity,const char *manufacture_date,const char *serial_number,const char *name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_BATTERY_INFO_LEN]; + _mav_put_float(buf, 0, discharge_minimum_voltage); + _mav_put_float(buf, 4, charging_minimum_voltage); + _mav_put_float(buf, 8, resting_minimum_voltage); + _mav_put_float(buf, 12, charging_maximum_voltage); + _mav_put_float(buf, 16, charging_maximum_current); + _mav_put_float(buf, 20, nominal_voltage); + _mav_put_float(buf, 24, discharge_maximum_current); + _mav_put_float(buf, 28, discharge_maximum_burst_current); + _mav_put_float(buf, 32, design_capacity); + _mav_put_float(buf, 36, full_charge_capacity); + _mav_put_uint16_t(buf, 40, cycle_count); + _mav_put_uint16_t(buf, 42, weight); + _mav_put_uint8_t(buf, 44, id); + _mav_put_uint8_t(buf, 45, battery_function); + _mav_put_uint8_t(buf, 46, type); + _mav_put_uint8_t(buf, 47, state_of_health); + _mav_put_uint8_t(buf, 48, cells_in_series); + _mav_put_char_array(buf, 49, manufacture_date, 9); + _mav_put_char_array(buf, 58, serial_number, 32); + _mav_put_char_array(buf, 90, name, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY_INFO_LEN); +#else + mavlink_battery_info_t packet; + packet.discharge_minimum_voltage = discharge_minimum_voltage; + packet.charging_minimum_voltage = charging_minimum_voltage; + packet.resting_minimum_voltage = resting_minimum_voltage; + packet.charging_maximum_voltage = charging_maximum_voltage; + packet.charging_maximum_current = charging_maximum_current; + packet.nominal_voltage = nominal_voltage; + packet.discharge_maximum_current = discharge_maximum_current; + packet.discharge_maximum_burst_current = discharge_maximum_burst_current; + packet.design_capacity = design_capacity; + packet.full_charge_capacity = full_charge_capacity; + packet.cycle_count = cycle_count; + packet.weight = weight; + packet.id = id; + packet.battery_function = battery_function; + packet.type = type; + packet.state_of_health = state_of_health; + packet.cells_in_series = cells_in_series; + mav_array_assign_char(packet.manufacture_date, manufacture_date, 9); + mav_array_assign_char(packet.serial_number, serial_number, 32); + mav_array_assign_char(packet.name, name, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_INFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_BATTERY_INFO; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_BATTERY_INFO_LEN, MAVLINK_MSG_ID_BATTERY_INFO_CRC); +} + +/** + * @brief Encode a battery_info struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param battery_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_battery_info_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_battery_info_t* battery_info) +{ + return mavlink_msg_battery_info_pack(system_id, component_id, msg, battery_info->id, battery_info->battery_function, battery_info->type, battery_info->state_of_health, battery_info->cells_in_series, battery_info->cycle_count, battery_info->weight, battery_info->discharge_minimum_voltage, battery_info->charging_minimum_voltage, battery_info->resting_minimum_voltage, battery_info->charging_maximum_voltage, battery_info->charging_maximum_current, battery_info->nominal_voltage, battery_info->discharge_maximum_current, battery_info->discharge_maximum_burst_current, battery_info->design_capacity, battery_info->full_charge_capacity, battery_info->manufacture_date, battery_info->serial_number, battery_info->name); +} + +/** + * @brief Encode a battery_info struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param battery_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_battery_info_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_battery_info_t* battery_info) +{ + return mavlink_msg_battery_info_pack_chan(system_id, component_id, chan, msg, battery_info->id, battery_info->battery_function, battery_info->type, battery_info->state_of_health, battery_info->cells_in_series, battery_info->cycle_count, battery_info->weight, battery_info->discharge_minimum_voltage, battery_info->charging_minimum_voltage, battery_info->resting_minimum_voltage, battery_info->charging_maximum_voltage, battery_info->charging_maximum_current, battery_info->nominal_voltage, battery_info->discharge_maximum_current, battery_info->discharge_maximum_burst_current, battery_info->design_capacity, battery_info->full_charge_capacity, battery_info->manufacture_date, battery_info->serial_number, battery_info->name); +} + +/** + * @brief Encode a battery_info struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param battery_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_battery_info_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_battery_info_t* battery_info) +{ + return mavlink_msg_battery_info_pack_status(system_id, component_id, _status, msg, battery_info->id, battery_info->battery_function, battery_info->type, battery_info->state_of_health, battery_info->cells_in_series, battery_info->cycle_count, battery_info->weight, battery_info->discharge_minimum_voltage, battery_info->charging_minimum_voltage, battery_info->resting_minimum_voltage, battery_info->charging_maximum_voltage, battery_info->charging_maximum_current, battery_info->nominal_voltage, battery_info->discharge_maximum_current, battery_info->discharge_maximum_burst_current, battery_info->design_capacity, battery_info->full_charge_capacity, battery_info->manufacture_date, battery_info->serial_number, battery_info->name); +} + +/** + * @brief Send a battery_info message + * @param chan MAVLink channel to send the message + * + * @param id Battery ID + * @param battery_function Function of the battery. + * @param type Type (chemistry) of the battery. + * @param state_of_health [%] State of Health (SOH) estimate. Typically 100% at the time of manufacture and will decrease over time and use. -1: field not provided. + * @param cells_in_series Number of battery cells in series. 0: field not provided. + * @param cycle_count Lifetime count of the number of charge/discharge cycles (https://en.wikipedia.org/wiki/Charge_cycle). UINT16_MAX: field not provided. + * @param weight [g] Battery weight. 0: field not provided. + * @param discharge_minimum_voltage [V] Minimum per-cell voltage when discharging. 0: field not provided. + * @param charging_minimum_voltage [V] Minimum per-cell voltage when charging. 0: field not provided. + * @param resting_minimum_voltage [V] Minimum per-cell voltage when resting. 0: field not provided. + * @param charging_maximum_voltage [V] Maximum per-cell voltage when charged. 0: field not provided. + * @param charging_maximum_current [A] Maximum pack continuous charge current. 0: field not provided. + * @param nominal_voltage [V] Battery nominal voltage. Used for conversion between Wh and Ah. 0: field not provided. + * @param discharge_maximum_current [A] Maximum pack discharge current. 0: field not provided. + * @param discharge_maximum_burst_current [A] Maximum pack discharge burst current. 0: field not provided. + * @param design_capacity [Ah] Fully charged design capacity. 0: field not provided. + * @param full_charge_capacity [Ah] Predicted battery capacity when fully charged (accounting for battery degradation). NAN: field not provided. + * @param manufacture_date Manufacture date (DDMMYYYY) in ASCII characters, 0 terminated. All 0: field not provided. + * @param serial_number Serial number in ASCII characters, 0 terminated. All 0: field not provided. + * @param name Battery device name. Formatted as manufacturer name then product name, separated with an underscore (in ASCII characters), 0 terminated. All 0: field not provided. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_battery_info_send(mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, uint8_t state_of_health, uint8_t cells_in_series, uint16_t cycle_count, uint16_t weight, float discharge_minimum_voltage, float charging_minimum_voltage, float resting_minimum_voltage, float charging_maximum_voltage, float charging_maximum_current, float nominal_voltage, float discharge_maximum_current, float discharge_maximum_burst_current, float design_capacity, float full_charge_capacity, const char *manufacture_date, const char *serial_number, const char *name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_BATTERY_INFO_LEN]; + _mav_put_float(buf, 0, discharge_minimum_voltage); + _mav_put_float(buf, 4, charging_minimum_voltage); + _mav_put_float(buf, 8, resting_minimum_voltage); + _mav_put_float(buf, 12, charging_maximum_voltage); + _mav_put_float(buf, 16, charging_maximum_current); + _mav_put_float(buf, 20, nominal_voltage); + _mav_put_float(buf, 24, discharge_maximum_current); + _mav_put_float(buf, 28, discharge_maximum_burst_current); + _mav_put_float(buf, 32, design_capacity); + _mav_put_float(buf, 36, full_charge_capacity); + _mav_put_uint16_t(buf, 40, cycle_count); + _mav_put_uint16_t(buf, 42, weight); + _mav_put_uint8_t(buf, 44, id); + _mav_put_uint8_t(buf, 45, battery_function); + _mav_put_uint8_t(buf, 46, type); + _mav_put_uint8_t(buf, 47, state_of_health); + _mav_put_uint8_t(buf, 48, cells_in_series); + _mav_put_char_array(buf, 49, manufacture_date, 9); + _mav_put_char_array(buf, 58, serial_number, 32); + _mav_put_char_array(buf, 90, name, 50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_INFO, buf, MAVLINK_MSG_ID_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_BATTERY_INFO_LEN, MAVLINK_MSG_ID_BATTERY_INFO_CRC); +#else + mavlink_battery_info_t packet; + packet.discharge_minimum_voltage = discharge_minimum_voltage; + packet.charging_minimum_voltage = charging_minimum_voltage; + packet.resting_minimum_voltage = resting_minimum_voltage; + packet.charging_maximum_voltage = charging_maximum_voltage; + packet.charging_maximum_current = charging_maximum_current; + packet.nominal_voltage = nominal_voltage; + packet.discharge_maximum_current = discharge_maximum_current; + packet.discharge_maximum_burst_current = discharge_maximum_burst_current; + packet.design_capacity = design_capacity; + packet.full_charge_capacity = full_charge_capacity; + packet.cycle_count = cycle_count; + packet.weight = weight; + packet.id = id; + packet.battery_function = battery_function; + packet.type = type; + packet.state_of_health = state_of_health; + packet.cells_in_series = cells_in_series; + mav_array_assign_char(packet.manufacture_date, manufacture_date, 9); + mav_array_assign_char(packet.serial_number, serial_number, 32); + mav_array_assign_char(packet.name, name, 50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_INFO, (const char *)&packet, MAVLINK_MSG_ID_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_BATTERY_INFO_LEN, MAVLINK_MSG_ID_BATTERY_INFO_CRC); +#endif +} + +/** + * @brief Send a battery_info message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_battery_info_send_struct(mavlink_channel_t chan, const mavlink_battery_info_t* battery_info) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_battery_info_send(chan, battery_info->id, battery_info->battery_function, battery_info->type, battery_info->state_of_health, battery_info->cells_in_series, battery_info->cycle_count, battery_info->weight, battery_info->discharge_minimum_voltage, battery_info->charging_minimum_voltage, battery_info->resting_minimum_voltage, battery_info->charging_maximum_voltage, battery_info->charging_maximum_current, battery_info->nominal_voltage, battery_info->discharge_maximum_current, battery_info->discharge_maximum_burst_current, battery_info->design_capacity, battery_info->full_charge_capacity, battery_info->manufacture_date, battery_info->serial_number, battery_info->name); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_INFO, (const char *)battery_info, MAVLINK_MSG_ID_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_BATTERY_INFO_LEN, MAVLINK_MSG_ID_BATTERY_INFO_CRC); +#endif +} + +#if MAVLINK_MSG_ID_BATTERY_INFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_battery_info_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, uint8_t state_of_health, uint8_t cells_in_series, uint16_t cycle_count, uint16_t weight, float discharge_minimum_voltage, float charging_minimum_voltage, float resting_minimum_voltage, float charging_maximum_voltage, float charging_maximum_current, float nominal_voltage, float discharge_maximum_current, float discharge_maximum_burst_current, float design_capacity, float full_charge_capacity, const char *manufacture_date, const char *serial_number, const char *name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, discharge_minimum_voltage); + _mav_put_float(buf, 4, charging_minimum_voltage); + _mav_put_float(buf, 8, resting_minimum_voltage); + _mav_put_float(buf, 12, charging_maximum_voltage); + _mav_put_float(buf, 16, charging_maximum_current); + _mav_put_float(buf, 20, nominal_voltage); + _mav_put_float(buf, 24, discharge_maximum_current); + _mav_put_float(buf, 28, discharge_maximum_burst_current); + _mav_put_float(buf, 32, design_capacity); + _mav_put_float(buf, 36, full_charge_capacity); + _mav_put_uint16_t(buf, 40, cycle_count); + _mav_put_uint16_t(buf, 42, weight); + _mav_put_uint8_t(buf, 44, id); + _mav_put_uint8_t(buf, 45, battery_function); + _mav_put_uint8_t(buf, 46, type); + _mav_put_uint8_t(buf, 47, state_of_health); + _mav_put_uint8_t(buf, 48, cells_in_series); + _mav_put_char_array(buf, 49, manufacture_date, 9); + _mav_put_char_array(buf, 58, serial_number, 32); + _mav_put_char_array(buf, 90, name, 50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_INFO, buf, MAVLINK_MSG_ID_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_BATTERY_INFO_LEN, MAVLINK_MSG_ID_BATTERY_INFO_CRC); +#else + mavlink_battery_info_t *packet = (mavlink_battery_info_t *)msgbuf; + packet->discharge_minimum_voltage = discharge_minimum_voltage; + packet->charging_minimum_voltage = charging_minimum_voltage; + packet->resting_minimum_voltage = resting_minimum_voltage; + packet->charging_maximum_voltage = charging_maximum_voltage; + packet->charging_maximum_current = charging_maximum_current; + packet->nominal_voltage = nominal_voltage; + packet->discharge_maximum_current = discharge_maximum_current; + packet->discharge_maximum_burst_current = discharge_maximum_burst_current; + packet->design_capacity = design_capacity; + packet->full_charge_capacity = full_charge_capacity; + packet->cycle_count = cycle_count; + packet->weight = weight; + packet->id = id; + packet->battery_function = battery_function; + packet->type = type; + packet->state_of_health = state_of_health; + packet->cells_in_series = cells_in_series; + mav_array_assign_char(packet->manufacture_date, manufacture_date, 9); + mav_array_assign_char(packet->serial_number, serial_number, 32); + mav_array_assign_char(packet->name, name, 50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_INFO, (const char *)packet, MAVLINK_MSG_ID_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_BATTERY_INFO_LEN, MAVLINK_MSG_ID_BATTERY_INFO_CRC); +#endif +} +#endif + +#endif + +// MESSAGE BATTERY_INFO UNPACKING + + +/** + * @brief Get field id from battery_info message + * + * @return Battery ID + */ +static inline uint8_t mavlink_msg_battery_info_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 44); +} + +/** + * @brief Get field battery_function from battery_info message + * + * @return Function of the battery. + */ +static inline uint8_t mavlink_msg_battery_info_get_battery_function(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 45); +} + +/** + * @brief Get field type from battery_info message + * + * @return Type (chemistry) of the battery. + */ +static inline uint8_t mavlink_msg_battery_info_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 46); +} + +/** + * @brief Get field state_of_health from battery_info message + * + * @return [%] State of Health (SOH) estimate. Typically 100% at the time of manufacture and will decrease over time and use. -1: field not provided. + */ +static inline uint8_t mavlink_msg_battery_info_get_state_of_health(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 47); +} + +/** + * @brief Get field cells_in_series from battery_info message + * + * @return Number of battery cells in series. 0: field not provided. + */ +static inline uint8_t mavlink_msg_battery_info_get_cells_in_series(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 48); +} + +/** + * @brief Get field cycle_count from battery_info message + * + * @return Lifetime count of the number of charge/discharge cycles (https://en.wikipedia.org/wiki/Charge_cycle). UINT16_MAX: field not provided. + */ +static inline uint16_t mavlink_msg_battery_info_get_cycle_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 40); +} + +/** + * @brief Get field weight from battery_info message + * + * @return [g] Battery weight. 0: field not provided. + */ +static inline uint16_t mavlink_msg_battery_info_get_weight(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 42); +} + +/** + * @brief Get field discharge_minimum_voltage from battery_info message + * + * @return [V] Minimum per-cell voltage when discharging. 0: field not provided. + */ +static inline float mavlink_msg_battery_info_get_discharge_minimum_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field charging_minimum_voltage from battery_info message + * + * @return [V] Minimum per-cell voltage when charging. 0: field not provided. + */ +static inline float mavlink_msg_battery_info_get_charging_minimum_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field resting_minimum_voltage from battery_info message + * + * @return [V] Minimum per-cell voltage when resting. 0: field not provided. + */ +static inline float mavlink_msg_battery_info_get_resting_minimum_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field charging_maximum_voltage from battery_info message + * + * @return [V] Maximum per-cell voltage when charged. 0: field not provided. + */ +static inline float mavlink_msg_battery_info_get_charging_maximum_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field charging_maximum_current from battery_info message + * + * @return [A] Maximum pack continuous charge current. 0: field not provided. + */ +static inline float mavlink_msg_battery_info_get_charging_maximum_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field nominal_voltage from battery_info message + * + * @return [V] Battery nominal voltage. Used for conversion between Wh and Ah. 0: field not provided. + */ +static inline float mavlink_msg_battery_info_get_nominal_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field discharge_maximum_current from battery_info message + * + * @return [A] Maximum pack discharge current. 0: field not provided. + */ +static inline float mavlink_msg_battery_info_get_discharge_maximum_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field discharge_maximum_burst_current from battery_info message + * + * @return [A] Maximum pack discharge burst current. 0: field not provided. + */ +static inline float mavlink_msg_battery_info_get_discharge_maximum_burst_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field design_capacity from battery_info message + * + * @return [Ah] Fully charged design capacity. 0: field not provided. + */ +static inline float mavlink_msg_battery_info_get_design_capacity(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field full_charge_capacity from battery_info message + * + * @return [Ah] Predicted battery capacity when fully charged (accounting for battery degradation). NAN: field not provided. + */ +static inline float mavlink_msg_battery_info_get_full_charge_capacity(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field manufacture_date from battery_info message + * + * @return Manufacture date (DDMMYYYY) in ASCII characters, 0 terminated. All 0: field not provided. + */ +static inline uint16_t mavlink_msg_battery_info_get_manufacture_date(const mavlink_message_t* msg, char *manufacture_date) +{ + return _MAV_RETURN_char_array(msg, manufacture_date, 9, 49); +} + +/** + * @brief Get field serial_number from battery_info message + * + * @return Serial number in ASCII characters, 0 terminated. All 0: field not provided. + */ +static inline uint16_t mavlink_msg_battery_info_get_serial_number(const mavlink_message_t* msg, char *serial_number) +{ + return _MAV_RETURN_char_array(msg, serial_number, 32, 58); +} + +/** + * @brief Get field name from battery_info message + * + * @return Battery device name. Formatted as manufacturer name then product name, separated with an underscore (in ASCII characters), 0 terminated. All 0: field not provided. + */ +static inline uint16_t mavlink_msg_battery_info_get_name(const mavlink_message_t* msg, char *name) +{ + return _MAV_RETURN_char_array(msg, name, 50, 90); +} + +/** + * @brief Decode a battery_info message into a struct + * + * @param msg The message to decode + * @param battery_info C-struct to decode the message contents into + */ +static inline void mavlink_msg_battery_info_decode(const mavlink_message_t* msg, mavlink_battery_info_t* battery_info) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + battery_info->discharge_minimum_voltage = mavlink_msg_battery_info_get_discharge_minimum_voltage(msg); + battery_info->charging_minimum_voltage = mavlink_msg_battery_info_get_charging_minimum_voltage(msg); + battery_info->resting_minimum_voltage = mavlink_msg_battery_info_get_resting_minimum_voltage(msg); + battery_info->charging_maximum_voltage = mavlink_msg_battery_info_get_charging_maximum_voltage(msg); + battery_info->charging_maximum_current = mavlink_msg_battery_info_get_charging_maximum_current(msg); + battery_info->nominal_voltage = mavlink_msg_battery_info_get_nominal_voltage(msg); + battery_info->discharge_maximum_current = mavlink_msg_battery_info_get_discharge_maximum_current(msg); + battery_info->discharge_maximum_burst_current = mavlink_msg_battery_info_get_discharge_maximum_burst_current(msg); + battery_info->design_capacity = mavlink_msg_battery_info_get_design_capacity(msg); + battery_info->full_charge_capacity = mavlink_msg_battery_info_get_full_charge_capacity(msg); + battery_info->cycle_count = mavlink_msg_battery_info_get_cycle_count(msg); + battery_info->weight = mavlink_msg_battery_info_get_weight(msg); + battery_info->id = mavlink_msg_battery_info_get_id(msg); + battery_info->battery_function = mavlink_msg_battery_info_get_battery_function(msg); + battery_info->type = mavlink_msg_battery_info_get_type(msg); + battery_info->state_of_health = mavlink_msg_battery_info_get_state_of_health(msg); + battery_info->cells_in_series = mavlink_msg_battery_info_get_cells_in_series(msg); + mavlink_msg_battery_info_get_manufacture_date(msg, battery_info->manufacture_date); + mavlink_msg_battery_info_get_serial_number(msg, battery_info->serial_number); + mavlink_msg_battery_info_get_name(msg, battery_info->name); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_BATTERY_INFO_LEN? msg->len : MAVLINK_MSG_ID_BATTERY_INFO_LEN; + memset(battery_info, 0, MAVLINK_MSG_ID_BATTERY_INFO_LEN); + memcpy(battery_info, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_battery_status.h b/lib/main/MAVLink/common/mavlink_msg_battery_status.h index 084b2a38860..aa751206411 100755 --- a/lib/main/MAVLink/common/mavlink_msg_battery_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_battery_status.h @@ -100,6 +100,72 @@ typedef struct __mavlink_battery_status_t { static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining, int32_t time_remaining, uint8_t charge_state, const uint16_t *voltages_ext, uint8_t mode, uint32_t fault_bitmask) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN]; + _mav_put_int32_t(buf, 0, current_consumed); + _mav_put_int32_t(buf, 4, energy_consumed); + _mav_put_int16_t(buf, 8, temperature); + _mav_put_int16_t(buf, 30, current_battery); + _mav_put_uint8_t(buf, 32, id); + _mav_put_uint8_t(buf, 33, battery_function); + _mav_put_uint8_t(buf, 34, type); + _mav_put_int8_t(buf, 35, battery_remaining); + _mav_put_int32_t(buf, 36, time_remaining); + _mav_put_uint8_t(buf, 40, charge_state); + _mav_put_uint8_t(buf, 49, mode); + _mav_put_uint32_t(buf, 50, fault_bitmask); + _mav_put_uint16_t_array(buf, 10, voltages, 10); + _mav_put_uint16_t_array(buf, 41, voltages_ext, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#else + mavlink_battery_status_t packet; + packet.current_consumed = current_consumed; + packet.energy_consumed = energy_consumed; + packet.temperature = temperature; + packet.current_battery = current_battery; + packet.id = id; + packet.battery_function = battery_function; + packet.type = type; + packet.battery_remaining = battery_remaining; + packet.time_remaining = time_remaining; + packet.charge_state = charge_state; + packet.mode = mode; + packet.fault_bitmask = fault_bitmask; + mav_array_assign_uint16_t(packet.voltages, voltages, 10); + mav_array_assign_uint16_t(packet.voltages_ext, voltages_ext, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_BATTERY_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); +} + +/** + * @brief Pack a battery_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param id Battery ID + * @param battery_function Function of the battery + * @param type Type (chemistry) of the battery + * @param temperature [cdegC] Temperature of the battery. INT16_MAX for unknown temperature. + * @param voltages [mV] Battery voltage of cells 1 to 10 (see voltages_ext for cells 11-14). Cells in this field above the valid cell count for this battery should have the UINT16_MAX value. If individual cell voltages are unknown or not measured for this battery, then the overall battery voltage should be filled in cell 0, with all others set to UINT16_MAX. If the voltage of the battery is greater than (UINT16_MAX - 1), then cell 0 should be set to (UINT16_MAX - 1), and cell 1 to the remaining voltage. This can be extended to multiple cells if the total voltage is greater than 2 * (UINT16_MAX - 1). + * @param current_battery [cA] Battery current, -1: autopilot does not measure the current + * @param current_consumed [mAh] Consumed charge, -1: autopilot does not provide consumption estimate + * @param energy_consumed [hJ] Consumed energy, -1: autopilot does not provide energy consumption estimate + * @param battery_remaining [%] Remaining battery energy. Values: [0-100], -1: autopilot does not estimate the remaining battery. + * @param time_remaining [s] Remaining battery time, 0: autopilot does not provide remaining battery time estimate + * @param charge_state State for extent of discharge, provided by autopilot for warning or external reactions + * @param voltages_ext [mV] Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead. + * @param mode Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode. + * @param fault_bitmask Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_battery_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining, int32_t time_remaining, uint8_t charge_state, const uint16_t *voltages_ext, uint8_t mode, uint32_t fault_bitmask) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN]; _mav_put_int32_t(buf, 0, current_consumed); @@ -137,7 +203,11 @@ static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_ #endif msg->msgid = MAVLINK_MSG_ID_BATTERY_STATUS; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#endif } /** @@ -197,8 +267,8 @@ static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, u packet.charge_state = charge_state; packet.mode = mode; packet.fault_bitmask = fault_bitmask; - mav_array_memcpy(packet.voltages, voltages, sizeof(uint16_t)*10); - mav_array_memcpy(packet.voltages_ext, voltages_ext, sizeof(uint16_t)*4); + mav_array_assign_uint16_t(packet.voltages, voltages, 10); + mav_array_assign_uint16_t(packet.voltages_ext, voltages_ext, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); #endif @@ -233,6 +303,20 @@ static inline uint16_t mavlink_msg_battery_status_encode_chan(uint8_t system_id, return mavlink_msg_battery_status_pack_chan(system_id, component_id, chan, msg, battery_status->id, battery_status->battery_function, battery_status->type, battery_status->temperature, battery_status->voltages, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining, battery_status->time_remaining, battery_status->charge_state, battery_status->voltages_ext, battery_status->mode, battery_status->fault_bitmask); } +/** + * @brief Encode a battery_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param battery_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_battery_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status) +{ + return mavlink_msg_battery_status_pack_status(system_id, component_id, _status, msg, battery_status->id, battery_status->battery_function, battery_status->type, battery_status->temperature, battery_status->voltages, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining, battery_status->time_remaining, battery_status->charge_state, battery_status->voltages_ext, battery_status->mode, battery_status->fault_bitmask); +} + /** * @brief Send a battery_status message * @param chan MAVLink channel to send the message @@ -287,8 +371,8 @@ static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8 packet.charge_state = charge_state; packet.mode = mode; packet.fault_bitmask = fault_bitmask; - mav_array_memcpy(packet.voltages, voltages, sizeof(uint16_t)*10); - mav_array_memcpy(packet.voltages_ext, voltages_ext, sizeof(uint16_t)*4); + mav_array_assign_uint16_t(packet.voltages, voltages, 10); + mav_array_assign_uint16_t(packet.voltages_ext, voltages_ext, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)&packet, MAVLINK_MSG_ID_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); #endif } @@ -309,7 +393,7 @@ static inline void mavlink_msg_battery_status_send_struct(mavlink_channel_t chan #if MAVLINK_MSG_ID_BATTERY_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -348,8 +432,8 @@ static inline void mavlink_msg_battery_status_send_buf(mavlink_message_t *msgbuf packet->charge_state = charge_state; packet->mode = mode; packet->fault_bitmask = fault_bitmask; - mav_array_memcpy(packet->voltages, voltages, sizeof(uint16_t)*10); - mav_array_memcpy(packet->voltages_ext, voltages_ext, sizeof(uint16_t)*4); + mav_array_assign_uint16_t(packet->voltages, voltages, 10); + mav_array_assign_uint16_t(packet->voltages_ext, voltages_ext, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)packet, MAVLINK_MSG_ID_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_button_change.h b/lib/main/MAVLink/common/mavlink_msg_button_change.h index 831bc60fc36..d29988bc63f 100644 --- a/lib/main/MAVLink/common/mavlink_msg_button_change.h +++ b/lib/main/MAVLink/common/mavlink_msg_button_change.h @@ -75,6 +75,45 @@ static inline uint16_t mavlink_msg_button_change_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_CRC); } +/** + * @brief Pack a button_change message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param last_change_ms [ms] Time of last change of button state. + * @param state Bitmap for state of buttons. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_button_change_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint32_t last_change_ms, uint8_t state) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_BUTTON_CHANGE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, last_change_ms); + _mav_put_uint8_t(buf, 8, state); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN); +#else + mavlink_button_change_t packet; + packet.time_boot_ms = time_boot_ms; + packet.last_change_ms = last_change_ms; + packet.state = state; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_BUTTON_CHANGE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN); +#endif +} + /** * @brief Pack a button_change message on a channel * @param system_id ID of this system @@ -137,6 +176,20 @@ static inline uint16_t mavlink_msg_button_change_encode_chan(uint8_t system_id, return mavlink_msg_button_change_pack_chan(system_id, component_id, chan, msg, button_change->time_boot_ms, button_change->last_change_ms, button_change->state); } +/** + * @brief Encode a button_change struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param button_change C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_button_change_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_button_change_t* button_change) +{ + return mavlink_msg_button_change_pack_status(system_id, component_id, _status, msg, button_change->time_boot_ms, button_change->last_change_ms, button_change->state); +} + /** * @brief Send a button_change message * @param chan MAVLink channel to send the message @@ -182,7 +235,7 @@ static inline void mavlink_msg_button_change_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_BUTTON_CHANGE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_capture_status.h b/lib/main/MAVLink/common/mavlink_msg_camera_capture_status.h index 8951db6a79f..35024f92b15 100644 --- a/lib/main/MAVLink/common/mavlink_msg_camera_capture_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_camera_capture_status.h @@ -7,16 +7,17 @@ MAVPACKED( typedef struct __mavlink_camera_capture_status_t { uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ float image_interval; /*< [s] Image capture interval*/ - uint32_t recording_time_ms; /*< [ms] Time since recording started*/ + uint32_t recording_time_ms; /*< [ms] Elapsed time since recording started (0: Not supported/available). A GCS should compute recording time and use non-zero values of this field to correct any discrepancy.*/ float available_capacity; /*< [MiB] Available storage capacity.*/ uint8_t image_status; /*< Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress)*/ uint8_t video_status; /*< Current status of video capturing (0: idle, 1: capture in progress)*/ int32_t image_count; /*< Total number of images captured ('forever', or until reset using MAV_CMD_STORAGE_FORMAT).*/ + uint8_t camera_device_id; /*< Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id).*/ }) mavlink_camera_capture_status_t; -#define MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN 22 +#define MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN 23 #define MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN 18 -#define MAVLINK_MSG_ID_262_LEN 22 +#define MAVLINK_MSG_ID_262_LEN 23 #define MAVLINK_MSG_ID_262_MIN_LEN 18 #define MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC 12 @@ -28,7 +29,7 @@ typedef struct __mavlink_camera_capture_status_t { #define MAVLINK_MESSAGE_INFO_CAMERA_CAPTURE_STATUS { \ 262, \ "CAMERA_CAPTURE_STATUS", \ - 7, \ + 8, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_capture_status_t, time_boot_ms) }, \ { "image_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_camera_capture_status_t, image_status) }, \ { "video_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_camera_capture_status_t, video_status) }, \ @@ -36,12 +37,13 @@ typedef struct __mavlink_camera_capture_status_t { { "recording_time_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_camera_capture_status_t, recording_time_ms) }, \ { "available_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_capture_status_t, available_capacity) }, \ { "image_count", NULL, MAVLINK_TYPE_INT32_T, 0, 18, offsetof(mavlink_camera_capture_status_t, image_count) }, \ + { "camera_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_camera_capture_status_t, camera_device_id) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_CAMERA_CAPTURE_STATUS { \ "CAMERA_CAPTURE_STATUS", \ - 7, \ + 8, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_capture_status_t, time_boot_ms) }, \ { "image_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_camera_capture_status_t, image_status) }, \ { "video_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_camera_capture_status_t, video_status) }, \ @@ -49,6 +51,7 @@ typedef struct __mavlink_camera_capture_status_t { { "recording_time_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_camera_capture_status_t, recording_time_ms) }, \ { "available_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_capture_status_t, available_capacity) }, \ { "image_count", NULL, MAVLINK_TYPE_INT32_T, 0, 18, offsetof(mavlink_camera_capture_status_t, image_count) }, \ + { "camera_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_camera_capture_status_t, camera_device_id) }, \ } \ } #endif @@ -63,13 +66,14 @@ typedef struct __mavlink_camera_capture_status_t { * @param image_status Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress) * @param video_status Current status of video capturing (0: idle, 1: capture in progress) * @param image_interval [s] Image capture interval - * @param recording_time_ms [ms] Time since recording started + * @param recording_time_ms [ms] Elapsed time since recording started (0: Not supported/available). A GCS should compute recording time and use non-zero values of this field to correct any discrepancy. * @param available_capacity [MiB] Available storage capacity. * @param image_count Total number of images captured ('forever', or until reset using MAV_CMD_STORAGE_FORMAT). + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_camera_capture_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, uint8_t image_status, uint8_t video_status, float image_interval, uint32_t recording_time_ms, float available_capacity, int32_t image_count) + uint32_t time_boot_ms, uint8_t image_status, uint8_t video_status, float image_interval, uint32_t recording_time_ms, float available_capacity, int32_t image_count, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN]; @@ -80,6 +84,7 @@ static inline uint16_t mavlink_msg_camera_capture_status_pack(uint8_t system_id, _mav_put_uint8_t(buf, 16, image_status); _mav_put_uint8_t(buf, 17, video_status); _mav_put_int32_t(buf, 18, image_count); + _mav_put_uint8_t(buf, 22, camera_device_id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN); #else @@ -91,6 +96,7 @@ static inline uint16_t mavlink_msg_camera_capture_status_pack(uint8_t system_id, packet.image_status = image_status; packet.video_status = video_status; packet.image_count = image_count; + packet.camera_device_id = camera_device_id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN); #endif @@ -99,6 +105,60 @@ static inline uint16_t mavlink_msg_camera_capture_status_pack(uint8_t system_id, return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); } +/** + * @brief Pack a camera_capture_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param image_status Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress) + * @param video_status Current status of video capturing (0: idle, 1: capture in progress) + * @param image_interval [s] Image capture interval + * @param recording_time_ms [ms] Elapsed time since recording started (0: Not supported/available). A GCS should compute recording time and use non-zero values of this field to correct any discrepancy. + * @param available_capacity [MiB] Available storage capacity. + * @param image_count Total number of images captured ('forever', or until reset using MAV_CMD_STORAGE_FORMAT). + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_capture_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t image_status, uint8_t video_status, float image_interval, uint32_t recording_time_ms, float available_capacity, int32_t image_count, uint8_t camera_device_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, image_interval); + _mav_put_uint32_t(buf, 8, recording_time_ms); + _mav_put_float(buf, 12, available_capacity); + _mav_put_uint8_t(buf, 16, image_status); + _mav_put_uint8_t(buf, 17, video_status); + _mav_put_int32_t(buf, 18, image_count); + _mav_put_uint8_t(buf, 22, camera_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN); +#else + mavlink_camera_capture_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.image_interval = image_interval; + packet.recording_time_ms = recording_time_ms; + packet.available_capacity = available_capacity; + packet.image_status = image_status; + packet.video_status = video_status; + packet.image_count = image_count; + packet.camera_device_id = camera_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN); +#endif +} + /** * @brief Pack a camera_capture_status message on a channel * @param system_id ID of this system @@ -109,14 +169,15 @@ static inline uint16_t mavlink_msg_camera_capture_status_pack(uint8_t system_id, * @param image_status Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress) * @param video_status Current status of video capturing (0: idle, 1: capture in progress) * @param image_interval [s] Image capture interval - * @param recording_time_ms [ms] Time since recording started + * @param recording_time_ms [ms] Elapsed time since recording started (0: Not supported/available). A GCS should compute recording time and use non-zero values of this field to correct any discrepancy. * @param available_capacity [MiB] Available storage capacity. * @param image_count Total number of images captured ('forever', or until reset using MAV_CMD_STORAGE_FORMAT). + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_camera_capture_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,uint8_t image_status,uint8_t video_status,float image_interval,uint32_t recording_time_ms,float available_capacity,int32_t image_count) + uint32_t time_boot_ms,uint8_t image_status,uint8_t video_status,float image_interval,uint32_t recording_time_ms,float available_capacity,int32_t image_count,uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN]; @@ -127,6 +188,7 @@ static inline uint16_t mavlink_msg_camera_capture_status_pack_chan(uint8_t syste _mav_put_uint8_t(buf, 16, image_status); _mav_put_uint8_t(buf, 17, video_status); _mav_put_int32_t(buf, 18, image_count); + _mav_put_uint8_t(buf, 22, camera_device_id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN); #else @@ -138,6 +200,7 @@ static inline uint16_t mavlink_msg_camera_capture_status_pack_chan(uint8_t syste packet.image_status = image_status; packet.video_status = video_status; packet.image_count = image_count; + packet.camera_device_id = camera_device_id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN); #endif @@ -156,7 +219,7 @@ static inline uint16_t mavlink_msg_camera_capture_status_pack_chan(uint8_t syste */ static inline uint16_t mavlink_msg_camera_capture_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_capture_status_t* camera_capture_status) { - return mavlink_msg_camera_capture_status_pack(system_id, component_id, msg, camera_capture_status->time_boot_ms, camera_capture_status->image_status, camera_capture_status->video_status, camera_capture_status->image_interval, camera_capture_status->recording_time_ms, camera_capture_status->available_capacity, camera_capture_status->image_count); + return mavlink_msg_camera_capture_status_pack(system_id, component_id, msg, camera_capture_status->time_boot_ms, camera_capture_status->image_status, camera_capture_status->video_status, camera_capture_status->image_interval, camera_capture_status->recording_time_ms, camera_capture_status->available_capacity, camera_capture_status->image_count, camera_capture_status->camera_device_id); } /** @@ -170,7 +233,21 @@ static inline uint16_t mavlink_msg_camera_capture_status_encode(uint8_t system_i */ static inline uint16_t mavlink_msg_camera_capture_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_capture_status_t* camera_capture_status) { - return mavlink_msg_camera_capture_status_pack_chan(system_id, component_id, chan, msg, camera_capture_status->time_boot_ms, camera_capture_status->image_status, camera_capture_status->video_status, camera_capture_status->image_interval, camera_capture_status->recording_time_ms, camera_capture_status->available_capacity, camera_capture_status->image_count); + return mavlink_msg_camera_capture_status_pack_chan(system_id, component_id, chan, msg, camera_capture_status->time_boot_ms, camera_capture_status->image_status, camera_capture_status->video_status, camera_capture_status->image_interval, camera_capture_status->recording_time_ms, camera_capture_status->available_capacity, camera_capture_status->image_count, camera_capture_status->camera_device_id); +} + +/** + * @brief Encode a camera_capture_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param camera_capture_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_capture_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_camera_capture_status_t* camera_capture_status) +{ + return mavlink_msg_camera_capture_status_pack_status(system_id, component_id, _status, msg, camera_capture_status->time_boot_ms, camera_capture_status->image_status, camera_capture_status->video_status, camera_capture_status->image_interval, camera_capture_status->recording_time_ms, camera_capture_status->available_capacity, camera_capture_status->image_count, camera_capture_status->camera_device_id); } /** @@ -181,13 +258,14 @@ static inline uint16_t mavlink_msg_camera_capture_status_encode_chan(uint8_t sys * @param image_status Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress) * @param video_status Current status of video capturing (0: idle, 1: capture in progress) * @param image_interval [s] Image capture interval - * @param recording_time_ms [ms] Time since recording started + * @param recording_time_ms [ms] Elapsed time since recording started (0: Not supported/available). A GCS should compute recording time and use non-zero values of this field to correct any discrepancy. * @param available_capacity [MiB] Available storage capacity. * @param image_count Total number of images captured ('forever', or until reset using MAV_CMD_STORAGE_FORMAT). + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_camera_capture_status_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t image_status, uint8_t video_status, float image_interval, uint32_t recording_time_ms, float available_capacity, int32_t image_count) +static inline void mavlink_msg_camera_capture_status_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t image_status, uint8_t video_status, float image_interval, uint32_t recording_time_ms, float available_capacity, int32_t image_count, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN]; @@ -198,6 +276,7 @@ static inline void mavlink_msg_camera_capture_status_send(mavlink_channel_t chan _mav_put_uint8_t(buf, 16, image_status); _mav_put_uint8_t(buf, 17, video_status); _mav_put_int32_t(buf, 18, image_count); + _mav_put_uint8_t(buf, 22, camera_device_id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, buf, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); #else @@ -209,6 +288,7 @@ static inline void mavlink_msg_camera_capture_status_send(mavlink_channel_t chan packet.image_status = image_status; packet.video_status = video_status; packet.image_count = image_count; + packet.camera_device_id = camera_device_id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); #endif @@ -222,7 +302,7 @@ static inline void mavlink_msg_camera_capture_status_send(mavlink_channel_t chan static inline void mavlink_msg_camera_capture_status_send_struct(mavlink_channel_t chan, const mavlink_camera_capture_status_t* camera_capture_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_camera_capture_status_send(chan, camera_capture_status->time_boot_ms, camera_capture_status->image_status, camera_capture_status->video_status, camera_capture_status->image_interval, camera_capture_status->recording_time_ms, camera_capture_status->available_capacity, camera_capture_status->image_count); + mavlink_msg_camera_capture_status_send(chan, camera_capture_status->time_boot_ms, camera_capture_status->image_status, camera_capture_status->video_status, camera_capture_status->image_interval, camera_capture_status->recording_time_ms, camera_capture_status->available_capacity, camera_capture_status->image_count, camera_capture_status->camera_device_id); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, (const char *)camera_capture_status, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); #endif @@ -230,13 +310,13 @@ static inline void mavlink_msg_camera_capture_status_send_struct(mavlink_channel #if MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_camera_capture_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t image_status, uint8_t video_status, float image_interval, uint32_t recording_time_ms, float available_capacity, int32_t image_count) +static inline void mavlink_msg_camera_capture_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t image_status, uint8_t video_status, float image_interval, uint32_t recording_time_ms, float available_capacity, int32_t image_count, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -247,6 +327,7 @@ static inline void mavlink_msg_camera_capture_status_send_buf(mavlink_message_t _mav_put_uint8_t(buf, 16, image_status); _mav_put_uint8_t(buf, 17, video_status); _mav_put_int32_t(buf, 18, image_count); + _mav_put_uint8_t(buf, 22, camera_device_id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, buf, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); #else @@ -258,6 +339,7 @@ static inline void mavlink_msg_camera_capture_status_send_buf(mavlink_message_t packet->image_status = image_status; packet->video_status = video_status; packet->image_count = image_count; + packet->camera_device_id = camera_device_id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, (const char *)packet, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); #endif @@ -312,7 +394,7 @@ static inline float mavlink_msg_camera_capture_status_get_image_interval(const m /** * @brief Get field recording_time_ms from camera_capture_status message * - * @return [ms] Time since recording started + * @return [ms] Elapsed time since recording started (0: Not supported/available). A GCS should compute recording time and use non-zero values of this field to correct any discrepancy. */ static inline uint32_t mavlink_msg_camera_capture_status_get_recording_time_ms(const mavlink_message_t* msg) { @@ -339,6 +421,16 @@ static inline int32_t mavlink_msg_camera_capture_status_get_image_count(const ma return _MAV_RETURN_int32_t(msg, 18); } +/** + * @brief Get field camera_device_id from camera_capture_status message + * + * @return Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + */ +static inline uint8_t mavlink_msg_camera_capture_status_get_camera_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 22); +} + /** * @brief Decode a camera_capture_status message into a struct * @@ -355,6 +447,7 @@ static inline void mavlink_msg_camera_capture_status_decode(const mavlink_messag camera_capture_status->image_status = mavlink_msg_camera_capture_status_get_image_status(msg); camera_capture_status->video_status = mavlink_msg_camera_capture_status_get_video_status(msg); camera_capture_status->image_count = mavlink_msg_camera_capture_status_get_image_count(msg); + camera_capture_status->camera_device_id = mavlink_msg_camera_capture_status_get_camera_device_id(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN; memset(camera_capture_status, 0, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_fov_status.h b/lib/main/MAVLink/common/mavlink_msg_camera_fov_status.h index 6378e56aa83..ee8a06dc1c1 100644 --- a/lib/main/MAVLink/common/mavlink_msg_camera_fov_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_camera_fov_status.h @@ -15,11 +15,12 @@ typedef struct __mavlink_camera_fov_status_t { float q[4]; /*< Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0)*/ float hfov; /*< [deg] Horizontal field of view (NaN if unknown).*/ float vfov; /*< [deg] Vertical field of view (NaN if unknown).*/ + uint8_t camera_device_id; /*< Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id).*/ } mavlink_camera_fov_status_t; -#define MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN 52 +#define MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN 53 #define MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN 52 -#define MAVLINK_MSG_ID_271_LEN 52 +#define MAVLINK_MSG_ID_271_LEN 53 #define MAVLINK_MSG_ID_271_MIN_LEN 52 #define MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC 22 @@ -31,7 +32,7 @@ typedef struct __mavlink_camera_fov_status_t { #define MAVLINK_MESSAGE_INFO_CAMERA_FOV_STATUS { \ 271, \ "CAMERA_FOV_STATUS", \ - 10, \ + 11, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_fov_status_t, time_boot_ms) }, \ { "lat_camera", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_camera_fov_status_t, lat_camera) }, \ { "lon_camera", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_camera_fov_status_t, lon_camera) }, \ @@ -42,12 +43,13 @@ typedef struct __mavlink_camera_fov_status_t { { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 28, offsetof(mavlink_camera_fov_status_t, q) }, \ { "hfov", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_camera_fov_status_t, hfov) }, \ { "vfov", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_camera_fov_status_t, vfov) }, \ + { "camera_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_camera_fov_status_t, camera_device_id) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_CAMERA_FOV_STATUS { \ "CAMERA_FOV_STATUS", \ - 10, \ + 11, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_fov_status_t, time_boot_ms) }, \ { "lat_camera", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_camera_fov_status_t, lat_camera) }, \ { "lon_camera", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_camera_fov_status_t, lon_camera) }, \ @@ -58,6 +60,7 @@ typedef struct __mavlink_camera_fov_status_t { { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 28, offsetof(mavlink_camera_fov_status_t, q) }, \ { "hfov", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_camera_fov_status_t, hfov) }, \ { "vfov", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_camera_fov_status_t, vfov) }, \ + { "camera_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_camera_fov_status_t, camera_device_id) }, \ } \ } #endif @@ -78,10 +81,11 @@ typedef struct __mavlink_camera_fov_status_t { * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param hfov [deg] Horizontal field of view (NaN if unknown). * @param vfov [deg] Vertical field of view (NaN if unknown). + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_camera_fov_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, int32_t lat_camera, int32_t lon_camera, int32_t alt_camera, int32_t lat_image, int32_t lon_image, int32_t alt_image, const float *q, float hfov, float vfov) + uint32_t time_boot_ms, int32_t lat_camera, int32_t lon_camera, int32_t alt_camera, int32_t lat_image, int32_t lon_image, int32_t alt_image, const float *q, float hfov, float vfov, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN]; @@ -94,6 +98,7 @@ static inline uint16_t mavlink_msg_camera_fov_status_pack(uint8_t system_id, uin _mav_put_int32_t(buf, 24, alt_image); _mav_put_float(buf, 44, hfov); _mav_put_float(buf, 48, vfov); + _mav_put_uint8_t(buf, 52, camera_device_id); _mav_put_float_array(buf, 28, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN); #else @@ -107,7 +112,8 @@ static inline uint16_t mavlink_msg_camera_fov_status_pack(uint8_t system_id, uin packet.alt_image = alt_image; packet.hfov = hfov; packet.vfov = vfov; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + packet.camera_device_id = camera_device_id; + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN); #endif @@ -115,6 +121,67 @@ static inline uint16_t mavlink_msg_camera_fov_status_pack(uint8_t system_id, uin return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); } +/** + * @brief Pack a camera_fov_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param lat_camera [degE7] Latitude of camera (INT32_MAX if unknown). + * @param lon_camera [degE7] Longitude of camera (INT32_MAX if unknown). + * @param alt_camera [mm] Altitude (MSL) of camera (INT32_MAX if unknown). + * @param lat_image [degE7] Latitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param lon_image [degE7] Longitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param alt_image [mm] Altitude (MSL) of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param hfov [deg] Horizontal field of view (NaN if unknown). + * @param vfov [deg] Vertical field of view (NaN if unknown). + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_fov_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, int32_t lat_camera, int32_t lon_camera, int32_t alt_camera, int32_t lat_image, int32_t lon_image, int32_t alt_image, const float *q, float hfov, float vfov, uint8_t camera_device_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_camera); + _mav_put_int32_t(buf, 8, lon_camera); + _mav_put_int32_t(buf, 12, alt_camera); + _mav_put_int32_t(buf, 16, lat_image); + _mav_put_int32_t(buf, 20, lon_image); + _mav_put_int32_t(buf, 24, alt_image); + _mav_put_float(buf, 44, hfov); + _mav_put_float(buf, 48, vfov); + _mav_put_uint8_t(buf, 52, camera_device_id); + _mav_put_float_array(buf, 28, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN); +#else + mavlink_camera_fov_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_camera = lat_camera; + packet.lon_camera = lon_camera; + packet.alt_camera = alt_camera; + packet.lat_image = lat_image; + packet.lon_image = lon_image; + packet.alt_image = alt_image; + packet.hfov = hfov; + packet.vfov = vfov; + packet.camera_device_id = camera_device_id; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_FOV_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN); +#endif +} + /** * @brief Pack a camera_fov_status message on a channel * @param system_id ID of this system @@ -131,11 +198,12 @@ static inline uint16_t mavlink_msg_camera_fov_status_pack(uint8_t system_id, uin * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param hfov [deg] Horizontal field of view (NaN if unknown). * @param vfov [deg] Vertical field of view (NaN if unknown). + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_camera_fov_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,int32_t lat_camera,int32_t lon_camera,int32_t alt_camera,int32_t lat_image,int32_t lon_image,int32_t alt_image,const float *q,float hfov,float vfov) + uint32_t time_boot_ms,int32_t lat_camera,int32_t lon_camera,int32_t alt_camera,int32_t lat_image,int32_t lon_image,int32_t alt_image,const float *q,float hfov,float vfov,uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN]; @@ -148,6 +216,7 @@ static inline uint16_t mavlink_msg_camera_fov_status_pack_chan(uint8_t system_id _mav_put_int32_t(buf, 24, alt_image); _mav_put_float(buf, 44, hfov); _mav_put_float(buf, 48, vfov); + _mav_put_uint8_t(buf, 52, camera_device_id); _mav_put_float_array(buf, 28, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN); #else @@ -161,7 +230,8 @@ static inline uint16_t mavlink_msg_camera_fov_status_pack_chan(uint8_t system_id packet.alt_image = alt_image; packet.hfov = hfov; packet.vfov = vfov; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + packet.camera_device_id = camera_device_id; + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN); #endif @@ -179,7 +249,7 @@ static inline uint16_t mavlink_msg_camera_fov_status_pack_chan(uint8_t system_id */ static inline uint16_t mavlink_msg_camera_fov_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_fov_status_t* camera_fov_status) { - return mavlink_msg_camera_fov_status_pack(system_id, component_id, msg, camera_fov_status->time_boot_ms, camera_fov_status->lat_camera, camera_fov_status->lon_camera, camera_fov_status->alt_camera, camera_fov_status->lat_image, camera_fov_status->lon_image, camera_fov_status->alt_image, camera_fov_status->q, camera_fov_status->hfov, camera_fov_status->vfov); + return mavlink_msg_camera_fov_status_pack(system_id, component_id, msg, camera_fov_status->time_boot_ms, camera_fov_status->lat_camera, camera_fov_status->lon_camera, camera_fov_status->alt_camera, camera_fov_status->lat_image, camera_fov_status->lon_image, camera_fov_status->alt_image, camera_fov_status->q, camera_fov_status->hfov, camera_fov_status->vfov, camera_fov_status->camera_device_id); } /** @@ -193,7 +263,21 @@ static inline uint16_t mavlink_msg_camera_fov_status_encode(uint8_t system_id, u */ static inline uint16_t mavlink_msg_camera_fov_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_fov_status_t* camera_fov_status) { - return mavlink_msg_camera_fov_status_pack_chan(system_id, component_id, chan, msg, camera_fov_status->time_boot_ms, camera_fov_status->lat_camera, camera_fov_status->lon_camera, camera_fov_status->alt_camera, camera_fov_status->lat_image, camera_fov_status->lon_image, camera_fov_status->alt_image, camera_fov_status->q, camera_fov_status->hfov, camera_fov_status->vfov); + return mavlink_msg_camera_fov_status_pack_chan(system_id, component_id, chan, msg, camera_fov_status->time_boot_ms, camera_fov_status->lat_camera, camera_fov_status->lon_camera, camera_fov_status->alt_camera, camera_fov_status->lat_image, camera_fov_status->lon_image, camera_fov_status->alt_image, camera_fov_status->q, camera_fov_status->hfov, camera_fov_status->vfov, camera_fov_status->camera_device_id); +} + +/** + * @brief Encode a camera_fov_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param camera_fov_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_fov_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_camera_fov_status_t* camera_fov_status) +{ + return mavlink_msg_camera_fov_status_pack_status(system_id, component_id, _status, msg, camera_fov_status->time_boot_ms, camera_fov_status->lat_camera, camera_fov_status->lon_camera, camera_fov_status->alt_camera, camera_fov_status->lat_image, camera_fov_status->lon_image, camera_fov_status->alt_image, camera_fov_status->q, camera_fov_status->hfov, camera_fov_status->vfov, camera_fov_status->camera_device_id); } /** @@ -210,10 +294,11 @@ static inline uint16_t mavlink_msg_camera_fov_status_encode_chan(uint8_t system_ * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param hfov [deg] Horizontal field of view (NaN if unknown). * @param vfov [deg] Vertical field of view (NaN if unknown). + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_camera_fov_status_send(mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat_camera, int32_t lon_camera, int32_t alt_camera, int32_t lat_image, int32_t lon_image, int32_t alt_image, const float *q, float hfov, float vfov) +static inline void mavlink_msg_camera_fov_status_send(mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat_camera, int32_t lon_camera, int32_t alt_camera, int32_t lat_image, int32_t lon_image, int32_t alt_image, const float *q, float hfov, float vfov, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN]; @@ -226,6 +311,7 @@ static inline void mavlink_msg_camera_fov_status_send(mavlink_channel_t chan, ui _mav_put_int32_t(buf, 24, alt_image); _mav_put_float(buf, 44, hfov); _mav_put_float(buf, 48, vfov); + _mav_put_uint8_t(buf, 52, camera_device_id); _mav_put_float_array(buf, 28, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FOV_STATUS, buf, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); #else @@ -239,7 +325,8 @@ static inline void mavlink_msg_camera_fov_status_send(mavlink_channel_t chan, ui packet.alt_image = alt_image; packet.hfov = hfov; packet.vfov = vfov; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + packet.camera_device_id = camera_device_id; + mav_array_assign_float(packet.q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FOV_STATUS, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); #endif } @@ -252,7 +339,7 @@ static inline void mavlink_msg_camera_fov_status_send(mavlink_channel_t chan, ui static inline void mavlink_msg_camera_fov_status_send_struct(mavlink_channel_t chan, const mavlink_camera_fov_status_t* camera_fov_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_camera_fov_status_send(chan, camera_fov_status->time_boot_ms, camera_fov_status->lat_camera, camera_fov_status->lon_camera, camera_fov_status->alt_camera, camera_fov_status->lat_image, camera_fov_status->lon_image, camera_fov_status->alt_image, camera_fov_status->q, camera_fov_status->hfov, camera_fov_status->vfov); + mavlink_msg_camera_fov_status_send(chan, camera_fov_status->time_boot_ms, camera_fov_status->lat_camera, camera_fov_status->lon_camera, camera_fov_status->alt_camera, camera_fov_status->lat_image, camera_fov_status->lon_image, camera_fov_status->alt_image, camera_fov_status->q, camera_fov_status->hfov, camera_fov_status->vfov, camera_fov_status->camera_device_id); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FOV_STATUS, (const char *)camera_fov_status, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); #endif @@ -260,13 +347,13 @@ static inline void mavlink_msg_camera_fov_status_send_struct(mavlink_channel_t c #if MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_camera_fov_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat_camera, int32_t lon_camera, int32_t alt_camera, int32_t lat_image, int32_t lon_image, int32_t alt_image, const float *q, float hfov, float vfov) +static inline void mavlink_msg_camera_fov_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat_camera, int32_t lon_camera, int32_t alt_camera, int32_t lat_image, int32_t lon_image, int32_t alt_image, const float *q, float hfov, float vfov, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -279,6 +366,7 @@ static inline void mavlink_msg_camera_fov_status_send_buf(mavlink_message_t *msg _mav_put_int32_t(buf, 24, alt_image); _mav_put_float(buf, 44, hfov); _mav_put_float(buf, 48, vfov); + _mav_put_uint8_t(buf, 52, camera_device_id); _mav_put_float_array(buf, 28, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FOV_STATUS, buf, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); #else @@ -292,7 +380,8 @@ static inline void mavlink_msg_camera_fov_status_send_buf(mavlink_message_t *msg packet->alt_image = alt_image; packet->hfov = hfov; packet->vfov = vfov; - mav_array_memcpy(packet->q, q, sizeof(float)*4); + packet->camera_device_id = camera_device_id; + mav_array_assign_float(packet->q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FOV_STATUS, (const char *)packet, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); #endif } @@ -403,6 +492,16 @@ static inline float mavlink_msg_camera_fov_status_get_vfov(const mavlink_message return _MAV_RETURN_float(msg, 48); } +/** + * @brief Get field camera_device_id from camera_fov_status message + * + * @return Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + */ +static inline uint8_t mavlink_msg_camera_fov_status_get_camera_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 52); +} + /** * @brief Decode a camera_fov_status message into a struct * @@ -422,6 +521,7 @@ static inline void mavlink_msg_camera_fov_status_decode(const mavlink_message_t* mavlink_msg_camera_fov_status_get_q(msg, camera_fov_status->q); camera_fov_status->hfov = mavlink_msg_camera_fov_status_get_hfov(msg); camera_fov_status->vfov = mavlink_msg_camera_fov_status_get_vfov(msg); + camera_fov_status->camera_device_id = mavlink_msg_camera_fov_status_get_camera_device_id(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN; memset(camera_fov_status, 0, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_image_captured.h b/lib/main/MAVLink/common/mavlink_msg_camera_image_captured.h index 01d772b62a7..0689fabdf72 100644 --- a/lib/main/MAVLink/common/mavlink_msg_camera_image_captured.h +++ b/lib/main/MAVLink/common/mavlink_msg_camera_image_captured.h @@ -13,8 +13,8 @@ typedef struct __mavlink_camera_image_captured_t { int32_t relative_alt; /*< [mm] Altitude above ground*/ float q[4]; /*< Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0)*/ int32_t image_index; /*< Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1)*/ - uint8_t camera_id; /*< Deprecated/unused. Component IDs are used to differentiate multiple cameras.*/ - int8_t capture_result; /*< Boolean indicating success (1) or failure (0) while capturing this image.*/ + uint8_t camera_id; /*< Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). Field name is usually camera_device_id.*/ + int8_t capture_result; /*< Image was captured successfully (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid.*/ char file_url[205]; /*< URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface.*/ } mavlink_camera_image_captured_t; @@ -74,20 +74,77 @@ typedef struct __mavlink_camera_image_captured_t { * * @param time_boot_ms [ms] Timestamp (time since system boot). * @param time_utc [us] Timestamp (time since UNIX epoch) in UTC. 0 for unknown. - * @param camera_id Deprecated/unused. Component IDs are used to differentiate multiple cameras. + * @param camera_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). Field name is usually camera_device_id. * @param lat [degE7] Latitude where image was taken * @param lon [degE7] Longitude where capture was taken * @param alt [mm] Altitude (MSL) where image was taken * @param relative_alt [mm] Altitude above ground * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param image_index Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1) - * @param capture_result Boolean indicating success (1) or failure (0) while capturing this image. + * @param capture_result Image was captured successfully (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid. * @param file_url URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_camera_image_captured_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t time_boot_ms, uint64_t time_utc, uint8_t camera_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, const float *q, int32_t image_index, int8_t capture_result, const char *file_url) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_int32_t(buf, 12, lat); + _mav_put_int32_t(buf, 16, lon); + _mav_put_int32_t(buf, 20, alt); + _mav_put_int32_t(buf, 24, relative_alt); + _mav_put_int32_t(buf, 44, image_index); + _mav_put_uint8_t(buf, 48, camera_id); + _mav_put_int8_t(buf, 49, capture_result); + _mav_put_float_array(buf, 28, q, 4); + _mav_put_char_array(buf, 50, file_url, 205); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN); +#else + mavlink_camera_image_captured_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.image_index = image_index; + packet.camera_id = camera_id; + packet.capture_result = capture_result; + mav_array_assign_float(packet.q, q, 4); + mav_array_assign_char(packet.file_url, file_url, 205); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); +} + +/** + * @brief Pack a camera_image_captured message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param time_utc [us] Timestamp (time since UNIX epoch) in UTC. 0 for unknown. + * @param camera_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). Field name is usually camera_device_id. + * @param lat [degE7] Latitude where image was taken + * @param lon [degE7] Longitude where capture was taken + * @param alt [mm] Altitude (MSL) where image was taken + * @param relative_alt [mm] Altitude above ground + * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param image_index Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1) + * @param capture_result Image was captured successfully (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid. + * @param file_url URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_image_captured_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint64_t time_utc, uint8_t camera_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, const float *q, int32_t image_index, int8_t capture_result, const char *file_url) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN]; _mav_put_uint64_t(buf, 0, time_utc); @@ -119,7 +176,11 @@ static inline uint16_t mavlink_msg_camera_image_captured_pack(uint8_t system_id, #endif msg->msgid = MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN); +#endif } /** @@ -130,14 +191,14 @@ static inline uint16_t mavlink_msg_camera_image_captured_pack(uint8_t system_id, * @param msg The MAVLink message to compress the data into * @param time_boot_ms [ms] Timestamp (time since system boot). * @param time_utc [us] Timestamp (time since UNIX epoch) in UTC. 0 for unknown. - * @param camera_id Deprecated/unused. Component IDs are used to differentiate multiple cameras. + * @param camera_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). Field name is usually camera_device_id. * @param lat [degE7] Latitude where image was taken * @param lon [degE7] Longitude where capture was taken * @param alt [mm] Altitude (MSL) where image was taken * @param relative_alt [mm] Altitude above ground * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param image_index Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1) - * @param capture_result Boolean indicating success (1) or failure (0) while capturing this image. + * @param capture_result Image was captured successfully (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid. * @param file_url URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -170,8 +231,8 @@ static inline uint16_t mavlink_msg_camera_image_captured_pack_chan(uint8_t syste packet.image_index = image_index; packet.camera_id = camera_id; packet.capture_result = capture_result; - mav_array_memcpy(packet.q, q, sizeof(float)*4); - mav_array_memcpy(packet.file_url, file_url, sizeof(char)*205); + mav_array_assign_float(packet.q, q, 4); + mav_array_assign_char(packet.file_url, file_url, 205); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN); #endif @@ -206,20 +267,34 @@ static inline uint16_t mavlink_msg_camera_image_captured_encode_chan(uint8_t sys return mavlink_msg_camera_image_captured_pack_chan(system_id, component_id, chan, msg, camera_image_captured->time_boot_ms, camera_image_captured->time_utc, camera_image_captured->camera_id, camera_image_captured->lat, camera_image_captured->lon, camera_image_captured->alt, camera_image_captured->relative_alt, camera_image_captured->q, camera_image_captured->image_index, camera_image_captured->capture_result, camera_image_captured->file_url); } +/** + * @brief Encode a camera_image_captured struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param camera_image_captured C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_image_captured_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_camera_image_captured_t* camera_image_captured) +{ + return mavlink_msg_camera_image_captured_pack_status(system_id, component_id, _status, msg, camera_image_captured->time_boot_ms, camera_image_captured->time_utc, camera_image_captured->camera_id, camera_image_captured->lat, camera_image_captured->lon, camera_image_captured->alt, camera_image_captured->relative_alt, camera_image_captured->q, camera_image_captured->image_index, camera_image_captured->capture_result, camera_image_captured->file_url); +} + /** * @brief Send a camera_image_captured message * @param chan MAVLink channel to send the message * * @param time_boot_ms [ms] Timestamp (time since system boot). * @param time_utc [us] Timestamp (time since UNIX epoch) in UTC. 0 for unknown. - * @param camera_id Deprecated/unused. Component IDs are used to differentiate multiple cameras. + * @param camera_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). Field name is usually camera_device_id. * @param lat [degE7] Latitude where image was taken * @param lon [degE7] Longitude where capture was taken * @param alt [mm] Altitude (MSL) where image was taken * @param relative_alt [mm] Altitude above ground * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param image_index Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1) - * @param capture_result Boolean indicating success (1) or failure (0) while capturing this image. + * @param capture_result Image was captured successfully (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid. * @param file_url URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -251,8 +326,8 @@ static inline void mavlink_msg_camera_image_captured_send(mavlink_channel_t chan packet.image_index = image_index; packet.camera_id = camera_id; packet.capture_result = capture_result; - mav_array_memcpy(packet.q, q, sizeof(float)*4); - mav_array_memcpy(packet.file_url, file_url, sizeof(char)*205); + mav_array_assign_float(packet.q, q, 4); + mav_array_assign_char(packet.file_url, file_url, 205); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); #endif } @@ -273,7 +348,7 @@ static inline void mavlink_msg_camera_image_captured_send_struct(mavlink_channel #if MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -306,8 +381,8 @@ static inline void mavlink_msg_camera_image_captured_send_buf(mavlink_message_t packet->image_index = image_index; packet->camera_id = camera_id; packet->capture_result = capture_result; - mav_array_memcpy(packet->q, q, sizeof(float)*4); - mav_array_memcpy(packet->file_url, file_url, sizeof(char)*205); + mav_array_assign_float(packet->q, q, 4); + mav_array_assign_char(packet->file_url, file_url, 205); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, (const char *)packet, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); #endif } @@ -341,7 +416,7 @@ static inline uint64_t mavlink_msg_camera_image_captured_get_time_utc(const mavl /** * @brief Get field camera_id from camera_image_captured message * - * @return Deprecated/unused. Component IDs are used to differentiate multiple cameras. + * @return Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). Field name is usually camera_device_id. */ static inline uint8_t mavlink_msg_camera_image_captured_get_camera_id(const mavlink_message_t* msg) { @@ -411,7 +486,7 @@ static inline int32_t mavlink_msg_camera_image_captured_get_image_index(const ma /** * @brief Get field capture_result from camera_image_captured message * - * @return Boolean indicating success (1) or failure (0) while capturing this image. + * @return Image was captured successfully (MAV_BOOL_TRUE). Values not equal to 0 or 1 are invalid. */ static inline int8_t mavlink_msg_camera_image_captured_get_capture_result(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_information.h b/lib/main/MAVLink/common/mavlink_msg_camera_information.h index 0ae20c4b3e4..bc876acf18b 100644 --- a/lib/main/MAVLink/common/mavlink_msg_camera_information.h +++ b/lib/main/MAVLink/common/mavlink_msg_camera_information.h @@ -6,23 +6,25 @@ typedef struct __mavlink_camera_information_t { uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ - uint32_t firmware_version; /*< Version of the camera firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff)*/ - float focal_length; /*< [mm] Focal length*/ - float sensor_size_h; /*< [mm] Image sensor size horizontal*/ - float sensor_size_v; /*< [mm] Image sensor size vertical*/ + uint32_t firmware_version; /*< Version of the camera firmware, encoded as: `(Dev & 0xff) << 24 \| (Patch & 0xff) << 16 \| (Minor & 0xff) << 8 \| (Major & 0xff)`. Use 0 if not known.*/ + float focal_length; /*< [mm] Focal length. Use NaN if not known.*/ + float sensor_size_h; /*< [mm] Image sensor size horizontal. Use NaN if not known.*/ + float sensor_size_v; /*< [mm] Image sensor size vertical. Use NaN if not known.*/ uint32_t flags; /*< Bitmap of camera capability flags.*/ - uint16_t resolution_h; /*< [pix] Horizontal image resolution*/ - uint16_t resolution_v; /*< [pix] Vertical image resolution*/ - uint16_t cam_definition_version; /*< Camera definition version (iteration)*/ + uint16_t resolution_h; /*< [pix] Horizontal image resolution. Use 0 if not known.*/ + uint16_t resolution_v; /*< [pix] Vertical image resolution. Use 0 if not known.*/ + uint16_t cam_definition_version; /*< Camera definition version (iteration). Use 0 if not known.*/ uint8_t vendor_name[32]; /*< Name of the camera vendor*/ uint8_t model_name[32]; /*< Name of the camera model*/ - uint8_t lens_id; /*< Reserved for a lens ID*/ - char cam_definition_uri[140]; /*< Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol).*/ + uint8_t lens_id; /*< Reserved for a lens ID. Use 0 if not known.*/ + char cam_definition_uri[140]; /*< Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). The definition file may be xz compressed, which will be indicated by the file extension .xml.xz (a GCS that implements the protocol must support decompressing the file). The string needs to be zero terminated. Use a zero-length string if not known.*/ + uint8_t gimbal_device_id; /*< Gimbal id of a gimbal associated with this camera. This is the component id of the gimbal device, or 1-6 for non mavlink gimbals. Use 0 if no gimbal is associated with the camera.*/ + uint8_t camera_device_id; /*< Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id).*/ } mavlink_camera_information_t; -#define MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN 235 +#define MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN 237 #define MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN 235 -#define MAVLINK_MSG_ID_259_LEN 235 +#define MAVLINK_MSG_ID_259_LEN 237 #define MAVLINK_MSG_ID_259_MIN_LEN 235 #define MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC 92 @@ -36,7 +38,7 @@ typedef struct __mavlink_camera_information_t { #define MAVLINK_MESSAGE_INFO_CAMERA_INFORMATION { \ 259, \ "CAMERA_INFORMATION", \ - 13, \ + 15, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_information_t, time_boot_ms) }, \ { "vendor_name", NULL, MAVLINK_TYPE_UINT8_T, 32, 30, offsetof(mavlink_camera_information_t, vendor_name) }, \ { "model_name", NULL, MAVLINK_TYPE_UINT8_T, 32, 62, offsetof(mavlink_camera_information_t, model_name) }, \ @@ -50,12 +52,14 @@ typedef struct __mavlink_camera_information_t { { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_camera_information_t, flags) }, \ { "cam_definition_version", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_camera_information_t, cam_definition_version) }, \ { "cam_definition_uri", NULL, MAVLINK_TYPE_CHAR, 140, 95, offsetof(mavlink_camera_information_t, cam_definition_uri) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 235, offsetof(mavlink_camera_information_t, gimbal_device_id) }, \ + { "camera_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 236, offsetof(mavlink_camera_information_t, camera_device_id) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_CAMERA_INFORMATION { \ "CAMERA_INFORMATION", \ - 13, \ + 15, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_information_t, time_boot_ms) }, \ { "vendor_name", NULL, MAVLINK_TYPE_UINT8_T, 32, 30, offsetof(mavlink_camera_information_t, vendor_name) }, \ { "model_name", NULL, MAVLINK_TYPE_UINT8_T, 32, 62, offsetof(mavlink_camera_information_t, model_name) }, \ @@ -69,6 +73,8 @@ typedef struct __mavlink_camera_information_t { { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_camera_information_t, flags) }, \ { "cam_definition_version", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_camera_information_t, cam_definition_version) }, \ { "cam_definition_uri", NULL, MAVLINK_TYPE_CHAR, 140, 95, offsetof(mavlink_camera_information_t, cam_definition_uri) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 235, offsetof(mavlink_camera_information_t, gimbal_device_id) }, \ + { "camera_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 236, offsetof(mavlink_camera_information_t, camera_device_id) }, \ } \ } #endif @@ -82,20 +88,22 @@ typedef struct __mavlink_camera_information_t { * @param time_boot_ms [ms] Timestamp (time since system boot). * @param vendor_name Name of the camera vendor * @param model_name Name of the camera model - * @param firmware_version Version of the camera firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff) - * @param focal_length [mm] Focal length - * @param sensor_size_h [mm] Image sensor size horizontal - * @param sensor_size_v [mm] Image sensor size vertical - * @param resolution_h [pix] Horizontal image resolution - * @param resolution_v [pix] Vertical image resolution - * @param lens_id Reserved for a lens ID + * @param firmware_version Version of the camera firmware, encoded as: `(Dev & 0xff) << 24 \| (Patch & 0xff) << 16 \| (Minor & 0xff) << 8 \| (Major & 0xff)`. Use 0 if not known. + * @param focal_length [mm] Focal length. Use NaN if not known. + * @param sensor_size_h [mm] Image sensor size horizontal. Use NaN if not known. + * @param sensor_size_v [mm] Image sensor size vertical. Use NaN if not known. + * @param resolution_h [pix] Horizontal image resolution. Use 0 if not known. + * @param resolution_v [pix] Vertical image resolution. Use 0 if not known. + * @param lens_id Reserved for a lens ID. Use 0 if not known. * @param flags Bitmap of camera capability flags. - * @param cam_definition_version Camera definition version (iteration) - * @param cam_definition_uri Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). + * @param cam_definition_version Camera definition version (iteration). Use 0 if not known. + * @param cam_definition_uri Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). The definition file may be xz compressed, which will be indicated by the file extension .xml.xz (a GCS that implements the protocol must support decompressing the file). The string needs to be zero terminated. Use a zero-length string if not known. + * @param gimbal_device_id Gimbal id of a gimbal associated with this camera. This is the component id of the gimbal device, or 1-6 for non mavlink gimbals. Use 0 if no gimbal is associated with the camera. + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_camera_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, const uint8_t *vendor_name, const uint8_t *model_name, uint32_t firmware_version, float focal_length, float sensor_size_h, float sensor_size_v, uint16_t resolution_h, uint16_t resolution_v, uint8_t lens_id, uint32_t flags, uint16_t cam_definition_version, const char *cam_definition_uri) + uint32_t time_boot_ms, const uint8_t *vendor_name, const uint8_t *model_name, uint32_t firmware_version, float focal_length, float sensor_size_h, float sensor_size_v, uint16_t resolution_h, uint16_t resolution_v, uint8_t lens_id, uint32_t flags, uint16_t cam_definition_version, const char *cam_definition_uri, uint8_t gimbal_device_id, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN]; @@ -109,6 +117,8 @@ static inline uint16_t mavlink_msg_camera_information_pack(uint8_t system_id, ui _mav_put_uint16_t(buf, 26, resolution_v); _mav_put_uint16_t(buf, 28, cam_definition_version); _mav_put_uint8_t(buf, 94, lens_id); + _mav_put_uint8_t(buf, 235, gimbal_device_id); + _mav_put_uint8_t(buf, 236, camera_device_id); _mav_put_uint8_t_array(buf, 30, vendor_name, 32); _mav_put_uint8_t_array(buf, 62, model_name, 32); _mav_put_char_array(buf, 95, cam_definition_uri, 140); @@ -125,6 +135,77 @@ static inline uint16_t mavlink_msg_camera_information_pack(uint8_t system_id, ui packet.resolution_v = resolution_v; packet.cam_definition_version = cam_definition_version; packet.lens_id = lens_id; + packet.gimbal_device_id = gimbal_device_id; + packet.camera_device_id = camera_device_id; + mav_array_assign_uint8_t(packet.vendor_name, vendor_name, 32); + mav_array_assign_uint8_t(packet.model_name, model_name, 32); + mav_array_assign_char(packet.cam_definition_uri, cam_definition_uri, 140); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_INFORMATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); +} + +/** + * @brief Pack a camera_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param vendor_name Name of the camera vendor + * @param model_name Name of the camera model + * @param firmware_version Version of the camera firmware, encoded as: `(Dev & 0xff) << 24 \| (Patch & 0xff) << 16 \| (Minor & 0xff) << 8 \| (Major & 0xff)`. Use 0 if not known. + * @param focal_length [mm] Focal length. Use NaN if not known. + * @param sensor_size_h [mm] Image sensor size horizontal. Use NaN if not known. + * @param sensor_size_v [mm] Image sensor size vertical. Use NaN if not known. + * @param resolution_h [pix] Horizontal image resolution. Use 0 if not known. + * @param resolution_v [pix] Vertical image resolution. Use 0 if not known. + * @param lens_id Reserved for a lens ID. Use 0 if not known. + * @param flags Bitmap of camera capability flags. + * @param cam_definition_version Camera definition version (iteration). Use 0 if not known. + * @param cam_definition_uri Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). The definition file may be xz compressed, which will be indicated by the file extension .xml.xz (a GCS that implements the protocol must support decompressing the file). The string needs to be zero terminated. Use a zero-length string if not known. + * @param gimbal_device_id Gimbal id of a gimbal associated with this camera. This is the component id of the gimbal device, or 1-6 for non mavlink gimbals. Use 0 if no gimbal is associated with the camera. + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_information_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, const uint8_t *vendor_name, const uint8_t *model_name, uint32_t firmware_version, float focal_length, float sensor_size_h, float sensor_size_v, uint16_t resolution_h, uint16_t resolution_v, uint8_t lens_id, uint32_t flags, uint16_t cam_definition_version, const char *cam_definition_uri, uint8_t gimbal_device_id, uint8_t camera_device_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, firmware_version); + _mav_put_float(buf, 8, focal_length); + _mav_put_float(buf, 12, sensor_size_h); + _mav_put_float(buf, 16, sensor_size_v); + _mav_put_uint32_t(buf, 20, flags); + _mav_put_uint16_t(buf, 24, resolution_h); + _mav_put_uint16_t(buf, 26, resolution_v); + _mav_put_uint16_t(buf, 28, cam_definition_version); + _mav_put_uint8_t(buf, 94, lens_id); + _mav_put_uint8_t(buf, 235, gimbal_device_id); + _mav_put_uint8_t(buf, 236, camera_device_id); + _mav_put_uint8_t_array(buf, 30, vendor_name, 32); + _mav_put_uint8_t_array(buf, 62, model_name, 32); + _mav_put_char_array(buf, 95, cam_definition_uri, 140); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN); +#else + mavlink_camera_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.firmware_version = firmware_version; + packet.focal_length = focal_length; + packet.sensor_size_h = sensor_size_h; + packet.sensor_size_v = sensor_size_v; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.cam_definition_version = cam_definition_version; + packet.lens_id = lens_id; + packet.gimbal_device_id = gimbal_device_id; + packet.camera_device_id = camera_device_id; mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(uint8_t)*32); mav_array_memcpy(packet.model_name, model_name, sizeof(uint8_t)*32); mav_array_memcpy(packet.cam_definition_uri, cam_definition_uri, sizeof(char)*140); @@ -132,7 +213,11 @@ static inline uint16_t mavlink_msg_camera_information_pack(uint8_t system_id, ui #endif msg->msgid = MAVLINK_MSG_ID_CAMERA_INFORMATION; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN); +#endif } /** @@ -144,21 +229,23 @@ static inline uint16_t mavlink_msg_camera_information_pack(uint8_t system_id, ui * @param time_boot_ms [ms] Timestamp (time since system boot). * @param vendor_name Name of the camera vendor * @param model_name Name of the camera model - * @param firmware_version Version of the camera firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff) - * @param focal_length [mm] Focal length - * @param sensor_size_h [mm] Image sensor size horizontal - * @param sensor_size_v [mm] Image sensor size vertical - * @param resolution_h [pix] Horizontal image resolution - * @param resolution_v [pix] Vertical image resolution - * @param lens_id Reserved for a lens ID + * @param firmware_version Version of the camera firmware, encoded as: `(Dev & 0xff) << 24 \| (Patch & 0xff) << 16 \| (Minor & 0xff) << 8 \| (Major & 0xff)`. Use 0 if not known. + * @param focal_length [mm] Focal length. Use NaN if not known. + * @param sensor_size_h [mm] Image sensor size horizontal. Use NaN if not known. + * @param sensor_size_v [mm] Image sensor size vertical. Use NaN if not known. + * @param resolution_h [pix] Horizontal image resolution. Use 0 if not known. + * @param resolution_v [pix] Vertical image resolution. Use 0 if not known. + * @param lens_id Reserved for a lens ID. Use 0 if not known. * @param flags Bitmap of camera capability flags. - * @param cam_definition_version Camera definition version (iteration) - * @param cam_definition_uri Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). + * @param cam_definition_version Camera definition version (iteration). Use 0 if not known. + * @param cam_definition_uri Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). The definition file may be xz compressed, which will be indicated by the file extension .xml.xz (a GCS that implements the protocol must support decompressing the file). The string needs to be zero terminated. Use a zero-length string if not known. + * @param gimbal_device_id Gimbal id of a gimbal associated with this camera. This is the component id of the gimbal device, or 1-6 for non mavlink gimbals. Use 0 if no gimbal is associated with the camera. + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_camera_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,const uint8_t *vendor_name,const uint8_t *model_name,uint32_t firmware_version,float focal_length,float sensor_size_h,float sensor_size_v,uint16_t resolution_h,uint16_t resolution_v,uint8_t lens_id,uint32_t flags,uint16_t cam_definition_version,const char *cam_definition_uri) + uint32_t time_boot_ms,const uint8_t *vendor_name,const uint8_t *model_name,uint32_t firmware_version,float focal_length,float sensor_size_h,float sensor_size_v,uint16_t resolution_h,uint16_t resolution_v,uint8_t lens_id,uint32_t flags,uint16_t cam_definition_version,const char *cam_definition_uri,uint8_t gimbal_device_id,uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN]; @@ -172,6 +259,8 @@ static inline uint16_t mavlink_msg_camera_information_pack_chan(uint8_t system_i _mav_put_uint16_t(buf, 26, resolution_v); _mav_put_uint16_t(buf, 28, cam_definition_version); _mav_put_uint8_t(buf, 94, lens_id); + _mav_put_uint8_t(buf, 235, gimbal_device_id); + _mav_put_uint8_t(buf, 236, camera_device_id); _mav_put_uint8_t_array(buf, 30, vendor_name, 32); _mav_put_uint8_t_array(buf, 62, model_name, 32); _mav_put_char_array(buf, 95, cam_definition_uri, 140); @@ -188,9 +277,11 @@ static inline uint16_t mavlink_msg_camera_information_pack_chan(uint8_t system_i packet.resolution_v = resolution_v; packet.cam_definition_version = cam_definition_version; packet.lens_id = lens_id; - mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(uint8_t)*32); - mav_array_memcpy(packet.model_name, model_name, sizeof(uint8_t)*32); - mav_array_memcpy(packet.cam_definition_uri, cam_definition_uri, sizeof(char)*140); + packet.gimbal_device_id = gimbal_device_id; + packet.camera_device_id = camera_device_id; + mav_array_assign_uint8_t(packet.vendor_name, vendor_name, 32); + mav_array_assign_uint8_t(packet.model_name, model_name, 32); + mav_array_assign_char(packet.cam_definition_uri, cam_definition_uri, 140); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN); #endif @@ -208,7 +299,7 @@ static inline uint16_t mavlink_msg_camera_information_pack_chan(uint8_t system_i */ static inline uint16_t mavlink_msg_camera_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_information_t* camera_information) { - return mavlink_msg_camera_information_pack(system_id, component_id, msg, camera_information->time_boot_ms, camera_information->vendor_name, camera_information->model_name, camera_information->firmware_version, camera_information->focal_length, camera_information->sensor_size_h, camera_information->sensor_size_v, camera_information->resolution_h, camera_information->resolution_v, camera_information->lens_id, camera_information->flags, camera_information->cam_definition_version, camera_information->cam_definition_uri); + return mavlink_msg_camera_information_pack(system_id, component_id, msg, camera_information->time_boot_ms, camera_information->vendor_name, camera_information->model_name, camera_information->firmware_version, camera_information->focal_length, camera_information->sensor_size_h, camera_information->sensor_size_v, camera_information->resolution_h, camera_information->resolution_v, camera_information->lens_id, camera_information->flags, camera_information->cam_definition_version, camera_information->cam_definition_uri, camera_information->gimbal_device_id, camera_information->camera_device_id); } /** @@ -222,7 +313,21 @@ static inline uint16_t mavlink_msg_camera_information_encode(uint8_t system_id, */ static inline uint16_t mavlink_msg_camera_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_information_t* camera_information) { - return mavlink_msg_camera_information_pack_chan(system_id, component_id, chan, msg, camera_information->time_boot_ms, camera_information->vendor_name, camera_information->model_name, camera_information->firmware_version, camera_information->focal_length, camera_information->sensor_size_h, camera_information->sensor_size_v, camera_information->resolution_h, camera_information->resolution_v, camera_information->lens_id, camera_information->flags, camera_information->cam_definition_version, camera_information->cam_definition_uri); + return mavlink_msg_camera_information_pack_chan(system_id, component_id, chan, msg, camera_information->time_boot_ms, camera_information->vendor_name, camera_information->model_name, camera_information->firmware_version, camera_information->focal_length, camera_information->sensor_size_h, camera_information->sensor_size_v, camera_information->resolution_h, camera_information->resolution_v, camera_information->lens_id, camera_information->flags, camera_information->cam_definition_version, camera_information->cam_definition_uri, camera_information->gimbal_device_id, camera_information->camera_device_id); +} + +/** + * @brief Encode a camera_information struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param camera_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_information_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_camera_information_t* camera_information) +{ + return mavlink_msg_camera_information_pack_status(system_id, component_id, _status, msg, camera_information->time_boot_ms, camera_information->vendor_name, camera_information->model_name, camera_information->firmware_version, camera_information->focal_length, camera_information->sensor_size_h, camera_information->sensor_size_v, camera_information->resolution_h, camera_information->resolution_v, camera_information->lens_id, camera_information->flags, camera_information->cam_definition_version, camera_information->cam_definition_uri, camera_information->gimbal_device_id, camera_information->camera_device_id); } /** @@ -232,20 +337,22 @@ static inline uint16_t mavlink_msg_camera_information_encode_chan(uint8_t system * @param time_boot_ms [ms] Timestamp (time since system boot). * @param vendor_name Name of the camera vendor * @param model_name Name of the camera model - * @param firmware_version Version of the camera firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff) - * @param focal_length [mm] Focal length - * @param sensor_size_h [mm] Image sensor size horizontal - * @param sensor_size_v [mm] Image sensor size vertical - * @param resolution_h [pix] Horizontal image resolution - * @param resolution_v [pix] Vertical image resolution - * @param lens_id Reserved for a lens ID + * @param firmware_version Version of the camera firmware, encoded as: `(Dev & 0xff) << 24 \| (Patch & 0xff) << 16 \| (Minor & 0xff) << 8 \| (Major & 0xff)`. Use 0 if not known. + * @param focal_length [mm] Focal length. Use NaN if not known. + * @param sensor_size_h [mm] Image sensor size horizontal. Use NaN if not known. + * @param sensor_size_v [mm] Image sensor size vertical. Use NaN if not known. + * @param resolution_h [pix] Horizontal image resolution. Use 0 if not known. + * @param resolution_v [pix] Vertical image resolution. Use 0 if not known. + * @param lens_id Reserved for a lens ID. Use 0 if not known. * @param flags Bitmap of camera capability flags. - * @param cam_definition_version Camera definition version (iteration) - * @param cam_definition_uri Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). + * @param cam_definition_version Camera definition version (iteration). Use 0 if not known. + * @param cam_definition_uri Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). The definition file may be xz compressed, which will be indicated by the file extension .xml.xz (a GCS that implements the protocol must support decompressing the file). The string needs to be zero terminated. Use a zero-length string if not known. + * @param gimbal_device_id Gimbal id of a gimbal associated with this camera. This is the component id of the gimbal device, or 1-6 for non mavlink gimbals. Use 0 if no gimbal is associated with the camera. + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_camera_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, const uint8_t *vendor_name, const uint8_t *model_name, uint32_t firmware_version, float focal_length, float sensor_size_h, float sensor_size_v, uint16_t resolution_h, uint16_t resolution_v, uint8_t lens_id, uint32_t flags, uint16_t cam_definition_version, const char *cam_definition_uri) +static inline void mavlink_msg_camera_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, const uint8_t *vendor_name, const uint8_t *model_name, uint32_t firmware_version, float focal_length, float sensor_size_h, float sensor_size_v, uint16_t resolution_h, uint16_t resolution_v, uint8_t lens_id, uint32_t flags, uint16_t cam_definition_version, const char *cam_definition_uri, uint8_t gimbal_device_id, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN]; @@ -259,6 +366,8 @@ static inline void mavlink_msg_camera_information_send(mavlink_channel_t chan, u _mav_put_uint16_t(buf, 26, resolution_v); _mav_put_uint16_t(buf, 28, cam_definition_version); _mav_put_uint8_t(buf, 94, lens_id); + _mav_put_uint8_t(buf, 235, gimbal_device_id); + _mav_put_uint8_t(buf, 236, camera_device_id); _mav_put_uint8_t_array(buf, 30, vendor_name, 32); _mav_put_uint8_t_array(buf, 62, model_name, 32); _mav_put_char_array(buf, 95, cam_definition_uri, 140); @@ -275,9 +384,11 @@ static inline void mavlink_msg_camera_information_send(mavlink_channel_t chan, u packet.resolution_v = resolution_v; packet.cam_definition_version = cam_definition_version; packet.lens_id = lens_id; - mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(uint8_t)*32); - mav_array_memcpy(packet.model_name, model_name, sizeof(uint8_t)*32); - mav_array_memcpy(packet.cam_definition_uri, cam_definition_uri, sizeof(char)*140); + packet.gimbal_device_id = gimbal_device_id; + packet.camera_device_id = camera_device_id; + mav_array_assign_uint8_t(packet.vendor_name, vendor_name, 32); + mav_array_assign_uint8_t(packet.model_name, model_name, 32); + mav_array_assign_char(packet.cam_definition_uri, cam_definition_uri, 140); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); #endif } @@ -290,7 +401,7 @@ static inline void mavlink_msg_camera_information_send(mavlink_channel_t chan, u static inline void mavlink_msg_camera_information_send_struct(mavlink_channel_t chan, const mavlink_camera_information_t* camera_information) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_camera_information_send(chan, camera_information->time_boot_ms, camera_information->vendor_name, camera_information->model_name, camera_information->firmware_version, camera_information->focal_length, camera_information->sensor_size_h, camera_information->sensor_size_v, camera_information->resolution_h, camera_information->resolution_v, camera_information->lens_id, camera_information->flags, camera_information->cam_definition_version, camera_information->cam_definition_uri); + mavlink_msg_camera_information_send(chan, camera_information->time_boot_ms, camera_information->vendor_name, camera_information->model_name, camera_information->firmware_version, camera_information->focal_length, camera_information->sensor_size_h, camera_information->sensor_size_v, camera_information->resolution_h, camera_information->resolution_v, camera_information->lens_id, camera_information->flags, camera_information->cam_definition_version, camera_information->cam_definition_uri, camera_information->gimbal_device_id, camera_information->camera_device_id); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_INFORMATION, (const char *)camera_information, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); #endif @@ -298,13 +409,13 @@ static inline void mavlink_msg_camera_information_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_camera_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const uint8_t *vendor_name, const uint8_t *model_name, uint32_t firmware_version, float focal_length, float sensor_size_h, float sensor_size_v, uint16_t resolution_h, uint16_t resolution_v, uint8_t lens_id, uint32_t flags, uint16_t cam_definition_version, const char *cam_definition_uri) +static inline void mavlink_msg_camera_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const uint8_t *vendor_name, const uint8_t *model_name, uint32_t firmware_version, float focal_length, float sensor_size_h, float sensor_size_v, uint16_t resolution_h, uint16_t resolution_v, uint8_t lens_id, uint32_t flags, uint16_t cam_definition_version, const char *cam_definition_uri, uint8_t gimbal_device_id, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -318,6 +429,8 @@ static inline void mavlink_msg_camera_information_send_buf(mavlink_message_t *ms _mav_put_uint16_t(buf, 26, resolution_v); _mav_put_uint16_t(buf, 28, cam_definition_version); _mav_put_uint8_t(buf, 94, lens_id); + _mav_put_uint8_t(buf, 235, gimbal_device_id); + _mav_put_uint8_t(buf, 236, camera_device_id); _mav_put_uint8_t_array(buf, 30, vendor_name, 32); _mav_put_uint8_t_array(buf, 62, model_name, 32); _mav_put_char_array(buf, 95, cam_definition_uri, 140); @@ -334,9 +447,11 @@ static inline void mavlink_msg_camera_information_send_buf(mavlink_message_t *ms packet->resolution_v = resolution_v; packet->cam_definition_version = cam_definition_version; packet->lens_id = lens_id; - mav_array_memcpy(packet->vendor_name, vendor_name, sizeof(uint8_t)*32); - mav_array_memcpy(packet->model_name, model_name, sizeof(uint8_t)*32); - mav_array_memcpy(packet->cam_definition_uri, cam_definition_uri, sizeof(char)*140); + packet->gimbal_device_id = gimbal_device_id; + packet->camera_device_id = camera_device_id; + mav_array_assign_uint8_t(packet->vendor_name, vendor_name, 32); + mav_array_assign_uint8_t(packet->model_name, model_name, 32); + mav_array_assign_char(packet->cam_definition_uri, cam_definition_uri, 140); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); #endif } @@ -380,7 +495,7 @@ static inline uint16_t mavlink_msg_camera_information_get_model_name(const mavli /** * @brief Get field firmware_version from camera_information message * - * @return Version of the camera firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff) + * @return Version of the camera firmware, encoded as: `(Dev & 0xff) << 24 \| (Patch & 0xff) << 16 \| (Minor & 0xff) << 8 \| (Major & 0xff)`. Use 0 if not known. */ static inline uint32_t mavlink_msg_camera_information_get_firmware_version(const mavlink_message_t* msg) { @@ -390,7 +505,7 @@ static inline uint32_t mavlink_msg_camera_information_get_firmware_version(const /** * @brief Get field focal_length from camera_information message * - * @return [mm] Focal length + * @return [mm] Focal length. Use NaN if not known. */ static inline float mavlink_msg_camera_information_get_focal_length(const mavlink_message_t* msg) { @@ -400,7 +515,7 @@ static inline float mavlink_msg_camera_information_get_focal_length(const mavlin /** * @brief Get field sensor_size_h from camera_information message * - * @return [mm] Image sensor size horizontal + * @return [mm] Image sensor size horizontal. Use NaN if not known. */ static inline float mavlink_msg_camera_information_get_sensor_size_h(const mavlink_message_t* msg) { @@ -410,7 +525,7 @@ static inline float mavlink_msg_camera_information_get_sensor_size_h(const mavli /** * @brief Get field sensor_size_v from camera_information message * - * @return [mm] Image sensor size vertical + * @return [mm] Image sensor size vertical. Use NaN if not known. */ static inline float mavlink_msg_camera_information_get_sensor_size_v(const mavlink_message_t* msg) { @@ -420,7 +535,7 @@ static inline float mavlink_msg_camera_information_get_sensor_size_v(const mavli /** * @brief Get field resolution_h from camera_information message * - * @return [pix] Horizontal image resolution + * @return [pix] Horizontal image resolution. Use 0 if not known. */ static inline uint16_t mavlink_msg_camera_information_get_resolution_h(const mavlink_message_t* msg) { @@ -430,7 +545,7 @@ static inline uint16_t mavlink_msg_camera_information_get_resolution_h(const mav /** * @brief Get field resolution_v from camera_information message * - * @return [pix] Vertical image resolution + * @return [pix] Vertical image resolution. Use 0 if not known. */ static inline uint16_t mavlink_msg_camera_information_get_resolution_v(const mavlink_message_t* msg) { @@ -440,7 +555,7 @@ static inline uint16_t mavlink_msg_camera_information_get_resolution_v(const mav /** * @brief Get field lens_id from camera_information message * - * @return Reserved for a lens ID + * @return Reserved for a lens ID. Use 0 if not known. */ static inline uint8_t mavlink_msg_camera_information_get_lens_id(const mavlink_message_t* msg) { @@ -460,7 +575,7 @@ static inline uint32_t mavlink_msg_camera_information_get_flags(const mavlink_me /** * @brief Get field cam_definition_version from camera_information message * - * @return Camera definition version (iteration) + * @return Camera definition version (iteration). Use 0 if not known. */ static inline uint16_t mavlink_msg_camera_information_get_cam_definition_version(const mavlink_message_t* msg) { @@ -470,13 +585,33 @@ static inline uint16_t mavlink_msg_camera_information_get_cam_definition_version /** * @brief Get field cam_definition_uri from camera_information message * - * @return Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). + * @return Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). The definition file may be xz compressed, which will be indicated by the file extension .xml.xz (a GCS that implements the protocol must support decompressing the file). The string needs to be zero terminated. Use a zero-length string if not known. */ static inline uint16_t mavlink_msg_camera_information_get_cam_definition_uri(const mavlink_message_t* msg, char *cam_definition_uri) { return _MAV_RETURN_char_array(msg, cam_definition_uri, 140, 95); } +/** + * @brief Get field gimbal_device_id from camera_information message + * + * @return Gimbal id of a gimbal associated with this camera. This is the component id of the gimbal device, or 1-6 for non mavlink gimbals. Use 0 if no gimbal is associated with the camera. + */ +static inline uint8_t mavlink_msg_camera_information_get_gimbal_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 235); +} + +/** + * @brief Get field camera_device_id from camera_information message + * + * @return Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + */ +static inline uint8_t mavlink_msg_camera_information_get_camera_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 236); +} + /** * @brief Decode a camera_information message into a struct * @@ -499,6 +634,8 @@ static inline void mavlink_msg_camera_information_decode(const mavlink_message_t mavlink_msg_camera_information_get_model_name(msg, camera_information->model_name); camera_information->lens_id = mavlink_msg_camera_information_get_lens_id(msg); mavlink_msg_camera_information_get_cam_definition_uri(msg, camera_information->cam_definition_uri); + camera_information->gimbal_device_id = mavlink_msg_camera_information_get_gimbal_device_id(msg); + camera_information->camera_device_id = mavlink_msg_camera_information_get_camera_device_id(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN; memset(camera_information, 0, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_settings.h b/lib/main/MAVLink/common/mavlink_msg_camera_settings.h index 8f49780fadb..0083d155661 100644 --- a/lib/main/MAVLink/common/mavlink_msg_camera_settings.h +++ b/lib/main/MAVLink/common/mavlink_msg_camera_settings.h @@ -7,13 +7,14 @@ MAVPACKED( typedef struct __mavlink_camera_settings_t { uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ uint8_t mode_id; /*< Camera mode*/ - float zoomLevel; /*< Current zoom level (0.0 to 100.0, NaN if not known)*/ - float focusLevel; /*< Current focus level (0.0 to 100.0, NaN if not known)*/ + float zoomLevel; /*< Current zoom level as a percentage of the full range (0.0 to 100.0, NaN if not known)*/ + float focusLevel; /*< Current focus level as a percentage of the full range (0.0 to 100.0, NaN if not known)*/ + uint8_t camera_device_id; /*< Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id).*/ }) mavlink_camera_settings_t; -#define MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN 13 +#define MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN 14 #define MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN 5 -#define MAVLINK_MSG_ID_260_LEN 13 +#define MAVLINK_MSG_ID_260_LEN 14 #define MAVLINK_MSG_ID_260_MIN_LEN 5 #define MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC 146 @@ -25,21 +26,23 @@ typedef struct __mavlink_camera_settings_t { #define MAVLINK_MESSAGE_INFO_CAMERA_SETTINGS { \ 260, \ "CAMERA_SETTINGS", \ - 4, \ + 5, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_settings_t, time_boot_ms) }, \ { "mode_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_camera_settings_t, mode_id) }, \ { "zoomLevel", NULL, MAVLINK_TYPE_FLOAT, 0, 5, offsetof(mavlink_camera_settings_t, zoomLevel) }, \ { "focusLevel", NULL, MAVLINK_TYPE_FLOAT, 0, 9, offsetof(mavlink_camera_settings_t, focusLevel) }, \ + { "camera_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_camera_settings_t, camera_device_id) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_CAMERA_SETTINGS { \ "CAMERA_SETTINGS", \ - 4, \ + 5, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_settings_t, time_boot_ms) }, \ { "mode_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_camera_settings_t, mode_id) }, \ { "zoomLevel", NULL, MAVLINK_TYPE_FLOAT, 0, 5, offsetof(mavlink_camera_settings_t, zoomLevel) }, \ { "focusLevel", NULL, MAVLINK_TYPE_FLOAT, 0, 9, offsetof(mavlink_camera_settings_t, focusLevel) }, \ + { "camera_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_camera_settings_t, camera_device_id) }, \ } \ } #endif @@ -52,12 +55,13 @@ typedef struct __mavlink_camera_settings_t { * * @param time_boot_ms [ms] Timestamp (time since system boot). * @param mode_id Camera mode - * @param zoomLevel Current zoom level (0.0 to 100.0, NaN if not known) - * @param focusLevel Current focus level (0.0 to 100.0, NaN if not known) + * @param zoomLevel Current zoom level as a percentage of the full range (0.0 to 100.0, NaN if not known) + * @param focusLevel Current focus level as a percentage of the full range (0.0 to 100.0, NaN if not known) + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_camera_settings_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, uint8_t mode_id, float zoomLevel, float focusLevel) + uint32_t time_boot_ms, uint8_t mode_id, float zoomLevel, float focusLevel, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN]; @@ -65,6 +69,7 @@ static inline uint16_t mavlink_msg_camera_settings_pack(uint8_t system_id, uint8 _mav_put_uint8_t(buf, 4, mode_id); _mav_put_float(buf, 5, zoomLevel); _mav_put_float(buf, 9, focusLevel); + _mav_put_uint8_t(buf, 13, camera_device_id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN); #else @@ -73,6 +78,7 @@ static inline uint16_t mavlink_msg_camera_settings_pack(uint8_t system_id, uint8 packet.mode_id = mode_id; packet.zoomLevel = zoomLevel; packet.focusLevel = focusLevel; + packet.camera_device_id = camera_device_id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN); #endif @@ -81,6 +87,51 @@ static inline uint16_t mavlink_msg_camera_settings_pack(uint8_t system_id, uint8 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); } +/** + * @brief Pack a camera_settings message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param mode_id Camera mode + * @param zoomLevel Current zoom level as a percentage of the full range (0.0 to 100.0, NaN if not known) + * @param focusLevel Current focus level as a percentage of the full range (0.0 to 100.0, NaN if not known) + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_settings_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t mode_id, float zoomLevel, float focusLevel, uint8_t camera_device_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint8_t(buf, 4, mode_id); + _mav_put_float(buf, 5, zoomLevel); + _mav_put_float(buf, 9, focusLevel); + _mav_put_uint8_t(buf, 13, camera_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN); +#else + mavlink_camera_settings_t packet; + packet.time_boot_ms = time_boot_ms; + packet.mode_id = mode_id; + packet.zoomLevel = zoomLevel; + packet.focusLevel = focusLevel; + packet.camera_device_id = camera_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_SETTINGS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN); +#endif +} + /** * @brief Pack a camera_settings message on a channel * @param system_id ID of this system @@ -89,13 +140,14 @@ static inline uint16_t mavlink_msg_camera_settings_pack(uint8_t system_id, uint8 * @param msg The MAVLink message to compress the data into * @param time_boot_ms [ms] Timestamp (time since system boot). * @param mode_id Camera mode - * @param zoomLevel Current zoom level (0.0 to 100.0, NaN if not known) - * @param focusLevel Current focus level (0.0 to 100.0, NaN if not known) + * @param zoomLevel Current zoom level as a percentage of the full range (0.0 to 100.0, NaN if not known) + * @param focusLevel Current focus level as a percentage of the full range (0.0 to 100.0, NaN if not known) + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_camera_settings_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,uint8_t mode_id,float zoomLevel,float focusLevel) + uint32_t time_boot_ms,uint8_t mode_id,float zoomLevel,float focusLevel,uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN]; @@ -103,6 +155,7 @@ static inline uint16_t mavlink_msg_camera_settings_pack_chan(uint8_t system_id, _mav_put_uint8_t(buf, 4, mode_id); _mav_put_float(buf, 5, zoomLevel); _mav_put_float(buf, 9, focusLevel); + _mav_put_uint8_t(buf, 13, camera_device_id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN); #else @@ -111,6 +164,7 @@ static inline uint16_t mavlink_msg_camera_settings_pack_chan(uint8_t system_id, packet.mode_id = mode_id; packet.zoomLevel = zoomLevel; packet.focusLevel = focusLevel; + packet.camera_device_id = camera_device_id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN); #endif @@ -129,7 +183,7 @@ static inline uint16_t mavlink_msg_camera_settings_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_camera_settings_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_settings_t* camera_settings) { - return mavlink_msg_camera_settings_pack(system_id, component_id, msg, camera_settings->time_boot_ms, camera_settings->mode_id, camera_settings->zoomLevel, camera_settings->focusLevel); + return mavlink_msg_camera_settings_pack(system_id, component_id, msg, camera_settings->time_boot_ms, camera_settings->mode_id, camera_settings->zoomLevel, camera_settings->focusLevel, camera_settings->camera_device_id); } /** @@ -143,7 +197,21 @@ static inline uint16_t mavlink_msg_camera_settings_encode(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_camera_settings_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_settings_t* camera_settings) { - return mavlink_msg_camera_settings_pack_chan(system_id, component_id, chan, msg, camera_settings->time_boot_ms, camera_settings->mode_id, camera_settings->zoomLevel, camera_settings->focusLevel); + return mavlink_msg_camera_settings_pack_chan(system_id, component_id, chan, msg, camera_settings->time_boot_ms, camera_settings->mode_id, camera_settings->zoomLevel, camera_settings->focusLevel, camera_settings->camera_device_id); +} + +/** + * @brief Encode a camera_settings struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param camera_settings C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_settings_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_camera_settings_t* camera_settings) +{ + return mavlink_msg_camera_settings_pack_status(system_id, component_id, _status, msg, camera_settings->time_boot_ms, camera_settings->mode_id, camera_settings->zoomLevel, camera_settings->focusLevel, camera_settings->camera_device_id); } /** @@ -152,12 +220,13 @@ static inline uint16_t mavlink_msg_camera_settings_encode_chan(uint8_t system_id * * @param time_boot_ms [ms] Timestamp (time since system boot). * @param mode_id Camera mode - * @param zoomLevel Current zoom level (0.0 to 100.0, NaN if not known) - * @param focusLevel Current focus level (0.0 to 100.0, NaN if not known) + * @param zoomLevel Current zoom level as a percentage of the full range (0.0 to 100.0, NaN if not known) + * @param focusLevel Current focus level as a percentage of the full range (0.0 to 100.0, NaN if not known) + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_camera_settings_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t mode_id, float zoomLevel, float focusLevel) +static inline void mavlink_msg_camera_settings_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t mode_id, float zoomLevel, float focusLevel, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN]; @@ -165,6 +234,7 @@ static inline void mavlink_msg_camera_settings_send(mavlink_channel_t chan, uint _mav_put_uint8_t(buf, 4, mode_id); _mav_put_float(buf, 5, zoomLevel); _mav_put_float(buf, 9, focusLevel); + _mav_put_uint8_t(buf, 13, camera_device_id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_SETTINGS, buf, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); #else @@ -173,6 +243,7 @@ static inline void mavlink_msg_camera_settings_send(mavlink_channel_t chan, uint packet.mode_id = mode_id; packet.zoomLevel = zoomLevel; packet.focusLevel = focusLevel; + packet.camera_device_id = camera_device_id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_SETTINGS, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); #endif @@ -186,7 +257,7 @@ static inline void mavlink_msg_camera_settings_send(mavlink_channel_t chan, uint static inline void mavlink_msg_camera_settings_send_struct(mavlink_channel_t chan, const mavlink_camera_settings_t* camera_settings) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_camera_settings_send(chan, camera_settings->time_boot_ms, camera_settings->mode_id, camera_settings->zoomLevel, camera_settings->focusLevel); + mavlink_msg_camera_settings_send(chan, camera_settings->time_boot_ms, camera_settings->mode_id, camera_settings->zoomLevel, camera_settings->focusLevel, camera_settings->camera_device_id); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_SETTINGS, (const char *)camera_settings, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); #endif @@ -194,13 +265,13 @@ static inline void mavlink_msg_camera_settings_send_struct(mavlink_channel_t cha #if MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_camera_settings_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t mode_id, float zoomLevel, float focusLevel) +static inline void mavlink_msg_camera_settings_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t mode_id, float zoomLevel, float focusLevel, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,6 +279,7 @@ static inline void mavlink_msg_camera_settings_send_buf(mavlink_message_t *msgbu _mav_put_uint8_t(buf, 4, mode_id); _mav_put_float(buf, 5, zoomLevel); _mav_put_float(buf, 9, focusLevel); + _mav_put_uint8_t(buf, 13, camera_device_id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_SETTINGS, buf, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); #else @@ -216,6 +288,7 @@ static inline void mavlink_msg_camera_settings_send_buf(mavlink_message_t *msgbu packet->mode_id = mode_id; packet->zoomLevel = zoomLevel; packet->focusLevel = focusLevel; + packet->camera_device_id = camera_device_id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_SETTINGS, (const char *)packet, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); #endif @@ -250,7 +323,7 @@ static inline uint8_t mavlink_msg_camera_settings_get_mode_id(const mavlink_mess /** * @brief Get field zoomLevel from camera_settings message * - * @return Current zoom level (0.0 to 100.0, NaN if not known) + * @return Current zoom level as a percentage of the full range (0.0 to 100.0, NaN if not known) */ static inline float mavlink_msg_camera_settings_get_zoomLevel(const mavlink_message_t* msg) { @@ -260,13 +333,23 @@ static inline float mavlink_msg_camera_settings_get_zoomLevel(const mavlink_mess /** * @brief Get field focusLevel from camera_settings message * - * @return Current focus level (0.0 to 100.0, NaN if not known) + * @return Current focus level as a percentage of the full range (0.0 to 100.0, NaN if not known) */ static inline float mavlink_msg_camera_settings_get_focusLevel(const mavlink_message_t* msg) { return _MAV_RETURN_float(msg, 9); } +/** + * @brief Get field camera_device_id from camera_settings message + * + * @return Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + */ +static inline uint8_t mavlink_msg_camera_settings_get_camera_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 13); +} + /** * @brief Decode a camera_settings message into a struct * @@ -280,6 +363,7 @@ static inline void mavlink_msg_camera_settings_decode(const mavlink_message_t* m camera_settings->mode_id = mavlink_msg_camera_settings_get_mode_id(msg); camera_settings->zoomLevel = mavlink_msg_camera_settings_get_zoomLevel(msg); camera_settings->focusLevel = mavlink_msg_camera_settings_get_focusLevel(msg); + camera_settings->camera_device_id = mavlink_msg_camera_settings_get_camera_device_id(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN; memset(camera_settings, 0, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_thermal_range.h b/lib/main/MAVLink/common/mavlink_msg_camera_thermal_range.h new file mode 100644 index 00000000000..92ee507ff72 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_camera_thermal_range.h @@ -0,0 +1,484 @@ +#pragma once +// MESSAGE CAMERA_THERMAL_RANGE PACKING + +#define MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE 277 + + +typedef struct __mavlink_camera_thermal_range_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + float max; /*< [degC] Temperature max.*/ + float max_point_x; /*< Temperature max point x value (normalized 0..1, 0 is left, 1 is right), NAN if unknown.*/ + float max_point_y; /*< Temperature max point y value (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown.*/ + float min; /*< [degC] Temperature min.*/ + float min_point_x; /*< Temperature min point x value (normalized 0..1, 0 is left, 1 is right), NAN if unknown.*/ + float min_point_y; /*< Temperature min point y value (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown.*/ + uint8_t stream_id; /*< Video Stream ID (1 for first, 2 for second, etc.)*/ + uint8_t camera_device_id; /*< Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id).*/ +} mavlink_camera_thermal_range_t; + +#define MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN 30 +#define MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_MIN_LEN 30 +#define MAVLINK_MSG_ID_277_LEN 30 +#define MAVLINK_MSG_ID_277_MIN_LEN 30 + +#define MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_CRC 62 +#define MAVLINK_MSG_ID_277_CRC 62 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CAMERA_THERMAL_RANGE { \ + 277, \ + "CAMERA_THERMAL_RANGE", \ + 9, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_thermal_range_t, time_boot_ms) }, \ + { "stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_camera_thermal_range_t, stream_id) }, \ + { "camera_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_camera_thermal_range_t, camera_device_id) }, \ + { "max", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_camera_thermal_range_t, max) }, \ + { "max_point_x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_camera_thermal_range_t, max_point_x) }, \ + { "max_point_y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_thermal_range_t, max_point_y) }, \ + { "min", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_thermal_range_t, min) }, \ + { "min_point_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_camera_thermal_range_t, min_point_x) }, \ + { "min_point_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_camera_thermal_range_t, min_point_y) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CAMERA_THERMAL_RANGE { \ + "CAMERA_THERMAL_RANGE", \ + 9, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_thermal_range_t, time_boot_ms) }, \ + { "stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_camera_thermal_range_t, stream_id) }, \ + { "camera_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_camera_thermal_range_t, camera_device_id) }, \ + { "max", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_camera_thermal_range_t, max) }, \ + { "max_point_x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_camera_thermal_range_t, max_point_x) }, \ + { "max_point_y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_thermal_range_t, max_point_y) }, \ + { "min", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_thermal_range_t, min) }, \ + { "min_point_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_camera_thermal_range_t, min_point_x) }, \ + { "min_point_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_camera_thermal_range_t, min_point_y) }, \ + } \ +} +#endif + +/** + * @brief Pack a camera_thermal_range message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param stream_id Video Stream ID (1 for first, 2 for second, etc.) + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + * @param max [degC] Temperature max. + * @param max_point_x Temperature max point x value (normalized 0..1, 0 is left, 1 is right), NAN if unknown. + * @param max_point_y Temperature max point y value (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown. + * @param min [degC] Temperature min. + * @param min_point_x Temperature min point x value (normalized 0..1, 0 is left, 1 is right), NAN if unknown. + * @param min_point_y Temperature min point y value (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_thermal_range_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t stream_id, uint8_t camera_device_id, float max, float max_point_x, float max_point_y, float min, float min_point_x, float min_point_y) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, max); + _mav_put_float(buf, 8, max_point_x); + _mav_put_float(buf, 12, max_point_y); + _mav_put_float(buf, 16, min); + _mav_put_float(buf, 20, min_point_x); + _mav_put_float(buf, 24, min_point_y); + _mav_put_uint8_t(buf, 28, stream_id); + _mav_put_uint8_t(buf, 29, camera_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN); +#else + mavlink_camera_thermal_range_t packet; + packet.time_boot_ms = time_boot_ms; + packet.max = max; + packet.max_point_x = max_point_x; + packet.max_point_y = max_point_y; + packet.min = min; + packet.min_point_x = min_point_x; + packet.min_point_y = min_point_y; + packet.stream_id = stream_id; + packet.camera_device_id = camera_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_MIN_LEN, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_CRC); +} + +/** + * @brief Pack a camera_thermal_range message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param stream_id Video Stream ID (1 for first, 2 for second, etc.) + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + * @param max [degC] Temperature max. + * @param max_point_x Temperature max point x value (normalized 0..1, 0 is left, 1 is right), NAN if unknown. + * @param max_point_y Temperature max point y value (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown. + * @param min [degC] Temperature min. + * @param min_point_x Temperature min point x value (normalized 0..1, 0 is left, 1 is right), NAN if unknown. + * @param min_point_y Temperature min point y value (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_thermal_range_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t stream_id, uint8_t camera_device_id, float max, float max_point_x, float max_point_y, float min, float min_point_x, float min_point_y) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, max); + _mav_put_float(buf, 8, max_point_x); + _mav_put_float(buf, 12, max_point_y); + _mav_put_float(buf, 16, min); + _mav_put_float(buf, 20, min_point_x); + _mav_put_float(buf, 24, min_point_y); + _mav_put_uint8_t(buf, 28, stream_id); + _mav_put_uint8_t(buf, 29, camera_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN); +#else + mavlink_camera_thermal_range_t packet; + packet.time_boot_ms = time_boot_ms; + packet.max = max; + packet.max_point_x = max_point_x; + packet.max_point_y = max_point_y; + packet.min = min; + packet.min_point_x = min_point_x; + packet.min_point_y = min_point_y; + packet.stream_id = stream_id; + packet.camera_device_id = camera_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_MIN_LEN, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_MIN_LEN, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN); +#endif +} + +/** + * @brief Pack a camera_thermal_range message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param stream_id Video Stream ID (1 for first, 2 for second, etc.) + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + * @param max [degC] Temperature max. + * @param max_point_x Temperature max point x value (normalized 0..1, 0 is left, 1 is right), NAN if unknown. + * @param max_point_y Temperature max point y value (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown. + * @param min [degC] Temperature min. + * @param min_point_x Temperature min point x value (normalized 0..1, 0 is left, 1 is right), NAN if unknown. + * @param min_point_y Temperature min point y value (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_thermal_range_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t stream_id,uint8_t camera_device_id,float max,float max_point_x,float max_point_y,float min,float min_point_x,float min_point_y) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, max); + _mav_put_float(buf, 8, max_point_x); + _mav_put_float(buf, 12, max_point_y); + _mav_put_float(buf, 16, min); + _mav_put_float(buf, 20, min_point_x); + _mav_put_float(buf, 24, min_point_y); + _mav_put_uint8_t(buf, 28, stream_id); + _mav_put_uint8_t(buf, 29, camera_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN); +#else + mavlink_camera_thermal_range_t packet; + packet.time_boot_ms = time_boot_ms; + packet.max = max; + packet.max_point_x = max_point_x; + packet.max_point_y = max_point_y; + packet.min = min; + packet.min_point_x = min_point_x; + packet.min_point_y = min_point_y; + packet.stream_id = stream_id; + packet.camera_device_id = camera_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_MIN_LEN, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_CRC); +} + +/** + * @brief Encode a camera_thermal_range struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_thermal_range C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_thermal_range_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_thermal_range_t* camera_thermal_range) +{ + return mavlink_msg_camera_thermal_range_pack(system_id, component_id, msg, camera_thermal_range->time_boot_ms, camera_thermal_range->stream_id, camera_thermal_range->camera_device_id, camera_thermal_range->max, camera_thermal_range->max_point_x, camera_thermal_range->max_point_y, camera_thermal_range->min, camera_thermal_range->min_point_x, camera_thermal_range->min_point_y); +} + +/** + * @brief Encode a camera_thermal_range struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_thermal_range C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_thermal_range_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_thermal_range_t* camera_thermal_range) +{ + return mavlink_msg_camera_thermal_range_pack_chan(system_id, component_id, chan, msg, camera_thermal_range->time_boot_ms, camera_thermal_range->stream_id, camera_thermal_range->camera_device_id, camera_thermal_range->max, camera_thermal_range->max_point_x, camera_thermal_range->max_point_y, camera_thermal_range->min, camera_thermal_range->min_point_x, camera_thermal_range->min_point_y); +} + +/** + * @brief Encode a camera_thermal_range struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param camera_thermal_range C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_thermal_range_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_camera_thermal_range_t* camera_thermal_range) +{ + return mavlink_msg_camera_thermal_range_pack_status(system_id, component_id, _status, msg, camera_thermal_range->time_boot_ms, camera_thermal_range->stream_id, camera_thermal_range->camera_device_id, camera_thermal_range->max, camera_thermal_range->max_point_x, camera_thermal_range->max_point_y, camera_thermal_range->min, camera_thermal_range->min_point_x, camera_thermal_range->min_point_y); +} + +/** + * @brief Send a camera_thermal_range message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param stream_id Video Stream ID (1 for first, 2 for second, etc.) + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + * @param max [degC] Temperature max. + * @param max_point_x Temperature max point x value (normalized 0..1, 0 is left, 1 is right), NAN if unknown. + * @param max_point_y Temperature max point y value (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown. + * @param min [degC] Temperature min. + * @param min_point_x Temperature min point x value (normalized 0..1, 0 is left, 1 is right), NAN if unknown. + * @param min_point_y Temperature min point y value (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_thermal_range_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t stream_id, uint8_t camera_device_id, float max, float max_point_x, float max_point_y, float min, float min_point_x, float min_point_y) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, max); + _mav_put_float(buf, 8, max_point_x); + _mav_put_float(buf, 12, max_point_y); + _mav_put_float(buf, 16, min); + _mav_put_float(buf, 20, min_point_x); + _mav_put_float(buf, 24, min_point_y); + _mav_put_uint8_t(buf, 28, stream_id); + _mav_put_uint8_t(buf, 29, camera_device_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE, buf, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_MIN_LEN, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_CRC); +#else + mavlink_camera_thermal_range_t packet; + packet.time_boot_ms = time_boot_ms; + packet.max = max; + packet.max_point_x = max_point_x; + packet.max_point_y = max_point_y; + packet.min = min; + packet.min_point_x = min_point_x; + packet.min_point_y = min_point_y; + packet.stream_id = stream_id; + packet.camera_device_id = camera_device_id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_MIN_LEN, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_CRC); +#endif +} + +/** + * @brief Send a camera_thermal_range message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_camera_thermal_range_send_struct(mavlink_channel_t chan, const mavlink_camera_thermal_range_t* camera_thermal_range) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_camera_thermal_range_send(chan, camera_thermal_range->time_boot_ms, camera_thermal_range->stream_id, camera_thermal_range->camera_device_id, camera_thermal_range->max, camera_thermal_range->max_point_x, camera_thermal_range->max_point_y, camera_thermal_range->min, camera_thermal_range->min_point_x, camera_thermal_range->min_point_y); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE, (const char *)camera_thermal_range, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_MIN_LEN, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_thermal_range_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t stream_id, uint8_t camera_device_id, float max, float max_point_x, float max_point_y, float min, float min_point_x, float min_point_y) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, max); + _mav_put_float(buf, 8, max_point_x); + _mav_put_float(buf, 12, max_point_y); + _mav_put_float(buf, 16, min); + _mav_put_float(buf, 20, min_point_x); + _mav_put_float(buf, 24, min_point_y); + _mav_put_uint8_t(buf, 28, stream_id); + _mav_put_uint8_t(buf, 29, camera_device_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE, buf, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_MIN_LEN, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_CRC); +#else + mavlink_camera_thermal_range_t *packet = (mavlink_camera_thermal_range_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->max = max; + packet->max_point_x = max_point_x; + packet->max_point_y = max_point_y; + packet->min = min; + packet->min_point_x = min_point_x; + packet->min_point_y = min_point_y; + packet->stream_id = stream_id; + packet->camera_device_id = camera_device_id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE, (const char *)packet, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_MIN_LEN, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_THERMAL_RANGE UNPACKING + + +/** + * @brief Get field time_boot_ms from camera_thermal_range message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_camera_thermal_range_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field stream_id from camera_thermal_range message + * + * @return Video Stream ID (1 for first, 2 for second, etc.) + */ +static inline uint8_t mavlink_msg_camera_thermal_range_get_stream_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 28); +} + +/** + * @brief Get field camera_device_id from camera_thermal_range message + * + * @return Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + */ +static inline uint8_t mavlink_msg_camera_thermal_range_get_camera_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 29); +} + +/** + * @brief Get field max from camera_thermal_range message + * + * @return [degC] Temperature max. + */ +static inline float mavlink_msg_camera_thermal_range_get_max(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field max_point_x from camera_thermal_range message + * + * @return Temperature max point x value (normalized 0..1, 0 is left, 1 is right), NAN if unknown. + */ +static inline float mavlink_msg_camera_thermal_range_get_max_point_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field max_point_y from camera_thermal_range message + * + * @return Temperature max point y value (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown. + */ +static inline float mavlink_msg_camera_thermal_range_get_max_point_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field min from camera_thermal_range message + * + * @return [degC] Temperature min. + */ +static inline float mavlink_msg_camera_thermal_range_get_min(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field min_point_x from camera_thermal_range message + * + * @return Temperature min point x value (normalized 0..1, 0 is left, 1 is right), NAN if unknown. + */ +static inline float mavlink_msg_camera_thermal_range_get_min_point_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field min_point_y from camera_thermal_range message + * + * @return Temperature min point y value (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown. + */ +static inline float mavlink_msg_camera_thermal_range_get_min_point_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Decode a camera_thermal_range message into a struct + * + * @param msg The message to decode + * @param camera_thermal_range C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_thermal_range_decode(const mavlink_message_t* msg, mavlink_camera_thermal_range_t* camera_thermal_range) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + camera_thermal_range->time_boot_ms = mavlink_msg_camera_thermal_range_get_time_boot_ms(msg); + camera_thermal_range->max = mavlink_msg_camera_thermal_range_get_max(msg); + camera_thermal_range->max_point_x = mavlink_msg_camera_thermal_range_get_max_point_x(msg); + camera_thermal_range->max_point_y = mavlink_msg_camera_thermal_range_get_max_point_y(msg); + camera_thermal_range->min = mavlink_msg_camera_thermal_range_get_min(msg); + camera_thermal_range->min_point_x = mavlink_msg_camera_thermal_range_get_min_point_x(msg); + camera_thermal_range->min_point_y = mavlink_msg_camera_thermal_range_get_min_point_y(msg); + camera_thermal_range->stream_id = mavlink_msg_camera_thermal_range_get_stream_id(msg); + camera_thermal_range->camera_device_id = mavlink_msg_camera_thermal_range_get_camera_device_id(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN; + memset(camera_thermal_range, 0, MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_LEN); + memcpy(camera_thermal_range, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_tracking_geo_status.h b/lib/main/MAVLink/common/mavlink_msg_camera_tracking_geo_status.h index f4e22b98f96..4876870a5d2 100644 --- a/lib/main/MAVLink/common/mavlink_msg_camera_tracking_geo_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_camera_tracking_geo_status.h @@ -18,11 +18,12 @@ typedef struct __mavlink_camera_tracking_geo_status_t { float hdg; /*< [rad] Heading in radians, in NED. NAN if unknown*/ float hdg_acc; /*< [rad] Accuracy of heading, in NED. NAN if unknown*/ uint8_t tracking_status; /*< Current tracking status*/ + uint8_t camera_device_id; /*< Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id).*/ } mavlink_camera_tracking_geo_status_t; -#define MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN 49 +#define MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN 50 #define MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN 49 -#define MAVLINK_MSG_ID_276_LEN 49 +#define MAVLINK_MSG_ID_276_LEN 50 #define MAVLINK_MSG_ID_276_MIN_LEN 49 #define MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC 18 @@ -34,7 +35,7 @@ typedef struct __mavlink_camera_tracking_geo_status_t { #define MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_GEO_STATUS { \ 276, \ "CAMERA_TRACKING_GEO_STATUS", \ - 13, \ + 14, \ { { "tracking_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_camera_tracking_geo_status_t, tracking_status) }, \ { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_camera_tracking_geo_status_t, lat) }, \ { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_camera_tracking_geo_status_t, lon) }, \ @@ -48,12 +49,13 @@ typedef struct __mavlink_camera_tracking_geo_status_t { { "dist", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_camera_tracking_geo_status_t, dist) }, \ { "hdg", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_camera_tracking_geo_status_t, hdg) }, \ { "hdg_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_camera_tracking_geo_status_t, hdg_acc) }, \ + { "camera_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 49, offsetof(mavlink_camera_tracking_geo_status_t, camera_device_id) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_GEO_STATUS { \ "CAMERA_TRACKING_GEO_STATUS", \ - 13, \ + 14, \ { { "tracking_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_camera_tracking_geo_status_t, tracking_status) }, \ { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_camera_tracking_geo_status_t, lat) }, \ { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_camera_tracking_geo_status_t, lon) }, \ @@ -67,6 +69,7 @@ typedef struct __mavlink_camera_tracking_geo_status_t { { "dist", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_camera_tracking_geo_status_t, dist) }, \ { "hdg", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_camera_tracking_geo_status_t, hdg) }, \ { "hdg_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_camera_tracking_geo_status_t, hdg_acc) }, \ + { "camera_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 49, offsetof(mavlink_camera_tracking_geo_status_t, camera_device_id) }, \ } \ } #endif @@ -90,10 +93,11 @@ typedef struct __mavlink_camera_tracking_geo_status_t { * @param dist [m] Distance between camera and tracked object. NAN if unknown * @param hdg [rad] Heading in radians, in NED. NAN if unknown * @param hdg_acc [rad] Accuracy of heading, in NED. NAN if unknown + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_camera_tracking_geo_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t tracking_status, int32_t lat, int32_t lon, float alt, float h_acc, float v_acc, float vel_n, float vel_e, float vel_d, float vel_acc, float dist, float hdg, float hdg_acc) + uint8_t tracking_status, int32_t lat, int32_t lon, float alt, float h_acc, float v_acc, float vel_n, float vel_e, float vel_d, float vel_acc, float dist, float hdg, float hdg_acc, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN]; @@ -110,6 +114,7 @@ static inline uint16_t mavlink_msg_camera_tracking_geo_status_pack(uint8_t syste _mav_put_float(buf, 40, hdg); _mav_put_float(buf, 44, hdg_acc); _mav_put_uint8_t(buf, 48, tracking_status); + _mav_put_uint8_t(buf, 49, camera_device_id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN); #else @@ -127,6 +132,7 @@ static inline uint16_t mavlink_msg_camera_tracking_geo_status_pack(uint8_t syste packet.hdg = hdg; packet.hdg_acc = hdg_acc; packet.tracking_status = tracking_status; + packet.camera_device_id = camera_device_id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN); #endif @@ -135,6 +141,78 @@ static inline uint16_t mavlink_msg_camera_tracking_geo_status_pack(uint8_t syste return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); } +/** + * @brief Pack a camera_tracking_geo_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param tracking_status Current tracking status + * @param lat [degE7] Latitude of tracked object + * @param lon [degE7] Longitude of tracked object + * @param alt [m] Altitude of tracked object(AMSL, WGS84) + * @param h_acc [m] Horizontal accuracy. NAN if unknown + * @param v_acc [m] Vertical accuracy. NAN if unknown + * @param vel_n [m/s] North velocity of tracked object. NAN if unknown + * @param vel_e [m/s] East velocity of tracked object. NAN if unknown + * @param vel_d [m/s] Down velocity of tracked object. NAN if unknown + * @param vel_acc [m/s] Velocity accuracy. NAN if unknown + * @param dist [m] Distance between camera and tracked object. NAN if unknown + * @param hdg [rad] Heading in radians, in NED. NAN if unknown + * @param hdg_acc [rad] Accuracy of heading, in NED. NAN if unknown + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_tracking_geo_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t tracking_status, int32_t lat, int32_t lon, float alt, float h_acc, float v_acc, float vel_n, float vel_e, float vel_d, float vel_acc, float dist, float hdg, float hdg_acc, uint8_t camera_device_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, h_acc); + _mav_put_float(buf, 16, v_acc); + _mav_put_float(buf, 20, vel_n); + _mav_put_float(buf, 24, vel_e); + _mav_put_float(buf, 28, vel_d); + _mav_put_float(buf, 32, vel_acc); + _mav_put_float(buf, 36, dist); + _mav_put_float(buf, 40, hdg); + _mav_put_float(buf, 44, hdg_acc); + _mav_put_uint8_t(buf, 48, tracking_status); + _mav_put_uint8_t(buf, 49, camera_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN); +#else + mavlink_camera_tracking_geo_status_t packet; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_n = vel_n; + packet.vel_e = vel_e; + packet.vel_d = vel_d; + packet.vel_acc = vel_acc; + packet.dist = dist; + packet.hdg = hdg; + packet.hdg_acc = hdg_acc; + packet.tracking_status = tracking_status; + packet.camera_device_id = camera_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN); +#endif +} + /** * @brief Pack a camera_tracking_geo_status message on a channel * @param system_id ID of this system @@ -154,11 +232,12 @@ static inline uint16_t mavlink_msg_camera_tracking_geo_status_pack(uint8_t syste * @param dist [m] Distance between camera and tracked object. NAN if unknown * @param hdg [rad] Heading in radians, in NED. NAN if unknown * @param hdg_acc [rad] Accuracy of heading, in NED. NAN if unknown + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_camera_tracking_geo_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t tracking_status,int32_t lat,int32_t lon,float alt,float h_acc,float v_acc,float vel_n,float vel_e,float vel_d,float vel_acc,float dist,float hdg,float hdg_acc) + uint8_t tracking_status,int32_t lat,int32_t lon,float alt,float h_acc,float v_acc,float vel_n,float vel_e,float vel_d,float vel_acc,float dist,float hdg,float hdg_acc,uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN]; @@ -175,6 +254,7 @@ static inline uint16_t mavlink_msg_camera_tracking_geo_status_pack_chan(uint8_t _mav_put_float(buf, 40, hdg); _mav_put_float(buf, 44, hdg_acc); _mav_put_uint8_t(buf, 48, tracking_status); + _mav_put_uint8_t(buf, 49, camera_device_id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN); #else @@ -192,6 +272,7 @@ static inline uint16_t mavlink_msg_camera_tracking_geo_status_pack_chan(uint8_t packet.hdg = hdg; packet.hdg_acc = hdg_acc; packet.tracking_status = tracking_status; + packet.camera_device_id = camera_device_id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN); #endif @@ -210,7 +291,7 @@ static inline uint16_t mavlink_msg_camera_tracking_geo_status_pack_chan(uint8_t */ static inline uint16_t mavlink_msg_camera_tracking_geo_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_tracking_geo_status_t* camera_tracking_geo_status) { - return mavlink_msg_camera_tracking_geo_status_pack(system_id, component_id, msg, camera_tracking_geo_status->tracking_status, camera_tracking_geo_status->lat, camera_tracking_geo_status->lon, camera_tracking_geo_status->alt, camera_tracking_geo_status->h_acc, camera_tracking_geo_status->v_acc, camera_tracking_geo_status->vel_n, camera_tracking_geo_status->vel_e, camera_tracking_geo_status->vel_d, camera_tracking_geo_status->vel_acc, camera_tracking_geo_status->dist, camera_tracking_geo_status->hdg, camera_tracking_geo_status->hdg_acc); + return mavlink_msg_camera_tracking_geo_status_pack(system_id, component_id, msg, camera_tracking_geo_status->tracking_status, camera_tracking_geo_status->lat, camera_tracking_geo_status->lon, camera_tracking_geo_status->alt, camera_tracking_geo_status->h_acc, camera_tracking_geo_status->v_acc, camera_tracking_geo_status->vel_n, camera_tracking_geo_status->vel_e, camera_tracking_geo_status->vel_d, camera_tracking_geo_status->vel_acc, camera_tracking_geo_status->dist, camera_tracking_geo_status->hdg, camera_tracking_geo_status->hdg_acc, camera_tracking_geo_status->camera_device_id); } /** @@ -224,7 +305,21 @@ static inline uint16_t mavlink_msg_camera_tracking_geo_status_encode(uint8_t sys */ static inline uint16_t mavlink_msg_camera_tracking_geo_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_tracking_geo_status_t* camera_tracking_geo_status) { - return mavlink_msg_camera_tracking_geo_status_pack_chan(system_id, component_id, chan, msg, camera_tracking_geo_status->tracking_status, camera_tracking_geo_status->lat, camera_tracking_geo_status->lon, camera_tracking_geo_status->alt, camera_tracking_geo_status->h_acc, camera_tracking_geo_status->v_acc, camera_tracking_geo_status->vel_n, camera_tracking_geo_status->vel_e, camera_tracking_geo_status->vel_d, camera_tracking_geo_status->vel_acc, camera_tracking_geo_status->dist, camera_tracking_geo_status->hdg, camera_tracking_geo_status->hdg_acc); + return mavlink_msg_camera_tracking_geo_status_pack_chan(system_id, component_id, chan, msg, camera_tracking_geo_status->tracking_status, camera_tracking_geo_status->lat, camera_tracking_geo_status->lon, camera_tracking_geo_status->alt, camera_tracking_geo_status->h_acc, camera_tracking_geo_status->v_acc, camera_tracking_geo_status->vel_n, camera_tracking_geo_status->vel_e, camera_tracking_geo_status->vel_d, camera_tracking_geo_status->vel_acc, camera_tracking_geo_status->dist, camera_tracking_geo_status->hdg, camera_tracking_geo_status->hdg_acc, camera_tracking_geo_status->camera_device_id); +} + +/** + * @brief Encode a camera_tracking_geo_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param camera_tracking_geo_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_tracking_geo_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_camera_tracking_geo_status_t* camera_tracking_geo_status) +{ + return mavlink_msg_camera_tracking_geo_status_pack_status(system_id, component_id, _status, msg, camera_tracking_geo_status->tracking_status, camera_tracking_geo_status->lat, camera_tracking_geo_status->lon, camera_tracking_geo_status->alt, camera_tracking_geo_status->h_acc, camera_tracking_geo_status->v_acc, camera_tracking_geo_status->vel_n, camera_tracking_geo_status->vel_e, camera_tracking_geo_status->vel_d, camera_tracking_geo_status->vel_acc, camera_tracking_geo_status->dist, camera_tracking_geo_status->hdg, camera_tracking_geo_status->hdg_acc, camera_tracking_geo_status->camera_device_id); } /** @@ -244,10 +339,11 @@ static inline uint16_t mavlink_msg_camera_tracking_geo_status_encode_chan(uint8_ * @param dist [m] Distance between camera and tracked object. NAN if unknown * @param hdg [rad] Heading in radians, in NED. NAN if unknown * @param hdg_acc [rad] Accuracy of heading, in NED. NAN if unknown + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_camera_tracking_geo_status_send(mavlink_channel_t chan, uint8_t tracking_status, int32_t lat, int32_t lon, float alt, float h_acc, float v_acc, float vel_n, float vel_e, float vel_d, float vel_acc, float dist, float hdg, float hdg_acc) +static inline void mavlink_msg_camera_tracking_geo_status_send(mavlink_channel_t chan, uint8_t tracking_status, int32_t lat, int32_t lon, float alt, float h_acc, float v_acc, float vel_n, float vel_e, float vel_d, float vel_acc, float dist, float hdg, float hdg_acc, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN]; @@ -264,6 +360,7 @@ static inline void mavlink_msg_camera_tracking_geo_status_send(mavlink_channel_t _mav_put_float(buf, 40, hdg); _mav_put_float(buf, 44, hdg_acc); _mav_put_uint8_t(buf, 48, tracking_status); + _mav_put_uint8_t(buf, 49, camera_device_id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS, buf, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); #else @@ -281,6 +378,7 @@ static inline void mavlink_msg_camera_tracking_geo_status_send(mavlink_channel_t packet.hdg = hdg; packet.hdg_acc = hdg_acc; packet.tracking_status = tracking_status; + packet.camera_device_id = camera_device_id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); #endif @@ -294,7 +392,7 @@ static inline void mavlink_msg_camera_tracking_geo_status_send(mavlink_channel_t static inline void mavlink_msg_camera_tracking_geo_status_send_struct(mavlink_channel_t chan, const mavlink_camera_tracking_geo_status_t* camera_tracking_geo_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_camera_tracking_geo_status_send(chan, camera_tracking_geo_status->tracking_status, camera_tracking_geo_status->lat, camera_tracking_geo_status->lon, camera_tracking_geo_status->alt, camera_tracking_geo_status->h_acc, camera_tracking_geo_status->v_acc, camera_tracking_geo_status->vel_n, camera_tracking_geo_status->vel_e, camera_tracking_geo_status->vel_d, camera_tracking_geo_status->vel_acc, camera_tracking_geo_status->dist, camera_tracking_geo_status->hdg, camera_tracking_geo_status->hdg_acc); + mavlink_msg_camera_tracking_geo_status_send(chan, camera_tracking_geo_status->tracking_status, camera_tracking_geo_status->lat, camera_tracking_geo_status->lon, camera_tracking_geo_status->alt, camera_tracking_geo_status->h_acc, camera_tracking_geo_status->v_acc, camera_tracking_geo_status->vel_n, camera_tracking_geo_status->vel_e, camera_tracking_geo_status->vel_d, camera_tracking_geo_status->vel_acc, camera_tracking_geo_status->dist, camera_tracking_geo_status->hdg, camera_tracking_geo_status->hdg_acc, camera_tracking_geo_status->camera_device_id); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS, (const char *)camera_tracking_geo_status, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); #endif @@ -302,13 +400,13 @@ static inline void mavlink_msg_camera_tracking_geo_status_send_struct(mavlink_ch #if MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_camera_tracking_geo_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t tracking_status, int32_t lat, int32_t lon, float alt, float h_acc, float v_acc, float vel_n, float vel_e, float vel_d, float vel_acc, float dist, float hdg, float hdg_acc) +static inline void mavlink_msg_camera_tracking_geo_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t tracking_status, int32_t lat, int32_t lon, float alt, float h_acc, float v_acc, float vel_n, float vel_e, float vel_d, float vel_acc, float dist, float hdg, float hdg_acc, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -325,6 +423,7 @@ static inline void mavlink_msg_camera_tracking_geo_status_send_buf(mavlink_messa _mav_put_float(buf, 40, hdg); _mav_put_float(buf, 44, hdg_acc); _mav_put_uint8_t(buf, 48, tracking_status); + _mav_put_uint8_t(buf, 49, camera_device_id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS, buf, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); #else @@ -342,6 +441,7 @@ static inline void mavlink_msg_camera_tracking_geo_status_send_buf(mavlink_messa packet->hdg = hdg; packet->hdg_acc = hdg_acc; packet->tracking_status = tracking_status; + packet->camera_device_id = camera_device_id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS, (const char *)packet, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); #endif @@ -483,6 +583,16 @@ static inline float mavlink_msg_camera_tracking_geo_status_get_hdg_acc(const mav return _MAV_RETURN_float(msg, 44); } +/** + * @brief Get field camera_device_id from camera_tracking_geo_status message + * + * @return Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + */ +static inline uint8_t mavlink_msg_camera_tracking_geo_status_get_camera_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 49); +} + /** * @brief Decode a camera_tracking_geo_status message into a struct * @@ -505,6 +615,7 @@ static inline void mavlink_msg_camera_tracking_geo_status_decode(const mavlink_m camera_tracking_geo_status->hdg = mavlink_msg_camera_tracking_geo_status_get_hdg(msg); camera_tracking_geo_status->hdg_acc = mavlink_msg_camera_tracking_geo_status_get_hdg_acc(msg); camera_tracking_geo_status->tracking_status = mavlink_msg_camera_tracking_geo_status_get_tracking_status(msg); + camera_tracking_geo_status->camera_device_id = mavlink_msg_camera_tracking_geo_status_get_camera_device_id(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN; memset(camera_tracking_geo_status, 0, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_tracking_image_status.h b/lib/main/MAVLink/common/mavlink_msg_camera_tracking_image_status.h index 5f703fce5d4..48dcd6ef91e 100644 --- a/lib/main/MAVLink/common/mavlink_msg_camera_tracking_image_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_camera_tracking_image_status.h @@ -15,11 +15,12 @@ typedef struct __mavlink_camera_tracking_image_status_t { uint8_t tracking_status; /*< Current tracking status*/ uint8_t tracking_mode; /*< Current tracking mode*/ uint8_t target_data; /*< Defines location of target data*/ + uint8_t camera_device_id; /*< Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id).*/ } mavlink_camera_tracking_image_status_t; -#define MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN 31 +#define MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN 32 #define MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN 31 -#define MAVLINK_MSG_ID_275_LEN 31 +#define MAVLINK_MSG_ID_275_LEN 32 #define MAVLINK_MSG_ID_275_MIN_LEN 31 #define MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC 126 @@ -31,7 +32,7 @@ typedef struct __mavlink_camera_tracking_image_status_t { #define MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_IMAGE_STATUS { \ 275, \ "CAMERA_TRACKING_IMAGE_STATUS", \ - 10, \ + 11, \ { { "tracking_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_camera_tracking_image_status_t, tracking_status) }, \ { "tracking_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_camera_tracking_image_status_t, tracking_mode) }, \ { "target_data", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_camera_tracking_image_status_t, target_data) }, \ @@ -42,12 +43,13 @@ typedef struct __mavlink_camera_tracking_image_status_t { { "rec_top_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_tracking_image_status_t, rec_top_y) }, \ { "rec_bottom_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_camera_tracking_image_status_t, rec_bottom_x) }, \ { "rec_bottom_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_camera_tracking_image_status_t, rec_bottom_y) }, \ + { "camera_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_camera_tracking_image_status_t, camera_device_id) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_IMAGE_STATUS { \ "CAMERA_TRACKING_IMAGE_STATUS", \ - 10, \ + 11, \ { { "tracking_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_camera_tracking_image_status_t, tracking_status) }, \ { "tracking_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_camera_tracking_image_status_t, tracking_mode) }, \ { "target_data", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_camera_tracking_image_status_t, target_data) }, \ @@ -58,6 +60,7 @@ typedef struct __mavlink_camera_tracking_image_status_t { { "rec_top_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_tracking_image_status_t, rec_top_y) }, \ { "rec_bottom_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_camera_tracking_image_status_t, rec_bottom_x) }, \ { "rec_bottom_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_camera_tracking_image_status_t, rec_bottom_y) }, \ + { "camera_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_camera_tracking_image_status_t, camera_device_id) }, \ } \ } #endif @@ -78,10 +81,11 @@ typedef struct __mavlink_camera_tracking_image_status_t { * @param rec_top_y Current tracked rectangle top y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown * @param rec_bottom_x Current tracked rectangle bottom x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown * @param rec_bottom_y Current tracked rectangle bottom y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_camera_tracking_image_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t tracking_status, uint8_t tracking_mode, uint8_t target_data, float point_x, float point_y, float radius, float rec_top_x, float rec_top_y, float rec_bottom_x, float rec_bottom_y) + uint8_t tracking_status, uint8_t tracking_mode, uint8_t target_data, float point_x, float point_y, float radius, float rec_top_x, float rec_top_y, float rec_bottom_x, float rec_bottom_y, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN]; @@ -95,6 +99,7 @@ static inline uint16_t mavlink_msg_camera_tracking_image_status_pack(uint8_t sys _mav_put_uint8_t(buf, 28, tracking_status); _mav_put_uint8_t(buf, 29, tracking_mode); _mav_put_uint8_t(buf, 30, target_data); + _mav_put_uint8_t(buf, 31, camera_device_id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN); #else @@ -109,6 +114,7 @@ static inline uint16_t mavlink_msg_camera_tracking_image_status_pack(uint8_t sys packet.tracking_status = tracking_status; packet.tracking_mode = tracking_mode; packet.target_data = target_data; + packet.camera_device_id = camera_device_id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN); #endif @@ -117,6 +123,69 @@ static inline uint16_t mavlink_msg_camera_tracking_image_status_pack(uint8_t sys return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); } +/** + * @brief Pack a camera_tracking_image_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param tracking_status Current tracking status + * @param tracking_mode Current tracking mode + * @param target_data Defines location of target data + * @param point_x Current tracked point x value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param point_y Current tracked point y value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @param radius Current tracked radius if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is image left, 1 is image right), NAN if unknown + * @param rec_top_x Current tracked rectangle top x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param rec_top_y Current tracked rectangle top y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @param rec_bottom_x Current tracked rectangle bottom x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param rec_bottom_y Current tracked rectangle bottom y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_tracking_image_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t tracking_status, uint8_t tracking_mode, uint8_t target_data, float point_x, float point_y, float radius, float rec_top_x, float rec_top_y, float rec_bottom_x, float rec_bottom_y, uint8_t camera_device_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN]; + _mav_put_float(buf, 0, point_x); + _mav_put_float(buf, 4, point_y); + _mav_put_float(buf, 8, radius); + _mav_put_float(buf, 12, rec_top_x); + _mav_put_float(buf, 16, rec_top_y); + _mav_put_float(buf, 20, rec_bottom_x); + _mav_put_float(buf, 24, rec_bottom_y); + _mav_put_uint8_t(buf, 28, tracking_status); + _mav_put_uint8_t(buf, 29, tracking_mode); + _mav_put_uint8_t(buf, 30, target_data); + _mav_put_uint8_t(buf, 31, camera_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN); +#else + mavlink_camera_tracking_image_status_t packet; + packet.point_x = point_x; + packet.point_y = point_y; + packet.radius = radius; + packet.rec_top_x = rec_top_x; + packet.rec_top_y = rec_top_y; + packet.rec_bottom_x = rec_bottom_x; + packet.rec_bottom_y = rec_bottom_y; + packet.tracking_status = tracking_status; + packet.tracking_mode = tracking_mode; + packet.target_data = target_data; + packet.camera_device_id = camera_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN); +#endif +} + /** * @brief Pack a camera_tracking_image_status message on a channel * @param system_id ID of this system @@ -133,11 +202,12 @@ static inline uint16_t mavlink_msg_camera_tracking_image_status_pack(uint8_t sys * @param rec_top_y Current tracked rectangle top y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown * @param rec_bottom_x Current tracked rectangle bottom x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown * @param rec_bottom_y Current tracked rectangle bottom y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_camera_tracking_image_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t tracking_status,uint8_t tracking_mode,uint8_t target_data,float point_x,float point_y,float radius,float rec_top_x,float rec_top_y,float rec_bottom_x,float rec_bottom_y) + uint8_t tracking_status,uint8_t tracking_mode,uint8_t target_data,float point_x,float point_y,float radius,float rec_top_x,float rec_top_y,float rec_bottom_x,float rec_bottom_y,uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN]; @@ -151,6 +221,7 @@ static inline uint16_t mavlink_msg_camera_tracking_image_status_pack_chan(uint8_ _mav_put_uint8_t(buf, 28, tracking_status); _mav_put_uint8_t(buf, 29, tracking_mode); _mav_put_uint8_t(buf, 30, target_data); + _mav_put_uint8_t(buf, 31, camera_device_id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN); #else @@ -165,6 +236,7 @@ static inline uint16_t mavlink_msg_camera_tracking_image_status_pack_chan(uint8_ packet.tracking_status = tracking_status; packet.tracking_mode = tracking_mode; packet.target_data = target_data; + packet.camera_device_id = camera_device_id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN); #endif @@ -183,7 +255,7 @@ static inline uint16_t mavlink_msg_camera_tracking_image_status_pack_chan(uint8_ */ static inline uint16_t mavlink_msg_camera_tracking_image_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_tracking_image_status_t* camera_tracking_image_status) { - return mavlink_msg_camera_tracking_image_status_pack(system_id, component_id, msg, camera_tracking_image_status->tracking_status, camera_tracking_image_status->tracking_mode, camera_tracking_image_status->target_data, camera_tracking_image_status->point_x, camera_tracking_image_status->point_y, camera_tracking_image_status->radius, camera_tracking_image_status->rec_top_x, camera_tracking_image_status->rec_top_y, camera_tracking_image_status->rec_bottom_x, camera_tracking_image_status->rec_bottom_y); + return mavlink_msg_camera_tracking_image_status_pack(system_id, component_id, msg, camera_tracking_image_status->tracking_status, camera_tracking_image_status->tracking_mode, camera_tracking_image_status->target_data, camera_tracking_image_status->point_x, camera_tracking_image_status->point_y, camera_tracking_image_status->radius, camera_tracking_image_status->rec_top_x, camera_tracking_image_status->rec_top_y, camera_tracking_image_status->rec_bottom_x, camera_tracking_image_status->rec_bottom_y, camera_tracking_image_status->camera_device_id); } /** @@ -197,7 +269,21 @@ static inline uint16_t mavlink_msg_camera_tracking_image_status_encode(uint8_t s */ static inline uint16_t mavlink_msg_camera_tracking_image_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_tracking_image_status_t* camera_tracking_image_status) { - return mavlink_msg_camera_tracking_image_status_pack_chan(system_id, component_id, chan, msg, camera_tracking_image_status->tracking_status, camera_tracking_image_status->tracking_mode, camera_tracking_image_status->target_data, camera_tracking_image_status->point_x, camera_tracking_image_status->point_y, camera_tracking_image_status->radius, camera_tracking_image_status->rec_top_x, camera_tracking_image_status->rec_top_y, camera_tracking_image_status->rec_bottom_x, camera_tracking_image_status->rec_bottom_y); + return mavlink_msg_camera_tracking_image_status_pack_chan(system_id, component_id, chan, msg, camera_tracking_image_status->tracking_status, camera_tracking_image_status->tracking_mode, camera_tracking_image_status->target_data, camera_tracking_image_status->point_x, camera_tracking_image_status->point_y, camera_tracking_image_status->radius, camera_tracking_image_status->rec_top_x, camera_tracking_image_status->rec_top_y, camera_tracking_image_status->rec_bottom_x, camera_tracking_image_status->rec_bottom_y, camera_tracking_image_status->camera_device_id); +} + +/** + * @brief Encode a camera_tracking_image_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param camera_tracking_image_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_tracking_image_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_camera_tracking_image_status_t* camera_tracking_image_status) +{ + return mavlink_msg_camera_tracking_image_status_pack_status(system_id, component_id, _status, msg, camera_tracking_image_status->tracking_status, camera_tracking_image_status->tracking_mode, camera_tracking_image_status->target_data, camera_tracking_image_status->point_x, camera_tracking_image_status->point_y, camera_tracking_image_status->radius, camera_tracking_image_status->rec_top_x, camera_tracking_image_status->rec_top_y, camera_tracking_image_status->rec_bottom_x, camera_tracking_image_status->rec_bottom_y, camera_tracking_image_status->camera_device_id); } /** @@ -214,10 +300,11 @@ static inline uint16_t mavlink_msg_camera_tracking_image_status_encode_chan(uint * @param rec_top_y Current tracked rectangle top y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown * @param rec_bottom_x Current tracked rectangle bottom x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown * @param rec_bottom_y Current tracked rectangle bottom y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_camera_tracking_image_status_send(mavlink_channel_t chan, uint8_t tracking_status, uint8_t tracking_mode, uint8_t target_data, float point_x, float point_y, float radius, float rec_top_x, float rec_top_y, float rec_bottom_x, float rec_bottom_y) +static inline void mavlink_msg_camera_tracking_image_status_send(mavlink_channel_t chan, uint8_t tracking_status, uint8_t tracking_mode, uint8_t target_data, float point_x, float point_y, float radius, float rec_top_x, float rec_top_y, float rec_bottom_x, float rec_bottom_y, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN]; @@ -231,6 +318,7 @@ static inline void mavlink_msg_camera_tracking_image_status_send(mavlink_channel _mav_put_uint8_t(buf, 28, tracking_status); _mav_put_uint8_t(buf, 29, tracking_mode); _mav_put_uint8_t(buf, 30, target_data); + _mav_put_uint8_t(buf, 31, camera_device_id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, buf, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); #else @@ -245,6 +333,7 @@ static inline void mavlink_msg_camera_tracking_image_status_send(mavlink_channel packet.tracking_status = tracking_status; packet.tracking_mode = tracking_mode; packet.target_data = target_data; + packet.camera_device_id = camera_device_id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); #endif @@ -258,7 +347,7 @@ static inline void mavlink_msg_camera_tracking_image_status_send(mavlink_channel static inline void mavlink_msg_camera_tracking_image_status_send_struct(mavlink_channel_t chan, const mavlink_camera_tracking_image_status_t* camera_tracking_image_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_camera_tracking_image_status_send(chan, camera_tracking_image_status->tracking_status, camera_tracking_image_status->tracking_mode, camera_tracking_image_status->target_data, camera_tracking_image_status->point_x, camera_tracking_image_status->point_y, camera_tracking_image_status->radius, camera_tracking_image_status->rec_top_x, camera_tracking_image_status->rec_top_y, camera_tracking_image_status->rec_bottom_x, camera_tracking_image_status->rec_bottom_y); + mavlink_msg_camera_tracking_image_status_send(chan, camera_tracking_image_status->tracking_status, camera_tracking_image_status->tracking_mode, camera_tracking_image_status->target_data, camera_tracking_image_status->point_x, camera_tracking_image_status->point_y, camera_tracking_image_status->radius, camera_tracking_image_status->rec_top_x, camera_tracking_image_status->rec_top_y, camera_tracking_image_status->rec_bottom_x, camera_tracking_image_status->rec_bottom_y, camera_tracking_image_status->camera_device_id); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, (const char *)camera_tracking_image_status, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); #endif @@ -266,13 +355,13 @@ static inline void mavlink_msg_camera_tracking_image_status_send_struct(mavlink_ #if MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_camera_tracking_image_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t tracking_status, uint8_t tracking_mode, uint8_t target_data, float point_x, float point_y, float radius, float rec_top_x, float rec_top_y, float rec_bottom_x, float rec_bottom_y) +static inline void mavlink_msg_camera_tracking_image_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t tracking_status, uint8_t tracking_mode, uint8_t target_data, float point_x, float point_y, float radius, float rec_top_x, float rec_top_y, float rec_bottom_x, float rec_bottom_y, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -286,6 +375,7 @@ static inline void mavlink_msg_camera_tracking_image_status_send_buf(mavlink_mes _mav_put_uint8_t(buf, 28, tracking_status); _mav_put_uint8_t(buf, 29, tracking_mode); _mav_put_uint8_t(buf, 30, target_data); + _mav_put_uint8_t(buf, 31, camera_device_id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, buf, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); #else @@ -300,6 +390,7 @@ static inline void mavlink_msg_camera_tracking_image_status_send_buf(mavlink_mes packet->tracking_status = tracking_status; packet->tracking_mode = tracking_mode; packet->target_data = target_data; + packet->camera_device_id = camera_device_id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, (const char *)packet, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); #endif @@ -411,6 +502,16 @@ static inline float mavlink_msg_camera_tracking_image_status_get_rec_bottom_y(co return _MAV_RETURN_float(msg, 24); } +/** + * @brief Get field camera_device_id from camera_tracking_image_status message + * + * @return Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + */ +static inline uint8_t mavlink_msg_camera_tracking_image_status_get_camera_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 31); +} + /** * @brief Decode a camera_tracking_image_status message into a struct * @@ -430,6 +531,7 @@ static inline void mavlink_msg_camera_tracking_image_status_decode(const mavlink camera_tracking_image_status->tracking_status = mavlink_msg_camera_tracking_image_status_get_tracking_status(msg); camera_tracking_image_status->tracking_mode = mavlink_msg_camera_tracking_image_status_get_tracking_mode(msg); camera_tracking_image_status->target_data = mavlink_msg_camera_tracking_image_status_get_target_data(msg); + camera_tracking_image_status->camera_device_id = mavlink_msg_camera_tracking_image_status_get_camera_device_id(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN; memset(camera_tracking_image_status, 0, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_trigger.h b/lib/main/MAVLink/common/mavlink_msg_camera_trigger.h index ee2ad8e9b08..687a9a06ccd 100755 --- a/lib/main/MAVLink/common/mavlink_msg_camera_trigger.h +++ b/lib/main/MAVLink/common/mavlink_msg_camera_trigger.h @@ -69,6 +69,42 @@ static inline uint16_t mavlink_msg_camera_trigger_pack(uint8_t system_id, uint8_ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_TRIGGER_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_CRC); } +/** + * @brief Pack a camera_trigger message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp for image frame (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param seq Image frame sequence + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_trigger_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint32_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, seq); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); +#else + mavlink_camera_trigger_t packet; + packet.time_usec = time_usec; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_TRIGGER; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAMERA_TRIGGER_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAMERA_TRIGGER_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN); +#endif +} + /** * @brief Pack a camera_trigger message on a channel * @param system_id ID of this system @@ -128,6 +164,20 @@ static inline uint16_t mavlink_msg_camera_trigger_encode_chan(uint8_t system_id, return mavlink_msg_camera_trigger_pack_chan(system_id, component_id, chan, msg, camera_trigger->time_usec, camera_trigger->seq); } +/** + * @brief Encode a camera_trigger struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param camera_trigger C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_trigger_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_camera_trigger_t* camera_trigger) +{ + return mavlink_msg_camera_trigger_pack_status(system_id, component_id, _status, msg, camera_trigger->time_usec, camera_trigger->seq); +} + /** * @brief Send a camera_trigger message * @param chan MAVLink channel to send the message @@ -170,7 +220,7 @@ static inline void mavlink_msg_camera_trigger_send_struct(mavlink_channel_t chan #if MAVLINK_MSG_ID_CAMERA_TRIGGER_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_can_filter_modify.h b/lib/main/MAVLink/common/mavlink_msg_can_filter_modify.h new file mode 100644 index 00000000000..45d73011fcd --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_can_filter_modify.h @@ -0,0 +1,390 @@ +#pragma once +// MESSAGE CAN_FILTER_MODIFY PACKING + +#define MAVLINK_MSG_ID_CAN_FILTER_MODIFY 388 + + +typedef struct __mavlink_can_filter_modify_t { + uint16_t ids[16]; /*< filter IDs, length num_ids*/ + uint8_t target_system; /*< System ID.*/ + uint8_t target_component; /*< Component ID.*/ + uint8_t bus; /*< bus number*/ + uint8_t operation; /*< what operation to perform on the filter list. See CAN_FILTER_OP enum.*/ + uint8_t num_ids; /*< number of IDs in filter list*/ +} mavlink_can_filter_modify_t; + +#define MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN 37 +#define MAVLINK_MSG_ID_CAN_FILTER_MODIFY_MIN_LEN 37 +#define MAVLINK_MSG_ID_388_LEN 37 +#define MAVLINK_MSG_ID_388_MIN_LEN 37 + +#define MAVLINK_MSG_ID_CAN_FILTER_MODIFY_CRC 8 +#define MAVLINK_MSG_ID_388_CRC 8 + +#define MAVLINK_MSG_CAN_FILTER_MODIFY_FIELD_IDS_LEN 16 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CAN_FILTER_MODIFY { \ + 388, \ + "CAN_FILTER_MODIFY", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_can_filter_modify_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_can_filter_modify_t, target_component) }, \ + { "bus", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_can_filter_modify_t, bus) }, \ + { "operation", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_can_filter_modify_t, operation) }, \ + { "num_ids", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_can_filter_modify_t, num_ids) }, \ + { "ids", NULL, MAVLINK_TYPE_UINT16_T, 16, 0, offsetof(mavlink_can_filter_modify_t, ids) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CAN_FILTER_MODIFY { \ + "CAN_FILTER_MODIFY", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_can_filter_modify_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_can_filter_modify_t, target_component) }, \ + { "bus", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_can_filter_modify_t, bus) }, \ + { "operation", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_can_filter_modify_t, operation) }, \ + { "num_ids", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_can_filter_modify_t, num_ids) }, \ + { "ids", NULL, MAVLINK_TYPE_UINT16_T, 16, 0, offsetof(mavlink_can_filter_modify_t, ids) }, \ + } \ +} +#endif + +/** + * @brief Pack a can_filter_modify message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID. + * @param target_component Component ID. + * @param bus bus number + * @param operation what operation to perform on the filter list. See CAN_FILTER_OP enum. + * @param num_ids number of IDs in filter list + * @param ids filter IDs, length num_ids + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_can_filter_modify_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t bus, uint8_t operation, uint8_t num_ids, const uint16_t *ids) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN]; + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, bus); + _mav_put_uint8_t(buf, 35, operation); + _mav_put_uint8_t(buf, 36, num_ids); + _mav_put_uint16_t_array(buf, 0, ids, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN); +#else + mavlink_can_filter_modify_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.bus = bus; + packet.operation = operation; + packet.num_ids = num_ids; + mav_array_assign_uint16_t(packet.ids, ids, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAN_FILTER_MODIFY; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_MIN_LEN, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_CRC); +} + +/** + * @brief Pack a can_filter_modify message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID. + * @param target_component Component ID. + * @param bus bus number + * @param operation what operation to perform on the filter list. See CAN_FILTER_OP enum. + * @param num_ids number of IDs in filter list + * @param ids filter IDs, length num_ids + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_can_filter_modify_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t bus, uint8_t operation, uint8_t num_ids, const uint16_t *ids) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN]; + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, bus); + _mav_put_uint8_t(buf, 35, operation); + _mav_put_uint8_t(buf, 36, num_ids); + _mav_put_uint16_t_array(buf, 0, ids, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN); +#else + mavlink_can_filter_modify_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.bus = bus; + packet.operation = operation; + packet.num_ids = num_ids; + mav_array_memcpy(packet.ids, ids, sizeof(uint16_t)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAN_FILTER_MODIFY; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_MIN_LEN, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_MIN_LEN, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN); +#endif +} + +/** + * @brief Pack a can_filter_modify message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID. + * @param target_component Component ID. + * @param bus bus number + * @param operation what operation to perform on the filter list. See CAN_FILTER_OP enum. + * @param num_ids number of IDs in filter list + * @param ids filter IDs, length num_ids + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_can_filter_modify_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t bus,uint8_t operation,uint8_t num_ids,const uint16_t *ids) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN]; + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, bus); + _mav_put_uint8_t(buf, 35, operation); + _mav_put_uint8_t(buf, 36, num_ids); + _mav_put_uint16_t_array(buf, 0, ids, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN); +#else + mavlink_can_filter_modify_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.bus = bus; + packet.operation = operation; + packet.num_ids = num_ids; + mav_array_assign_uint16_t(packet.ids, ids, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAN_FILTER_MODIFY; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_MIN_LEN, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_CRC); +} + +/** + * @brief Encode a can_filter_modify struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param can_filter_modify C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_can_filter_modify_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_can_filter_modify_t* can_filter_modify) +{ + return mavlink_msg_can_filter_modify_pack(system_id, component_id, msg, can_filter_modify->target_system, can_filter_modify->target_component, can_filter_modify->bus, can_filter_modify->operation, can_filter_modify->num_ids, can_filter_modify->ids); +} + +/** + * @brief Encode a can_filter_modify struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param can_filter_modify C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_can_filter_modify_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_can_filter_modify_t* can_filter_modify) +{ + return mavlink_msg_can_filter_modify_pack_chan(system_id, component_id, chan, msg, can_filter_modify->target_system, can_filter_modify->target_component, can_filter_modify->bus, can_filter_modify->operation, can_filter_modify->num_ids, can_filter_modify->ids); +} + +/** + * @brief Encode a can_filter_modify struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param can_filter_modify C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_can_filter_modify_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_can_filter_modify_t* can_filter_modify) +{ + return mavlink_msg_can_filter_modify_pack_status(system_id, component_id, _status, msg, can_filter_modify->target_system, can_filter_modify->target_component, can_filter_modify->bus, can_filter_modify->operation, can_filter_modify->num_ids, can_filter_modify->ids); +} + +/** + * @brief Send a can_filter_modify message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID. + * @param target_component Component ID. + * @param bus bus number + * @param operation what operation to perform on the filter list. See CAN_FILTER_OP enum. + * @param num_ids number of IDs in filter list + * @param ids filter IDs, length num_ids + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_can_filter_modify_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t bus, uint8_t operation, uint8_t num_ids, const uint16_t *ids) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN]; + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, bus); + _mav_put_uint8_t(buf, 35, operation); + _mav_put_uint8_t(buf, 36, num_ids); + _mav_put_uint16_t_array(buf, 0, ids, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAN_FILTER_MODIFY, buf, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_MIN_LEN, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_CRC); +#else + mavlink_can_filter_modify_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.bus = bus; + packet.operation = operation; + packet.num_ids = num_ids; + mav_array_assign_uint16_t(packet.ids, ids, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAN_FILTER_MODIFY, (const char *)&packet, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_MIN_LEN, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_CRC); +#endif +} + +/** + * @brief Send a can_filter_modify message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_can_filter_modify_send_struct(mavlink_channel_t chan, const mavlink_can_filter_modify_t* can_filter_modify) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_can_filter_modify_send(chan, can_filter_modify->target_system, can_filter_modify->target_component, can_filter_modify->bus, can_filter_modify->operation, can_filter_modify->num_ids, can_filter_modify->ids); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAN_FILTER_MODIFY, (const char *)can_filter_modify, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_MIN_LEN, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_can_filter_modify_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t bus, uint8_t operation, uint8_t num_ids, const uint16_t *ids) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, bus); + _mav_put_uint8_t(buf, 35, operation); + _mav_put_uint8_t(buf, 36, num_ids); + _mav_put_uint16_t_array(buf, 0, ids, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAN_FILTER_MODIFY, buf, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_MIN_LEN, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_CRC); +#else + mavlink_can_filter_modify_t *packet = (mavlink_can_filter_modify_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->bus = bus; + packet->operation = operation; + packet->num_ids = num_ids; + mav_array_assign_uint16_t(packet->ids, ids, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAN_FILTER_MODIFY, (const char *)packet, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_MIN_LEN, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CAN_FILTER_MODIFY UNPACKING + + +/** + * @brief Get field target_system from can_filter_modify message + * + * @return System ID. + */ +static inline uint8_t mavlink_msg_can_filter_modify_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field target_component from can_filter_modify message + * + * @return Component ID. + */ +static inline uint8_t mavlink_msg_can_filter_modify_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field bus from can_filter_modify message + * + * @return bus number + */ +static inline uint8_t mavlink_msg_can_filter_modify_get_bus(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field operation from can_filter_modify message + * + * @return what operation to perform on the filter list. See CAN_FILTER_OP enum. + */ +static inline uint8_t mavlink_msg_can_filter_modify_get_operation(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 35); +} + +/** + * @brief Get field num_ids from can_filter_modify message + * + * @return number of IDs in filter list + */ +static inline uint8_t mavlink_msg_can_filter_modify_get_num_ids(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 36); +} + +/** + * @brief Get field ids from can_filter_modify message + * + * @return filter IDs, length num_ids + */ +static inline uint16_t mavlink_msg_can_filter_modify_get_ids(const mavlink_message_t* msg, uint16_t *ids) +{ + return _MAV_RETURN_uint16_t_array(msg, ids, 16, 0); +} + +/** + * @brief Decode a can_filter_modify message into a struct + * + * @param msg The message to decode + * @param can_filter_modify C-struct to decode the message contents into + */ +static inline void mavlink_msg_can_filter_modify_decode(const mavlink_message_t* msg, mavlink_can_filter_modify_t* can_filter_modify) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_can_filter_modify_get_ids(msg, can_filter_modify->ids); + can_filter_modify->target_system = mavlink_msg_can_filter_modify_get_target_system(msg); + can_filter_modify->target_component = mavlink_msg_can_filter_modify_get_target_component(msg); + can_filter_modify->bus = mavlink_msg_can_filter_modify_get_bus(msg); + can_filter_modify->operation = mavlink_msg_can_filter_modify_get_operation(msg); + can_filter_modify->num_ids = mavlink_msg_can_filter_modify_get_num_ids(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN? msg->len : MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN; + memset(can_filter_modify, 0, MAVLINK_MSG_ID_CAN_FILTER_MODIFY_LEN); + memcpy(can_filter_modify, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_can_frame.h b/lib/main/MAVLink/common/mavlink_msg_can_frame.h new file mode 100644 index 00000000000..a97caa78318 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_can_frame.h @@ -0,0 +1,390 @@ +#pragma once +// MESSAGE CAN_FRAME PACKING + +#define MAVLINK_MSG_ID_CAN_FRAME 386 + + +typedef struct __mavlink_can_frame_t { + uint32_t id; /*< Frame ID*/ + uint8_t target_system; /*< System ID.*/ + uint8_t target_component; /*< Component ID.*/ + uint8_t bus; /*< Bus number*/ + uint8_t len; /*< Frame length*/ + uint8_t data[8]; /*< Frame data*/ +} mavlink_can_frame_t; + +#define MAVLINK_MSG_ID_CAN_FRAME_LEN 16 +#define MAVLINK_MSG_ID_CAN_FRAME_MIN_LEN 16 +#define MAVLINK_MSG_ID_386_LEN 16 +#define MAVLINK_MSG_ID_386_MIN_LEN 16 + +#define MAVLINK_MSG_ID_CAN_FRAME_CRC 132 +#define MAVLINK_MSG_ID_386_CRC 132 + +#define MAVLINK_MSG_CAN_FRAME_FIELD_DATA_LEN 8 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CAN_FRAME { \ + 386, \ + "CAN_FRAME", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_can_frame_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_can_frame_t, target_component) }, \ + { "bus", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_can_frame_t, bus) }, \ + { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_can_frame_t, len) }, \ + { "id", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_can_frame_t, id) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 8, 8, offsetof(mavlink_can_frame_t, data) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CAN_FRAME { \ + "CAN_FRAME", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_can_frame_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_can_frame_t, target_component) }, \ + { "bus", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_can_frame_t, bus) }, \ + { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_can_frame_t, len) }, \ + { "id", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_can_frame_t, id) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 8, 8, offsetof(mavlink_can_frame_t, data) }, \ + } \ +} +#endif + +/** + * @brief Pack a can_frame message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID. + * @param target_component Component ID. + * @param bus Bus number + * @param len Frame length + * @param id Frame ID + * @param data Frame data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_can_frame_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t bus, uint8_t len, uint32_t id, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAN_FRAME_LEN]; + _mav_put_uint32_t(buf, 0, id); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, bus); + _mav_put_uint8_t(buf, 7, len); + _mav_put_uint8_t_array(buf, 8, data, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAN_FRAME_LEN); +#else + mavlink_can_frame_t packet; + packet.id = id; + packet.target_system = target_system; + packet.target_component = target_component; + packet.bus = bus; + packet.len = len; + mav_array_assign_uint8_t(packet.data, data, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAN_FRAME_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAN_FRAME; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAN_FRAME_MIN_LEN, MAVLINK_MSG_ID_CAN_FRAME_LEN, MAVLINK_MSG_ID_CAN_FRAME_CRC); +} + +/** + * @brief Pack a can_frame message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID. + * @param target_component Component ID. + * @param bus Bus number + * @param len Frame length + * @param id Frame ID + * @param data Frame data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_can_frame_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t bus, uint8_t len, uint32_t id, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAN_FRAME_LEN]; + _mav_put_uint32_t(buf, 0, id); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, bus); + _mav_put_uint8_t(buf, 7, len); + _mav_put_uint8_t_array(buf, 8, data, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAN_FRAME_LEN); +#else + mavlink_can_frame_t packet; + packet.id = id; + packet.target_system = target_system; + packet.target_component = target_component; + packet.bus = bus; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAN_FRAME_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAN_FRAME; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAN_FRAME_MIN_LEN, MAVLINK_MSG_ID_CAN_FRAME_LEN, MAVLINK_MSG_ID_CAN_FRAME_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CAN_FRAME_MIN_LEN, MAVLINK_MSG_ID_CAN_FRAME_LEN); +#endif +} + +/** + * @brief Pack a can_frame message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID. + * @param target_component Component ID. + * @param bus Bus number + * @param len Frame length + * @param id Frame ID + * @param data Frame data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_can_frame_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t bus,uint8_t len,uint32_t id,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAN_FRAME_LEN]; + _mav_put_uint32_t(buf, 0, id); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, bus); + _mav_put_uint8_t(buf, 7, len); + _mav_put_uint8_t_array(buf, 8, data, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAN_FRAME_LEN); +#else + mavlink_can_frame_t packet; + packet.id = id; + packet.target_system = target_system; + packet.target_component = target_component; + packet.bus = bus; + packet.len = len; + mav_array_assign_uint8_t(packet.data, data, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAN_FRAME_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAN_FRAME; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAN_FRAME_MIN_LEN, MAVLINK_MSG_ID_CAN_FRAME_LEN, MAVLINK_MSG_ID_CAN_FRAME_CRC); +} + +/** + * @brief Encode a can_frame struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param can_frame C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_can_frame_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_can_frame_t* can_frame) +{ + return mavlink_msg_can_frame_pack(system_id, component_id, msg, can_frame->target_system, can_frame->target_component, can_frame->bus, can_frame->len, can_frame->id, can_frame->data); +} + +/** + * @brief Encode a can_frame struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param can_frame C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_can_frame_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_can_frame_t* can_frame) +{ + return mavlink_msg_can_frame_pack_chan(system_id, component_id, chan, msg, can_frame->target_system, can_frame->target_component, can_frame->bus, can_frame->len, can_frame->id, can_frame->data); +} + +/** + * @brief Encode a can_frame struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param can_frame C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_can_frame_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_can_frame_t* can_frame) +{ + return mavlink_msg_can_frame_pack_status(system_id, component_id, _status, msg, can_frame->target_system, can_frame->target_component, can_frame->bus, can_frame->len, can_frame->id, can_frame->data); +} + +/** + * @brief Send a can_frame message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID. + * @param target_component Component ID. + * @param bus Bus number + * @param len Frame length + * @param id Frame ID + * @param data Frame data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_can_frame_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t bus, uint8_t len, uint32_t id, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAN_FRAME_LEN]; + _mav_put_uint32_t(buf, 0, id); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, bus); + _mav_put_uint8_t(buf, 7, len); + _mav_put_uint8_t_array(buf, 8, data, 8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAN_FRAME, buf, MAVLINK_MSG_ID_CAN_FRAME_MIN_LEN, MAVLINK_MSG_ID_CAN_FRAME_LEN, MAVLINK_MSG_ID_CAN_FRAME_CRC); +#else + mavlink_can_frame_t packet; + packet.id = id; + packet.target_system = target_system; + packet.target_component = target_component; + packet.bus = bus; + packet.len = len; + mav_array_assign_uint8_t(packet.data, data, 8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAN_FRAME, (const char *)&packet, MAVLINK_MSG_ID_CAN_FRAME_MIN_LEN, MAVLINK_MSG_ID_CAN_FRAME_LEN, MAVLINK_MSG_ID_CAN_FRAME_CRC); +#endif +} + +/** + * @brief Send a can_frame message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_can_frame_send_struct(mavlink_channel_t chan, const mavlink_can_frame_t* can_frame) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_can_frame_send(chan, can_frame->target_system, can_frame->target_component, can_frame->bus, can_frame->len, can_frame->id, can_frame->data); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAN_FRAME, (const char *)can_frame, MAVLINK_MSG_ID_CAN_FRAME_MIN_LEN, MAVLINK_MSG_ID_CAN_FRAME_LEN, MAVLINK_MSG_ID_CAN_FRAME_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CAN_FRAME_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_can_frame_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t bus, uint8_t len, uint32_t id, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, id); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, bus); + _mav_put_uint8_t(buf, 7, len); + _mav_put_uint8_t_array(buf, 8, data, 8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAN_FRAME, buf, MAVLINK_MSG_ID_CAN_FRAME_MIN_LEN, MAVLINK_MSG_ID_CAN_FRAME_LEN, MAVLINK_MSG_ID_CAN_FRAME_CRC); +#else + mavlink_can_frame_t *packet = (mavlink_can_frame_t *)msgbuf; + packet->id = id; + packet->target_system = target_system; + packet->target_component = target_component; + packet->bus = bus; + packet->len = len; + mav_array_assign_uint8_t(packet->data, data, 8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAN_FRAME, (const char *)packet, MAVLINK_MSG_ID_CAN_FRAME_MIN_LEN, MAVLINK_MSG_ID_CAN_FRAME_LEN, MAVLINK_MSG_ID_CAN_FRAME_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CAN_FRAME UNPACKING + + +/** + * @brief Get field target_system from can_frame message + * + * @return System ID. + */ +static inline uint8_t mavlink_msg_can_frame_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from can_frame message + * + * @return Component ID. + */ +static inline uint8_t mavlink_msg_can_frame_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field bus from can_frame message + * + * @return Bus number + */ +static inline uint8_t mavlink_msg_can_frame_get_bus(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field len from can_frame message + * + * @return Frame length + */ +static inline uint8_t mavlink_msg_can_frame_get_len(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 7); +} + +/** + * @brief Get field id from can_frame message + * + * @return Frame ID + */ +static inline uint32_t mavlink_msg_can_frame_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field data from can_frame message + * + * @return Frame data + */ +static inline uint16_t mavlink_msg_can_frame_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 8, 8); +} + +/** + * @brief Decode a can_frame message into a struct + * + * @param msg The message to decode + * @param can_frame C-struct to decode the message contents into + */ +static inline void mavlink_msg_can_frame_decode(const mavlink_message_t* msg, mavlink_can_frame_t* can_frame) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + can_frame->id = mavlink_msg_can_frame_get_id(msg); + can_frame->target_system = mavlink_msg_can_frame_get_target_system(msg); + can_frame->target_component = mavlink_msg_can_frame_get_target_component(msg); + can_frame->bus = mavlink_msg_can_frame_get_bus(msg); + can_frame->len = mavlink_msg_can_frame_get_len(msg); + mavlink_msg_can_frame_get_data(msg, can_frame->data); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CAN_FRAME_LEN? msg->len : MAVLINK_MSG_ID_CAN_FRAME_LEN; + memset(can_frame, 0, MAVLINK_MSG_ID_CAN_FRAME_LEN); + memcpy(can_frame, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_canfd_frame.h b/lib/main/MAVLink/common/mavlink_msg_canfd_frame.h new file mode 100644 index 00000000000..b64b249f162 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_canfd_frame.h @@ -0,0 +1,390 @@ +#pragma once +// MESSAGE CANFD_FRAME PACKING + +#define MAVLINK_MSG_ID_CANFD_FRAME 387 + + +typedef struct __mavlink_canfd_frame_t { + uint32_t id; /*< Frame ID*/ + uint8_t target_system; /*< System ID.*/ + uint8_t target_component; /*< Component ID.*/ + uint8_t bus; /*< bus number*/ + uint8_t len; /*< Frame length*/ + uint8_t data[64]; /*< Frame data*/ +} mavlink_canfd_frame_t; + +#define MAVLINK_MSG_ID_CANFD_FRAME_LEN 72 +#define MAVLINK_MSG_ID_CANFD_FRAME_MIN_LEN 72 +#define MAVLINK_MSG_ID_387_LEN 72 +#define MAVLINK_MSG_ID_387_MIN_LEN 72 + +#define MAVLINK_MSG_ID_CANFD_FRAME_CRC 4 +#define MAVLINK_MSG_ID_387_CRC 4 + +#define MAVLINK_MSG_CANFD_FRAME_FIELD_DATA_LEN 64 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CANFD_FRAME { \ + 387, \ + "CANFD_FRAME", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_canfd_frame_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_canfd_frame_t, target_component) }, \ + { "bus", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_canfd_frame_t, bus) }, \ + { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_canfd_frame_t, len) }, \ + { "id", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_canfd_frame_t, id) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 64, 8, offsetof(mavlink_canfd_frame_t, data) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CANFD_FRAME { \ + "CANFD_FRAME", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_canfd_frame_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_canfd_frame_t, target_component) }, \ + { "bus", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_canfd_frame_t, bus) }, \ + { "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_canfd_frame_t, len) }, \ + { "id", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_canfd_frame_t, id) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 64, 8, offsetof(mavlink_canfd_frame_t, data) }, \ + } \ +} +#endif + +/** + * @brief Pack a canfd_frame message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID. + * @param target_component Component ID. + * @param bus bus number + * @param len Frame length + * @param id Frame ID + * @param data Frame data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_canfd_frame_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t bus, uint8_t len, uint32_t id, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CANFD_FRAME_LEN]; + _mav_put_uint32_t(buf, 0, id); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, bus); + _mav_put_uint8_t(buf, 7, len); + _mav_put_uint8_t_array(buf, 8, data, 64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CANFD_FRAME_LEN); +#else + mavlink_canfd_frame_t packet; + packet.id = id; + packet.target_system = target_system; + packet.target_component = target_component; + packet.bus = bus; + packet.len = len; + mav_array_assign_uint8_t(packet.data, data, 64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CANFD_FRAME_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CANFD_FRAME; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CANFD_FRAME_MIN_LEN, MAVLINK_MSG_ID_CANFD_FRAME_LEN, MAVLINK_MSG_ID_CANFD_FRAME_CRC); +} + +/** + * @brief Pack a canfd_frame message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID. + * @param target_component Component ID. + * @param bus bus number + * @param len Frame length + * @param id Frame ID + * @param data Frame data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_canfd_frame_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t bus, uint8_t len, uint32_t id, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CANFD_FRAME_LEN]; + _mav_put_uint32_t(buf, 0, id); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, bus); + _mav_put_uint8_t(buf, 7, len); + _mav_put_uint8_t_array(buf, 8, data, 64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CANFD_FRAME_LEN); +#else + mavlink_canfd_frame_t packet; + packet.id = id; + packet.target_system = target_system; + packet.target_component = target_component; + packet.bus = bus; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CANFD_FRAME_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CANFD_FRAME; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CANFD_FRAME_MIN_LEN, MAVLINK_MSG_ID_CANFD_FRAME_LEN, MAVLINK_MSG_ID_CANFD_FRAME_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CANFD_FRAME_MIN_LEN, MAVLINK_MSG_ID_CANFD_FRAME_LEN); +#endif +} + +/** + * @brief Pack a canfd_frame message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID. + * @param target_component Component ID. + * @param bus bus number + * @param len Frame length + * @param id Frame ID + * @param data Frame data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_canfd_frame_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t bus,uint8_t len,uint32_t id,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CANFD_FRAME_LEN]; + _mav_put_uint32_t(buf, 0, id); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, bus); + _mav_put_uint8_t(buf, 7, len); + _mav_put_uint8_t_array(buf, 8, data, 64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CANFD_FRAME_LEN); +#else + mavlink_canfd_frame_t packet; + packet.id = id; + packet.target_system = target_system; + packet.target_component = target_component; + packet.bus = bus; + packet.len = len; + mav_array_assign_uint8_t(packet.data, data, 64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CANFD_FRAME_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CANFD_FRAME; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CANFD_FRAME_MIN_LEN, MAVLINK_MSG_ID_CANFD_FRAME_LEN, MAVLINK_MSG_ID_CANFD_FRAME_CRC); +} + +/** + * @brief Encode a canfd_frame struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param canfd_frame C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_canfd_frame_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_canfd_frame_t* canfd_frame) +{ + return mavlink_msg_canfd_frame_pack(system_id, component_id, msg, canfd_frame->target_system, canfd_frame->target_component, canfd_frame->bus, canfd_frame->len, canfd_frame->id, canfd_frame->data); +} + +/** + * @brief Encode a canfd_frame struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param canfd_frame C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_canfd_frame_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_canfd_frame_t* canfd_frame) +{ + return mavlink_msg_canfd_frame_pack_chan(system_id, component_id, chan, msg, canfd_frame->target_system, canfd_frame->target_component, canfd_frame->bus, canfd_frame->len, canfd_frame->id, canfd_frame->data); +} + +/** + * @brief Encode a canfd_frame struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param canfd_frame C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_canfd_frame_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_canfd_frame_t* canfd_frame) +{ + return mavlink_msg_canfd_frame_pack_status(system_id, component_id, _status, msg, canfd_frame->target_system, canfd_frame->target_component, canfd_frame->bus, canfd_frame->len, canfd_frame->id, canfd_frame->data); +} + +/** + * @brief Send a canfd_frame message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID. + * @param target_component Component ID. + * @param bus bus number + * @param len Frame length + * @param id Frame ID + * @param data Frame data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_canfd_frame_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t bus, uint8_t len, uint32_t id, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CANFD_FRAME_LEN]; + _mav_put_uint32_t(buf, 0, id); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, bus); + _mav_put_uint8_t(buf, 7, len); + _mav_put_uint8_t_array(buf, 8, data, 64); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CANFD_FRAME, buf, MAVLINK_MSG_ID_CANFD_FRAME_MIN_LEN, MAVLINK_MSG_ID_CANFD_FRAME_LEN, MAVLINK_MSG_ID_CANFD_FRAME_CRC); +#else + mavlink_canfd_frame_t packet; + packet.id = id; + packet.target_system = target_system; + packet.target_component = target_component; + packet.bus = bus; + packet.len = len; + mav_array_assign_uint8_t(packet.data, data, 64); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CANFD_FRAME, (const char *)&packet, MAVLINK_MSG_ID_CANFD_FRAME_MIN_LEN, MAVLINK_MSG_ID_CANFD_FRAME_LEN, MAVLINK_MSG_ID_CANFD_FRAME_CRC); +#endif +} + +/** + * @brief Send a canfd_frame message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_canfd_frame_send_struct(mavlink_channel_t chan, const mavlink_canfd_frame_t* canfd_frame) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_canfd_frame_send(chan, canfd_frame->target_system, canfd_frame->target_component, canfd_frame->bus, canfd_frame->len, canfd_frame->id, canfd_frame->data); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CANFD_FRAME, (const char *)canfd_frame, MAVLINK_MSG_ID_CANFD_FRAME_MIN_LEN, MAVLINK_MSG_ID_CANFD_FRAME_LEN, MAVLINK_MSG_ID_CANFD_FRAME_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CANFD_FRAME_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_canfd_frame_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t bus, uint8_t len, uint32_t id, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, id); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, bus); + _mav_put_uint8_t(buf, 7, len); + _mav_put_uint8_t_array(buf, 8, data, 64); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CANFD_FRAME, buf, MAVLINK_MSG_ID_CANFD_FRAME_MIN_LEN, MAVLINK_MSG_ID_CANFD_FRAME_LEN, MAVLINK_MSG_ID_CANFD_FRAME_CRC); +#else + mavlink_canfd_frame_t *packet = (mavlink_canfd_frame_t *)msgbuf; + packet->id = id; + packet->target_system = target_system; + packet->target_component = target_component; + packet->bus = bus; + packet->len = len; + mav_array_assign_uint8_t(packet->data, data, 64); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CANFD_FRAME, (const char *)packet, MAVLINK_MSG_ID_CANFD_FRAME_MIN_LEN, MAVLINK_MSG_ID_CANFD_FRAME_LEN, MAVLINK_MSG_ID_CANFD_FRAME_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CANFD_FRAME UNPACKING + + +/** + * @brief Get field target_system from canfd_frame message + * + * @return System ID. + */ +static inline uint8_t mavlink_msg_canfd_frame_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from canfd_frame message + * + * @return Component ID. + */ +static inline uint8_t mavlink_msg_canfd_frame_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field bus from canfd_frame message + * + * @return bus number + */ +static inline uint8_t mavlink_msg_canfd_frame_get_bus(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field len from canfd_frame message + * + * @return Frame length + */ +static inline uint8_t mavlink_msg_canfd_frame_get_len(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 7); +} + +/** + * @brief Get field id from canfd_frame message + * + * @return Frame ID + */ +static inline uint32_t mavlink_msg_canfd_frame_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field data from canfd_frame message + * + * @return Frame data + */ +static inline uint16_t mavlink_msg_canfd_frame_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 64, 8); +} + +/** + * @brief Decode a canfd_frame message into a struct + * + * @param msg The message to decode + * @param canfd_frame C-struct to decode the message contents into + */ +static inline void mavlink_msg_canfd_frame_decode(const mavlink_message_t* msg, mavlink_canfd_frame_t* canfd_frame) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + canfd_frame->id = mavlink_msg_canfd_frame_get_id(msg); + canfd_frame->target_system = mavlink_msg_canfd_frame_get_target_system(msg); + canfd_frame->target_component = mavlink_msg_canfd_frame_get_target_component(msg); + canfd_frame->bus = mavlink_msg_canfd_frame_get_bus(msg); + canfd_frame->len = mavlink_msg_canfd_frame_get_len(msg); + mavlink_msg_canfd_frame_get_data(msg, canfd_frame->data); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CANFD_FRAME_LEN? msg->len : MAVLINK_MSG_ID_CANFD_FRAME_LEN; + memset(canfd_frame, 0, MAVLINK_MSG_ID_CANFD_FRAME_LEN); + memcpy(canfd_frame, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_cellular_config.h b/lib/main/MAVLink/common/mavlink_msg_cellular_config.h index 5b8a1fb7637..5772ac9e916 100644 --- a/lib/main/MAVLink/common/mavlink_msg_cellular_config.h +++ b/lib/main/MAVLink/common/mavlink_msg_cellular_config.h @@ -78,6 +78,54 @@ typedef struct __mavlink_cellular_config_t { static inline uint16_t mavlink_msg_cellular_config_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t enable_lte, uint8_t enable_pin, const char *pin, const char *new_pin, const char *apn, const char *puk, uint8_t roaming, uint8_t response) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN]; + _mav_put_uint8_t(buf, 0, enable_lte); + _mav_put_uint8_t(buf, 1, enable_pin); + _mav_put_uint8_t(buf, 82, roaming); + _mav_put_uint8_t(buf, 83, response); + _mav_put_char_array(buf, 2, pin, 16); + _mav_put_char_array(buf, 18, new_pin, 16); + _mav_put_char_array(buf, 34, apn, 32); + _mav_put_char_array(buf, 66, puk, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN); +#else + mavlink_cellular_config_t packet; + packet.enable_lte = enable_lte; + packet.enable_pin = enable_pin; + packet.roaming = roaming; + packet.response = response; + mav_array_assign_char(packet.pin, pin, 16); + mav_array_assign_char(packet.new_pin, new_pin, 16); + mav_array_assign_char(packet.apn, apn, 32); + mav_array_assign_char(packet.puk, puk, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CELLULAR_CONFIG; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); +} + +/** + * @brief Pack a cellular_config message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param enable_lte Enable/disable LTE. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param enable_pin Enable/disable PIN on the SIM card. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param pin PIN sent to the SIM card. Blank when PIN is disabled. Empty when message is sent back as a response. + * @param new_pin New PIN when changing the PIN. Blank to leave it unchanged. Empty when message is sent back as a response. + * @param apn Name of the cellular APN. Blank to leave it unchanged. Current APN when sent back as a response. + * @param puk Required PUK code in case the user failed to authenticate 3 times with the PIN. Empty when message is sent back as a response. + * @param roaming Enable/disable roaming. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param response Message acceptance response (sent back to GS). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_cellular_config_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t enable_lte, uint8_t enable_pin, const char *pin, const char *new_pin, const char *apn, const char *puk, uint8_t roaming, uint8_t response) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN]; _mav_put_uint8_t(buf, 0, enable_lte); @@ -103,7 +151,11 @@ static inline uint16_t mavlink_msg_cellular_config_pack(uint8_t system_id, uint8 #endif msg->msgid = MAVLINK_MSG_ID_CELLULAR_CONFIG; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN); +#endif } /** @@ -143,10 +195,10 @@ static inline uint16_t mavlink_msg_cellular_config_pack_chan(uint8_t system_id, packet.enable_pin = enable_pin; packet.roaming = roaming; packet.response = response; - mav_array_memcpy(packet.pin, pin, sizeof(char)*16); - mav_array_memcpy(packet.new_pin, new_pin, sizeof(char)*16); - mav_array_memcpy(packet.apn, apn, sizeof(char)*32); - mav_array_memcpy(packet.puk, puk, sizeof(char)*16); + mav_array_assign_char(packet.pin, pin, 16); + mav_array_assign_char(packet.new_pin, new_pin, 16); + mav_array_assign_char(packet.apn, apn, 32); + mav_array_assign_char(packet.puk, puk, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN); #endif @@ -181,6 +233,20 @@ static inline uint16_t mavlink_msg_cellular_config_encode_chan(uint8_t system_id return mavlink_msg_cellular_config_pack_chan(system_id, component_id, chan, msg, cellular_config->enable_lte, cellular_config->enable_pin, cellular_config->pin, cellular_config->new_pin, cellular_config->apn, cellular_config->puk, cellular_config->roaming, cellular_config->response); } +/** + * @brief Encode a cellular_config struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param cellular_config C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_cellular_config_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_cellular_config_t* cellular_config) +{ + return mavlink_msg_cellular_config_pack_status(system_id, component_id, _status, msg, cellular_config->enable_lte, cellular_config->enable_pin, cellular_config->pin, cellular_config->new_pin, cellular_config->apn, cellular_config->puk, cellular_config->roaming, cellular_config->response); +} + /** * @brief Send a cellular_config message * @param chan MAVLink channel to send the message @@ -215,10 +281,10 @@ static inline void mavlink_msg_cellular_config_send(mavlink_channel_t chan, uint packet.enable_pin = enable_pin; packet.roaming = roaming; packet.response = response; - mav_array_memcpy(packet.pin, pin, sizeof(char)*16); - mav_array_memcpy(packet.new_pin, new_pin, sizeof(char)*16); - mav_array_memcpy(packet.apn, apn, sizeof(char)*32); - mav_array_memcpy(packet.puk, puk, sizeof(char)*16); + mav_array_assign_char(packet.pin, pin, 16); + mav_array_assign_char(packet.new_pin, new_pin, 16); + mav_array_assign_char(packet.apn, apn, 32); + mav_array_assign_char(packet.puk, puk, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_CONFIG, (const char *)&packet, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); #endif } @@ -239,7 +305,7 @@ static inline void mavlink_msg_cellular_config_send_struct(mavlink_channel_t cha #if MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -264,10 +330,10 @@ static inline void mavlink_msg_cellular_config_send_buf(mavlink_message_t *msgbu packet->enable_pin = enable_pin; packet->roaming = roaming; packet->response = response; - mav_array_memcpy(packet->pin, pin, sizeof(char)*16); - mav_array_memcpy(packet->new_pin, new_pin, sizeof(char)*16); - mav_array_memcpy(packet->apn, apn, sizeof(char)*32); - mav_array_memcpy(packet->puk, puk, sizeof(char)*16); + mav_array_assign_char(packet->pin, pin, 16); + mav_array_assign_char(packet->new_pin, new_pin, 16); + mav_array_assign_char(packet->apn, apn, 32); + mav_array_assign_char(packet->puk, puk, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_CONFIG, (const char *)packet, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_cellular_status.h b/lib/main/MAVLink/common/mavlink_msg_cellular_status.h index 5b1c195805c..8d7818e6e1d 100644 --- a/lib/main/MAVLink/common/mavlink_msg_cellular_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_cellular_status.h @@ -9,7 +9,7 @@ typedef struct __mavlink_cellular_status_t { uint16_t mnc; /*< Mobile network code. If unknown, set to UINT16_MAX*/ uint16_t lac; /*< Location area code. If unknown, set to 0*/ uint8_t status; /*< Cellular modem status*/ - uint8_t failure_reason; /*< Failure reason when status in in CELLUAR_STATUS_FAILED*/ + uint8_t failure_reason; /*< Failure reason when status in in CELLULAR_STATUS_FLAG_FAILED*/ uint8_t type; /*< Cellular network radio type: gsm, cdma, lte...*/ uint8_t quality; /*< Signal quality in percent. If unknown, set to UINT8_MAX*/ } mavlink_cellular_status_t; @@ -60,7 +60,7 @@ typedef struct __mavlink_cellular_status_t { * @param msg The MAVLink message to compress the data into * * @param status Cellular modem status - * @param failure_reason Failure reason when status in in CELLUAR_STATUS_FAILED + * @param failure_reason Failure reason when status in in CELLULAR_STATUS_FLAG_FAILED * @param type Cellular network radio type: gsm, cdma, lte... * @param quality Signal quality in percent. If unknown, set to UINT8_MAX * @param mcc Mobile country code. If unknown, set to UINT16_MAX @@ -99,6 +99,57 @@ static inline uint16_t mavlink_msg_cellular_status_pack(uint8_t system_id, uint8 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_CRC); } +/** + * @brief Pack a cellular_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param status Cellular modem status + * @param failure_reason Failure reason when status in in CELLULAR_STATUS_FLAG_FAILED + * @param type Cellular network radio type: gsm, cdma, lte... + * @param quality Signal quality in percent. If unknown, set to UINT8_MAX + * @param mcc Mobile country code. If unknown, set to UINT16_MAX + * @param mnc Mobile network code. If unknown, set to UINT16_MAX + * @param lac Location area code. If unknown, set to 0 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_cellular_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t status, uint8_t failure_reason, uint8_t type, uint8_t quality, uint16_t mcc, uint16_t mnc, uint16_t lac) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CELLULAR_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, mcc); + _mav_put_uint16_t(buf, 2, mnc); + _mav_put_uint16_t(buf, 4, lac); + _mav_put_uint8_t(buf, 6, status); + _mav_put_uint8_t(buf, 7, failure_reason); + _mav_put_uint8_t(buf, 8, type); + _mav_put_uint8_t(buf, 9, quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN); +#else + mavlink_cellular_status_t packet; + packet.mcc = mcc; + packet.mnc = mnc; + packet.lac = lac; + packet.status = status; + packet.failure_reason = failure_reason; + packet.type = type; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CELLULAR_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN); +#endif +} + /** * @brief Pack a cellular_status message on a channel * @param system_id ID of this system @@ -106,7 +157,7 @@ static inline uint16_t mavlink_msg_cellular_status_pack(uint8_t system_id, uint8 * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param status Cellular modem status - * @param failure_reason Failure reason when status in in CELLUAR_STATUS_FAILED + * @param failure_reason Failure reason when status in in CELLULAR_STATUS_FLAG_FAILED * @param type Cellular network radio type: gsm, cdma, lte... * @param quality Signal quality in percent. If unknown, set to UINT8_MAX * @param mcc Mobile country code. If unknown, set to UINT16_MAX @@ -173,12 +224,26 @@ static inline uint16_t mavlink_msg_cellular_status_encode_chan(uint8_t system_id return mavlink_msg_cellular_status_pack_chan(system_id, component_id, chan, msg, cellular_status->status, cellular_status->failure_reason, cellular_status->type, cellular_status->quality, cellular_status->mcc, cellular_status->mnc, cellular_status->lac); } +/** + * @brief Encode a cellular_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param cellular_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_cellular_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_cellular_status_t* cellular_status) +{ + return mavlink_msg_cellular_status_pack_status(system_id, component_id, _status, msg, cellular_status->status, cellular_status->failure_reason, cellular_status->type, cellular_status->quality, cellular_status->mcc, cellular_status->mnc, cellular_status->lac); +} + /** * @brief Send a cellular_status message * @param chan MAVLink channel to send the message * * @param status Cellular modem status - * @param failure_reason Failure reason when status in in CELLUAR_STATUS_FAILED + * @param failure_reason Failure reason when status in in CELLULAR_STATUS_FLAG_FAILED * @param type Cellular network radio type: gsm, cdma, lte... * @param quality Signal quality in percent. If unknown, set to UINT8_MAX * @param mcc Mobile country code. If unknown, set to UINT16_MAX @@ -230,7 +295,7 @@ static inline void mavlink_msg_cellular_status_send_struct(mavlink_channel_t cha #if MAVLINK_MSG_ID_CELLULAR_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -282,7 +347,7 @@ static inline uint8_t mavlink_msg_cellular_status_get_status(const mavlink_messa /** * @brief Get field failure_reason from cellular_status message * - * @return Failure reason when status in in CELLUAR_STATUS_FAILED + * @return Failure reason when status in in CELLULAR_STATUS_FLAG_FAILED */ static inline uint8_t mavlink_msg_cellular_status_get_failure_reason(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_change_operator_control.h b/lib/main/MAVLink/common/mavlink_msg_change_operator_control.h index 8fea8d78372..1391cde0415 100755 --- a/lib/main/MAVLink/common/mavlink_msg_change_operator_control.h +++ b/lib/main/MAVLink/common/mavlink_msg_change_operator_control.h @@ -71,7 +71,7 @@ static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_i packet.target_system = target_system; packet.control_request = control_request; packet.version = version; - mav_array_memcpy(packet.passkey, passkey, sizeof(char)*25); + mav_array_assign_char(packet.passkey, passkey, 25); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); #endif @@ -79,6 +79,46 @@ static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_i return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_MIN_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); } +/** + * @brief Pack a change_operator_control message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System the GCS requests control for + * @param control_request 0: request control of this MAV, 1: Release control of this MAV + * @param version [rad] 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_change_operator_control_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t control_request, uint8_t version, const char *passkey) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, version); + _mav_put_char_array(buf, 3, passkey, 25); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#else + mavlink_change_operator_control_t packet; + packet.target_system = target_system; + packet.control_request = control_request; + packet.version = version; + mav_array_memcpy(packet.passkey, passkey, sizeof(char)*25); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_MIN_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_MIN_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#endif +} + /** * @brief Pack a change_operator_control message on a channel * @param system_id ID of this system @@ -107,7 +147,7 @@ static inline uint16_t mavlink_msg_change_operator_control_pack_chan(uint8_t sys packet.target_system = target_system; packet.control_request = control_request; packet.version = version; - mav_array_memcpy(packet.passkey, passkey, sizeof(char)*25); + mav_array_assign_char(packet.passkey, passkey, 25); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); #endif @@ -142,6 +182,20 @@ static inline uint16_t mavlink_msg_change_operator_control_encode_chan(uint8_t s return mavlink_msg_change_operator_control_pack_chan(system_id, component_id, chan, msg, change_operator_control->target_system, change_operator_control->control_request, change_operator_control->version, change_operator_control->passkey); } +/** + * @brief Encode a change_operator_control struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param change_operator_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_change_operator_control_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_change_operator_control_t* change_operator_control) +{ + return mavlink_msg_change_operator_control_pack_status(system_id, component_id, _status, msg, change_operator_control->target_system, change_operator_control->control_request, change_operator_control->version, change_operator_control->passkey); +} + /** * @brief Send a change_operator_control message * @param chan MAVLink channel to send the message @@ -167,7 +221,7 @@ static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t ch packet.target_system = target_system; packet.control_request = control_request; packet.version = version; - mav_array_memcpy(packet.passkey, passkey, sizeof(char)*25); + mav_array_assign_char(packet.passkey, passkey, 25); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_MIN_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); #endif } @@ -188,7 +242,7 @@ static inline void mavlink_msg_change_operator_control_send_struct(mavlink_chann #if MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -208,7 +262,7 @@ static inline void mavlink_msg_change_operator_control_send_buf(mavlink_message_ packet->target_system = target_system; packet->control_request = control_request; packet->version = version; - mav_array_memcpy(packet->passkey, passkey, sizeof(char)*25); + mav_array_assign_char(packet->passkey, passkey, 25); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_MIN_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_change_operator_control_ack.h b/lib/main/MAVLink/common/mavlink_msg_change_operator_control_ack.h index d35d444ba3a..612c26953c3 100755 --- a/lib/main/MAVLink/common/mavlink_msg_change_operator_control_ack.h +++ b/lib/main/MAVLink/common/mavlink_msg_change_operator_control_ack.h @@ -75,6 +75,45 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack(uint8_t syst return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_MIN_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); } +/** + * @brief Pack a change_operator_control_ack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param gcs_system_id ID of the GCS this message + * @param control_request 0: request control of this MAV, 1: Release control of this MAV + * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_change_operator_control_ack_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN]; + _mav_put_uint8_t(buf, 0, gcs_system_id); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, ack); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#else + mavlink_change_operator_control_ack_t packet; + packet.gcs_system_id = gcs_system_id; + packet.control_request = control_request; + packet.ack = ack; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_MIN_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_MIN_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#endif +} + /** * @brief Pack a change_operator_control_ack message on a channel * @param system_id ID of this system @@ -137,6 +176,20 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_encode_chan(uint8 return mavlink_msg_change_operator_control_ack_pack_chan(system_id, component_id, chan, msg, change_operator_control_ack->gcs_system_id, change_operator_control_ack->control_request, change_operator_control_ack->ack); } +/** + * @brief Encode a change_operator_control_ack struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param change_operator_control_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_change_operator_control_ack_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_change_operator_control_ack_t* change_operator_control_ack) +{ + return mavlink_msg_change_operator_control_ack_pack_status(system_id, component_id, _status, msg, change_operator_control_ack->gcs_system_id, change_operator_control_ack->control_request, change_operator_control_ack->ack); +} + /** * @brief Send a change_operator_control_ack message * @param chan MAVLink channel to send the message @@ -182,7 +235,7 @@ static inline void mavlink_msg_change_operator_control_ack_send_struct(mavlink_c #if MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_collision.h b/lib/main/MAVLink/common/mavlink_msg_collision.h index b7d8a07f257..cb64679e8db 100755 --- a/lib/main/MAVLink/common/mavlink_msg_collision.h +++ b/lib/main/MAVLink/common/mavlink_msg_collision.h @@ -99,6 +99,57 @@ static inline uint16_t mavlink_msg_collision_pack(uint8_t system_id, uint8_t com return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COLLISION_MIN_LEN, MAVLINK_MSG_ID_COLLISION_LEN, MAVLINK_MSG_ID_COLLISION_CRC); } +/** + * @brief Pack a collision message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param src Collision data source + * @param id Unique identifier, domain based on src field + * @param action Action that is being taken to avoid this collision + * @param threat_level How concerned the aircraft is about this collision + * @param time_to_minimum_delta [s] Estimated time until collision occurs + * @param altitude_minimum_delta [m] Closest vertical distance between vehicle and object + * @param horizontal_minimum_delta [m] Closest horizontal distance between vehicle and object + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_collision_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t src, uint32_t id, uint8_t action, uint8_t threat_level, float time_to_minimum_delta, float altitude_minimum_delta, float horizontal_minimum_delta) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COLLISION_LEN]; + _mav_put_uint32_t(buf, 0, id); + _mav_put_float(buf, 4, time_to_minimum_delta); + _mav_put_float(buf, 8, altitude_minimum_delta); + _mav_put_float(buf, 12, horizontal_minimum_delta); + _mav_put_uint8_t(buf, 16, src); + _mav_put_uint8_t(buf, 17, action); + _mav_put_uint8_t(buf, 18, threat_level); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COLLISION_LEN); +#else + mavlink_collision_t packet; + packet.id = id; + packet.time_to_minimum_delta = time_to_minimum_delta; + packet.altitude_minimum_delta = altitude_minimum_delta; + packet.horizontal_minimum_delta = horizontal_minimum_delta; + packet.src = src; + packet.action = action; + packet.threat_level = threat_level; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COLLISION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COLLISION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_COLLISION_MIN_LEN, MAVLINK_MSG_ID_COLLISION_LEN, MAVLINK_MSG_ID_COLLISION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_COLLISION_MIN_LEN, MAVLINK_MSG_ID_COLLISION_LEN); +#endif +} + /** * @brief Pack a collision message on a channel * @param system_id ID of this system @@ -173,6 +224,20 @@ static inline uint16_t mavlink_msg_collision_encode_chan(uint8_t system_id, uint return mavlink_msg_collision_pack_chan(system_id, component_id, chan, msg, collision->src, collision->id, collision->action, collision->threat_level, collision->time_to_minimum_delta, collision->altitude_minimum_delta, collision->horizontal_minimum_delta); } +/** + * @brief Encode a collision struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param collision C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_collision_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_collision_t* collision) +{ + return mavlink_msg_collision_pack_status(system_id, component_id, _status, msg, collision->src, collision->id, collision->action, collision->threat_level, collision->time_to_minimum_delta, collision->altitude_minimum_delta, collision->horizontal_minimum_delta); +} + /** * @brief Send a collision message * @param chan MAVLink channel to send the message @@ -230,7 +295,7 @@ static inline void mavlink_msg_collision_send_struct(mavlink_channel_t chan, con #if MAVLINK_MSG_ID_COLLISION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_command_ack.h b/lib/main/MAVLink/common/mavlink_msg_command_ack.h index 58dd1fc6dfc..74ec9789290 100755 --- a/lib/main/MAVLink/common/mavlink_msg_command_ack.h +++ b/lib/main/MAVLink/common/mavlink_msg_command_ack.h @@ -7,10 +7,10 @@ typedef struct __mavlink_command_ack_t { uint16_t command; /*< Command ID (of acknowledged command).*/ uint8_t result; /*< Result of command.*/ - uint8_t progress; /*< WIP: Also used as result_param1, it can be set with an enum containing the errors reasons of why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS (255 if the progress is unknown).*/ - int32_t result_param2; /*< WIP: Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied.*/ - uint8_t target_system; /*< WIP: System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement.*/ - uint8_t target_component; /*< WIP: Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement.*/ + uint8_t progress; /*< [%] The progress percentage when result is MAV_RESULT_IN_PROGRESS. Values: [0-100], or UINT8_MAX if the progress is unknown.*/ + int32_t result_param2; /*< Additional result information. Can be set with a command-specific enum containing command-specific error reasons for why the command might be denied. If used, the associated enum must be documented in the corresponding MAV_CMD (this enum should have a 0 value to indicate "unused" or "unknown").*/ + uint8_t target_system; /*< System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement.*/ + uint8_t target_component; /*< Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement.*/ } mavlink_command_ack_t; #define MAVLINK_MSG_ID_COMMAND_ACK_LEN 10 @@ -58,10 +58,10 @@ typedef struct __mavlink_command_ack_t { * * @param command Command ID (of acknowledged command). * @param result Result of command. - * @param progress WIP: Also used as result_param1, it can be set with an enum containing the errors reasons of why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS (255 if the progress is unknown). - * @param result_param2 WIP: Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. - * @param target_system WIP: System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. - * @param target_component WIP: Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + * @param progress [%] The progress percentage when result is MAV_RESULT_IN_PROGRESS. Values: [0-100], or UINT8_MAX if the progress is unknown. + * @param result_param2 Additional result information. Can be set with a command-specific enum containing command-specific error reasons for why the command might be denied. If used, the associated enum must be documented in the corresponding MAV_CMD (this enum should have a 0 value to indicate "unused" or "unknown"). + * @param target_system System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + * @param target_component Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -93,6 +93,54 @@ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t c return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_ACK_MIN_LEN, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); } +/** + * @brief Pack a command_ack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param command Command ID (of acknowledged command). + * @param result Result of command. + * @param progress [%] The progress percentage when result is MAV_RESULT_IN_PROGRESS. Values: [0-100], or UINT8_MAX if the progress is unknown. + * @param result_param2 Additional result information. Can be set with a command-specific enum containing command-specific error reasons for why the command might be denied. If used, the associated enum must be documented in the corresponding MAV_CMD (this enum should have a 0 value to indicate "unused" or "unknown"). + * @param target_system System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + * @param target_component Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_command_ack_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint16_t command, uint8_t result, uint8_t progress, int32_t result_param2, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMMAND_ACK_LEN]; + _mav_put_uint16_t(buf, 0, command); + _mav_put_uint8_t(buf, 2, result); + _mav_put_uint8_t(buf, 3, progress); + _mav_put_int32_t(buf, 4, result_param2); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#else + mavlink_command_ack_t packet; + packet.command = command; + packet.result = result; + packet.progress = progress; + packet.result_param2 = result_param2; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_COMMAND_ACK_MIN_LEN, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_COMMAND_ACK_MIN_LEN, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#endif +} + /** * @brief Pack a command_ack message on a channel * @param system_id ID of this system @@ -101,10 +149,10 @@ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t c * @param msg The MAVLink message to compress the data into * @param command Command ID (of acknowledged command). * @param result Result of command. - * @param progress WIP: Also used as result_param1, it can be set with an enum containing the errors reasons of why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS (255 if the progress is unknown). - * @param result_param2 WIP: Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. - * @param target_system WIP: System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. - * @param target_component WIP: Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + * @param progress [%] The progress percentage when result is MAV_RESULT_IN_PROGRESS. Values: [0-100], or UINT8_MAX if the progress is unknown. + * @param result_param2 Additional result information. Can be set with a command-specific enum containing command-specific error reasons for why the command might be denied. If used, the associated enum must be documented in the corresponding MAV_CMD (this enum should have a 0 value to indicate "unused" or "unknown"). + * @param target_system System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + * @param target_component Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -164,16 +212,30 @@ static inline uint16_t mavlink_msg_command_ack_encode_chan(uint8_t system_id, ui return mavlink_msg_command_ack_pack_chan(system_id, component_id, chan, msg, command_ack->command, command_ack->result, command_ack->progress, command_ack->result_param2, command_ack->target_system, command_ack->target_component); } +/** + * @brief Encode a command_ack struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param command_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_ack_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_command_ack_t* command_ack) +{ + return mavlink_msg_command_ack_pack_status(system_id, component_id, _status, msg, command_ack->command, command_ack->result, command_ack->progress, command_ack->result_param2, command_ack->target_system, command_ack->target_component); +} + /** * @brief Send a command_ack message * @param chan MAVLink channel to send the message * * @param command Command ID (of acknowledged command). * @param result Result of command. - * @param progress WIP: Also used as result_param1, it can be set with an enum containing the errors reasons of why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS (255 if the progress is unknown). - * @param result_param2 WIP: Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. - * @param target_system WIP: System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. - * @param target_component WIP: Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + * @param progress [%] The progress percentage when result is MAV_RESULT_IN_PROGRESS. Values: [0-100], or UINT8_MAX if the progress is unknown. + * @param result_param2 Additional result information. Can be set with a command-specific enum containing command-specific error reasons for why the command might be denied. If used, the associated enum must be documented in the corresponding MAV_CMD (this enum should have a 0 value to indicate "unused" or "unknown"). + * @param target_system System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + * @param target_component Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -218,7 +280,7 @@ static inline void mavlink_msg_command_ack_send_struct(mavlink_channel_t chan, c #if MAVLINK_MSG_ID_COMMAND_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -278,7 +340,7 @@ static inline uint8_t mavlink_msg_command_ack_get_result(const mavlink_message_t /** * @brief Get field progress from command_ack message * - * @return WIP: Also used as result_param1, it can be set with an enum containing the errors reasons of why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS (255 if the progress is unknown). + * @return [%] The progress percentage when result is MAV_RESULT_IN_PROGRESS. Values: [0-100], or UINT8_MAX if the progress is unknown. */ static inline uint8_t mavlink_msg_command_ack_get_progress(const mavlink_message_t* msg) { @@ -288,7 +350,7 @@ static inline uint8_t mavlink_msg_command_ack_get_progress(const mavlink_message /** * @brief Get field result_param2 from command_ack message * - * @return WIP: Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. + * @return Additional result information. Can be set with a command-specific enum containing command-specific error reasons for why the command might be denied. If used, the associated enum must be documented in the corresponding MAV_CMD (this enum should have a 0 value to indicate "unused" or "unknown"). */ static inline int32_t mavlink_msg_command_ack_get_result_param2(const mavlink_message_t* msg) { @@ -298,7 +360,7 @@ static inline int32_t mavlink_msg_command_ack_get_result_param2(const mavlink_me /** * @brief Get field target_system from command_ack message * - * @return WIP: System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + * @return System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. */ static inline uint8_t mavlink_msg_command_ack_get_target_system(const mavlink_message_t* msg) { @@ -308,7 +370,7 @@ static inline uint8_t mavlink_msg_command_ack_get_target_system(const mavlink_me /** * @brief Get field target_component from command_ack message * - * @return WIP: Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + * @return Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. */ static inline uint8_t mavlink_msg_command_ack_get_target_component(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_command_cancel.h b/lib/main/MAVLink/common/mavlink_msg_command_cancel.h index 0ad7dac5845..6b437b159de 100644 --- a/lib/main/MAVLink/common/mavlink_msg_command_cancel.h +++ b/lib/main/MAVLink/common/mavlink_msg_command_cancel.h @@ -75,6 +75,45 @@ static inline uint16_t mavlink_msg_command_cancel_pack(uint8_t system_id, uint8_ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_CANCEL_MIN_LEN, MAVLINK_MSG_ID_COMMAND_CANCEL_LEN, MAVLINK_MSG_ID_COMMAND_CANCEL_CRC); } +/** + * @brief Pack a command_cancel message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System executing long running command. Should not be broadcast (0). + * @param target_component Component executing long running command. + * @param command Command ID (of command to cancel). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_command_cancel_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t command) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMMAND_CANCEL_LEN]; + _mav_put_uint16_t(buf, 0, command); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_CANCEL_LEN); +#else + mavlink_command_cancel_t packet; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_CANCEL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMMAND_CANCEL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_COMMAND_CANCEL_MIN_LEN, MAVLINK_MSG_ID_COMMAND_CANCEL_LEN, MAVLINK_MSG_ID_COMMAND_CANCEL_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_COMMAND_CANCEL_MIN_LEN, MAVLINK_MSG_ID_COMMAND_CANCEL_LEN); +#endif +} + /** * @brief Pack a command_cancel message on a channel * @param system_id ID of this system @@ -137,6 +176,20 @@ static inline uint16_t mavlink_msg_command_cancel_encode_chan(uint8_t system_id, return mavlink_msg_command_cancel_pack_chan(system_id, component_id, chan, msg, command_cancel->target_system, command_cancel->target_component, command_cancel->command); } +/** + * @brief Encode a command_cancel struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param command_cancel C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_cancel_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_command_cancel_t* command_cancel) +{ + return mavlink_msg_command_cancel_pack_status(system_id, component_id, _status, msg, command_cancel->target_system, command_cancel->target_component, command_cancel->command); +} + /** * @brief Send a command_cancel message * @param chan MAVLink channel to send the message @@ -182,7 +235,7 @@ static inline void mavlink_msg_command_cancel_send_struct(mavlink_channel_t chan #if MAVLINK_MSG_ID_COMMAND_CANCEL_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_command_int.h b/lib/main/MAVLink/common/mavlink_msg_command_int.h index a23f50895c2..8d84b44c6d5 100755 --- a/lib/main/MAVLink/common/mavlink_msg_command_int.h +++ b/lib/main/MAVLink/common/mavlink_msg_command_int.h @@ -135,6 +135,75 @@ static inline uint16_t mavlink_msg_command_int_pack(uint8_t system_id, uint8_t c return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_INT_MIN_LEN, MAVLINK_MSG_ID_COMMAND_INT_LEN, MAVLINK_MSG_ID_COMMAND_INT_CRC); } +/** + * @brief Pack a command_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param frame The coordinate system of the COMMAND. + * @param command The scheduled action for the mission item. + * @param current Not used. + * @param autocontinue Not used (set 0). + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum + * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + * @param y PARAM6 / local: y position in meters * 1e4, global: longitude in degrees * 10^7 + * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_command_int_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMMAND_INT_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_int32_t(buf, 16, x); + _mav_put_int32_t(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, command); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, frame); + _mav_put_uint8_t(buf, 33, current); + _mav_put_uint8_t(buf, 34, autocontinue); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#else + mavlink_command_int_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.current = current; + packet.autocontinue = autocontinue; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMMAND_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_COMMAND_INT_MIN_LEN, MAVLINK_MSG_ID_COMMAND_INT_LEN, MAVLINK_MSG_ID_COMMAND_INT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_COMMAND_INT_MIN_LEN, MAVLINK_MSG_ID_COMMAND_INT_LEN); +#endif +} + /** * @brief Pack a command_int message on a channel * @param system_id ID of this system @@ -227,6 +296,20 @@ static inline uint16_t mavlink_msg_command_int_encode_chan(uint8_t system_id, ui return mavlink_msg_command_int_pack_chan(system_id, component_id, chan, msg, command_int->target_system, command_int->target_component, command_int->frame, command_int->command, command_int->current, command_int->autocontinue, command_int->param1, command_int->param2, command_int->param3, command_int->param4, command_int->x, command_int->y, command_int->z); } +/** + * @brief Encode a command_int struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param command_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_int_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_command_int_t* command_int) +{ + return mavlink_msg_command_int_pack_status(system_id, component_id, _status, msg, command_int->target_system, command_int->target_component, command_int->frame, command_int->command, command_int->current, command_int->autocontinue, command_int->param1, command_int->param2, command_int->param3, command_int->param4, command_int->x, command_int->y, command_int->z); +} + /** * @brief Send a command_int message * @param chan MAVLink channel to send the message @@ -302,7 +385,7 @@ static inline void mavlink_msg_command_int_send_struct(mavlink_channel_t chan, c #if MAVLINK_MSG_ID_COMMAND_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_command_long.h b/lib/main/MAVLink/common/mavlink_msg_command_long.h index 6a9a1698755..68894e4b94c 100755 --- a/lib/main/MAVLink/common/mavlink_msg_command_long.h +++ b/lib/main/MAVLink/common/mavlink_msg_command_long.h @@ -123,6 +123,69 @@ static inline uint16_t mavlink_msg_command_long_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_LONG_MIN_LEN, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); } +/** + * @brief Pack a command_long message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System which should execute the command + * @param target_component Component which should execute the command, 0 for all components + * @param command Command ID (of command to send). + * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + * @param param1 Parameter 1 (for the specific command). + * @param param2 Parameter 2 (for the specific command). + * @param param3 Parameter 3 (for the specific command). + * @param param4 Parameter 4 (for the specific command). + * @param param5 Parameter 5 (for the specific command). + * @param param6 Parameter 6 (for the specific command). + * @param param7 Parameter 7 (for the specific command). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_command_long_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMMAND_LONG_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, param5); + _mav_put_float(buf, 20, param6); + _mav_put_float(buf, 24, param7); + _mav_put_uint16_t(buf, 28, command); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, confirmation); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#else + mavlink_command_long_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.param5 = param5; + packet.param6 = param6; + packet.param7 = param7; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.confirmation = confirmation; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_COMMAND_LONG_MIN_LEN, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_COMMAND_LONG_MIN_LEN, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#endif +} + /** * @brief Pack a command_long message on a channel * @param system_id ID of this system @@ -209,6 +272,20 @@ static inline uint16_t mavlink_msg_command_long_encode_chan(uint8_t system_id, u return mavlink_msg_command_long_pack_chan(system_id, component_id, chan, msg, command_long->target_system, command_long->target_component, command_long->command, command_long->confirmation, command_long->param1, command_long->param2, command_long->param3, command_long->param4, command_long->param5, command_long->param6, command_long->param7); } +/** + * @brief Encode a command_long struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param command_long C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_long_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_command_long_t* command_long) +{ + return mavlink_msg_command_long_pack_status(system_id, component_id, _status, msg, command_long->target_system, command_long->target_component, command_long->command, command_long->confirmation, command_long->param1, command_long->param2, command_long->param3, command_long->param4, command_long->param5, command_long->param6, command_long->param7); +} + /** * @brief Send a command_long message * @param chan MAVLink channel to send the message @@ -278,7 +355,7 @@ static inline void mavlink_msg_command_long_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_COMMAND_LONG_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_component_information.h b/lib/main/MAVLink/common/mavlink_msg_component_information.h index a916e6bee67..602dc93f2e8 100644 --- a/lib/main/MAVLink/common/mavlink_msg_component_information.h +++ b/lib/main/MAVLink/common/mavlink_msg_component_information.h @@ -6,47 +6,44 @@ typedef struct __mavlink_component_information_t { uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ - uint32_t metadata_type; /*< The type of metadata being requested.*/ - uint32_t metadata_uid; /*< Unique uid for this metadata which a gcs can use for created cached metadata and understanding whether it's cache it up to date or whether it needs to download new data.*/ - uint32_t translation_uid; /*< Unique uid for the translation file associated with the metadata.*/ - char metadata_uri[70]; /*< Component definition URI. If prefix mavlinkftp:// file is downloaded from vehicle over mavlink ftp protocol. If prefix http[s]:// file is downloaded over internet. Files are json format. They can end in .gz to indicate file is in gzip format.*/ - char translation_uri[70]; /*< The translations for strings within the metadata file. If null the either do not exist or may be included in the metadata file itself. The translations specified here supercede any which may be in the metadata file itself. The uri format is the same as component_metadata_uri . Files are in Json Translation spec format. Empty string indicates no tranlsation file.*/ + uint32_t general_metadata_file_crc; /*< CRC32 of the general metadata file (general_metadata_uri).*/ + uint32_t peripherals_metadata_file_crc; /*< CRC32 of peripherals metadata file (peripherals_metadata_uri).*/ + char general_metadata_uri[100]; /*< MAVLink FTP URI for the general metadata file (COMP_METADATA_TYPE_GENERAL), which may be compressed with xz. The file contains general component metadata, and may contain URI links for additional metadata (see COMP_METADATA_TYPE). The information is static from boot, and may be generated at compile time. The string needs to be zero terminated.*/ + char peripherals_metadata_uri[100]; /*< (Optional) MAVLink FTP URI for the peripherals metadata file (COMP_METADATA_TYPE_PERIPHERALS), which may be compressed with xz. This contains data about "attached components" such as UAVCAN nodes. The peripherals are in a separate file because the information must be generated dynamically at runtime. The string needs to be zero terminated.*/ } mavlink_component_information_t; -#define MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN 156 -#define MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN 156 -#define MAVLINK_MSG_ID_395_LEN 156 -#define MAVLINK_MSG_ID_395_MIN_LEN 156 +#define MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN 212 +#define MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN 212 +#define MAVLINK_MSG_ID_395_LEN 212 +#define MAVLINK_MSG_ID_395_MIN_LEN 212 -#define MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC 163 -#define MAVLINK_MSG_ID_395_CRC 163 +#define MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC 0 +#define MAVLINK_MSG_ID_395_CRC 0 -#define MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_METADATA_URI_LEN 70 -#define MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_TRANSLATION_URI_LEN 70 +#define MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_GENERAL_METADATA_URI_LEN 100 +#define MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_PERIPHERALS_METADATA_URI_LEN 100 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_COMPONENT_INFORMATION { \ 395, \ "COMPONENT_INFORMATION", \ - 6, \ + 5, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_component_information_t, time_boot_ms) }, \ - { "metadata_type", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_component_information_t, metadata_type) }, \ - { "metadata_uid", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_component_information_t, metadata_uid) }, \ - { "metadata_uri", NULL, MAVLINK_TYPE_CHAR, 70, 16, offsetof(mavlink_component_information_t, metadata_uri) }, \ - { "translation_uid", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_component_information_t, translation_uid) }, \ - { "translation_uri", NULL, MAVLINK_TYPE_CHAR, 70, 86, offsetof(mavlink_component_information_t, translation_uri) }, \ + { "general_metadata_file_crc", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_component_information_t, general_metadata_file_crc) }, \ + { "general_metadata_uri", NULL, MAVLINK_TYPE_CHAR, 100, 12, offsetof(mavlink_component_information_t, general_metadata_uri) }, \ + { "peripherals_metadata_file_crc", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_component_information_t, peripherals_metadata_file_crc) }, \ + { "peripherals_metadata_uri", NULL, MAVLINK_TYPE_CHAR, 100, 112, offsetof(mavlink_component_information_t, peripherals_metadata_uri) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_COMPONENT_INFORMATION { \ "COMPONENT_INFORMATION", \ - 6, \ + 5, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_component_information_t, time_boot_ms) }, \ - { "metadata_type", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_component_information_t, metadata_type) }, \ - { "metadata_uid", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_component_information_t, metadata_uid) }, \ - { "metadata_uri", NULL, MAVLINK_TYPE_CHAR, 70, 16, offsetof(mavlink_component_information_t, metadata_uri) }, \ - { "translation_uid", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_component_information_t, translation_uid) }, \ - { "translation_uri", NULL, MAVLINK_TYPE_CHAR, 70, 86, offsetof(mavlink_component_information_t, translation_uri) }, \ + { "general_metadata_file_crc", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_component_information_t, general_metadata_file_crc) }, \ + { "general_metadata_uri", NULL, MAVLINK_TYPE_CHAR, 100, 12, offsetof(mavlink_component_information_t, general_metadata_uri) }, \ + { "peripherals_metadata_file_crc", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_component_information_t, peripherals_metadata_file_crc) }, \ + { "peripherals_metadata_uri", NULL, MAVLINK_TYPE_CHAR, 100, 112, offsetof(mavlink_component_information_t, peripherals_metadata_uri) }, \ } \ } #endif @@ -58,33 +55,30 @@ typedef struct __mavlink_component_information_t { * @param msg The MAVLink message to compress the data into * * @param time_boot_ms [ms] Timestamp (time since system boot). - * @param metadata_type The type of metadata being requested. - * @param metadata_uid Unique uid for this metadata which a gcs can use for created cached metadata and understanding whether it's cache it up to date or whether it needs to download new data. - * @param metadata_uri Component definition URI. If prefix mavlinkftp:// file is downloaded from vehicle over mavlink ftp protocol. If prefix http[s]:// file is downloaded over internet. Files are json format. They can end in .gz to indicate file is in gzip format. - * @param translation_uid Unique uid for the translation file associated with the metadata. - * @param translation_uri The translations for strings within the metadata file. If null the either do not exist or may be included in the metadata file itself. The translations specified here supercede any which may be in the metadata file itself. The uri format is the same as component_metadata_uri . Files are in Json Translation spec format. Empty string indicates no tranlsation file. + * @param general_metadata_file_crc CRC32 of the general metadata file (general_metadata_uri). + * @param general_metadata_uri MAVLink FTP URI for the general metadata file (COMP_METADATA_TYPE_GENERAL), which may be compressed with xz. The file contains general component metadata, and may contain URI links for additional metadata (see COMP_METADATA_TYPE). The information is static from boot, and may be generated at compile time. The string needs to be zero terminated. + * @param peripherals_metadata_file_crc CRC32 of peripherals metadata file (peripherals_metadata_uri). + * @param peripherals_metadata_uri (Optional) MAVLink FTP URI for the peripherals metadata file (COMP_METADATA_TYPE_PERIPHERALS), which may be compressed with xz. This contains data about "attached components" such as UAVCAN nodes. The peripherals are in a separate file because the information must be generated dynamically at runtime. The string needs to be zero terminated. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_component_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, uint32_t metadata_type, uint32_t metadata_uid, const char *metadata_uri, uint32_t translation_uid, const char *translation_uri) + uint32_t time_boot_ms, uint32_t general_metadata_file_crc, const char *general_metadata_uri, uint32_t peripherals_metadata_file_crc, const char *peripherals_metadata_uri) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN]; _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint32_t(buf, 4, metadata_type); - _mav_put_uint32_t(buf, 8, metadata_uid); - _mav_put_uint32_t(buf, 12, translation_uid); - _mav_put_char_array(buf, 16, metadata_uri, 70); - _mav_put_char_array(buf, 86, translation_uri, 70); + _mav_put_uint32_t(buf, 4, general_metadata_file_crc); + _mav_put_uint32_t(buf, 8, peripherals_metadata_file_crc); + _mav_put_char_array(buf, 12, general_metadata_uri, 100); + _mav_put_char_array(buf, 112, peripherals_metadata_uri, 100); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN); #else mavlink_component_information_t packet; packet.time_boot_ms = time_boot_ms; - packet.metadata_type = metadata_type; - packet.metadata_uid = metadata_uid; - packet.translation_uid = translation_uid; - mav_array_memcpy(packet.metadata_uri, metadata_uri, sizeof(char)*70); - mav_array_memcpy(packet.translation_uri, translation_uri, sizeof(char)*70); + packet.general_metadata_file_crc = general_metadata_file_crc; + packet.peripherals_metadata_file_crc = peripherals_metadata_file_crc; + mav_array_assign_char(packet.general_metadata_uri, general_metadata_uri, 100); + mav_array_assign_char(packet.peripherals_metadata_uri, peripherals_metadata_uri, 100); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN); #endif @@ -92,6 +86,49 @@ static inline uint16_t mavlink_msg_component_information_pack(uint8_t system_id, return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); } +/** + * @brief Pack a component_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param general_metadata_file_crc CRC32 of the general metadata file (general_metadata_uri). + * @param general_metadata_uri MAVLink FTP URI for the general metadata file (COMP_METADATA_TYPE_GENERAL), which may be compressed with xz. The file contains general component metadata, and may contain URI links for additional metadata (see COMP_METADATA_TYPE). The information is static from boot, and may be generated at compile time. The string needs to be zero terminated. + * @param peripherals_metadata_file_crc CRC32 of peripherals metadata file (peripherals_metadata_uri). + * @param peripherals_metadata_uri (Optional) MAVLink FTP URI for the peripherals metadata file (COMP_METADATA_TYPE_PERIPHERALS), which may be compressed with xz. This contains data about "attached components" such as UAVCAN nodes. The peripherals are in a separate file because the information must be generated dynamically at runtime. The string needs to be zero terminated. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_component_information_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint32_t general_metadata_file_crc, const char *general_metadata_uri, uint32_t peripherals_metadata_file_crc, const char *peripherals_metadata_uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, general_metadata_file_crc); + _mav_put_uint32_t(buf, 8, peripherals_metadata_file_crc); + _mav_put_char_array(buf, 12, general_metadata_uri, 100); + _mav_put_char_array(buf, 112, peripherals_metadata_uri, 100); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN); +#else + mavlink_component_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.general_metadata_file_crc = general_metadata_file_crc; + packet.peripherals_metadata_file_crc = peripherals_metadata_file_crc; + mav_array_memcpy(packet.general_metadata_uri, general_metadata_uri, sizeof(char)*100); + mav_array_memcpy(packet.peripherals_metadata_uri, peripherals_metadata_uri, sizeof(char)*100); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMPONENT_INFORMATION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN); +#endif +} + /** * @brief Pack a component_information message on a channel * @param system_id ID of this system @@ -99,34 +136,31 @@ static inline uint16_t mavlink_msg_component_information_pack(uint8_t system_id, * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms [ms] Timestamp (time since system boot). - * @param metadata_type The type of metadata being requested. - * @param metadata_uid Unique uid for this metadata which a gcs can use for created cached metadata and understanding whether it's cache it up to date or whether it needs to download new data. - * @param metadata_uri Component definition URI. If prefix mavlinkftp:// file is downloaded from vehicle over mavlink ftp protocol. If prefix http[s]:// file is downloaded over internet. Files are json format. They can end in .gz to indicate file is in gzip format. - * @param translation_uid Unique uid for the translation file associated with the metadata. - * @param translation_uri The translations for strings within the metadata file. If null the either do not exist or may be included in the metadata file itself. The translations specified here supercede any which may be in the metadata file itself. The uri format is the same as component_metadata_uri . Files are in Json Translation spec format. Empty string indicates no tranlsation file. + * @param general_metadata_file_crc CRC32 of the general metadata file (general_metadata_uri). + * @param general_metadata_uri MAVLink FTP URI for the general metadata file (COMP_METADATA_TYPE_GENERAL), which may be compressed with xz. The file contains general component metadata, and may contain URI links for additional metadata (see COMP_METADATA_TYPE). The information is static from boot, and may be generated at compile time. The string needs to be zero terminated. + * @param peripherals_metadata_file_crc CRC32 of peripherals metadata file (peripherals_metadata_uri). + * @param peripherals_metadata_uri (Optional) MAVLink FTP URI for the peripherals metadata file (COMP_METADATA_TYPE_PERIPHERALS), which may be compressed with xz. This contains data about "attached components" such as UAVCAN nodes. The peripherals are in a separate file because the information must be generated dynamically at runtime. The string needs to be zero terminated. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_component_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,uint32_t metadata_type,uint32_t metadata_uid,const char *metadata_uri,uint32_t translation_uid,const char *translation_uri) + uint32_t time_boot_ms,uint32_t general_metadata_file_crc,const char *general_metadata_uri,uint32_t peripherals_metadata_file_crc,const char *peripherals_metadata_uri) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN]; _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint32_t(buf, 4, metadata_type); - _mav_put_uint32_t(buf, 8, metadata_uid); - _mav_put_uint32_t(buf, 12, translation_uid); - _mav_put_char_array(buf, 16, metadata_uri, 70); - _mav_put_char_array(buf, 86, translation_uri, 70); + _mav_put_uint32_t(buf, 4, general_metadata_file_crc); + _mav_put_uint32_t(buf, 8, peripherals_metadata_file_crc); + _mav_put_char_array(buf, 12, general_metadata_uri, 100); + _mav_put_char_array(buf, 112, peripherals_metadata_uri, 100); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN); #else mavlink_component_information_t packet; packet.time_boot_ms = time_boot_ms; - packet.metadata_type = metadata_type; - packet.metadata_uid = metadata_uid; - packet.translation_uid = translation_uid; - mav_array_memcpy(packet.metadata_uri, metadata_uri, sizeof(char)*70); - mav_array_memcpy(packet.translation_uri, translation_uri, sizeof(char)*70); + packet.general_metadata_file_crc = general_metadata_file_crc; + packet.peripherals_metadata_file_crc = peripherals_metadata_file_crc; + mav_array_assign_char(packet.general_metadata_uri, general_metadata_uri, 100); + mav_array_assign_char(packet.peripherals_metadata_uri, peripherals_metadata_uri, 100); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN); #endif @@ -144,7 +178,7 @@ static inline uint16_t mavlink_msg_component_information_pack_chan(uint8_t syste */ static inline uint16_t mavlink_msg_component_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_component_information_t* component_information) { - return mavlink_msg_component_information_pack(system_id, component_id, msg, component_information->time_boot_ms, component_information->metadata_type, component_information->metadata_uid, component_information->metadata_uri, component_information->translation_uid, component_information->translation_uri); + return mavlink_msg_component_information_pack(system_id, component_id, msg, component_information->time_boot_ms, component_information->general_metadata_file_crc, component_information->general_metadata_uri, component_information->peripherals_metadata_file_crc, component_information->peripherals_metadata_uri); } /** @@ -158,7 +192,21 @@ static inline uint16_t mavlink_msg_component_information_encode(uint8_t system_i */ static inline uint16_t mavlink_msg_component_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_component_information_t* component_information) { - return mavlink_msg_component_information_pack_chan(system_id, component_id, chan, msg, component_information->time_boot_ms, component_information->metadata_type, component_information->metadata_uid, component_information->metadata_uri, component_information->translation_uid, component_information->translation_uri); + return mavlink_msg_component_information_pack_chan(system_id, component_id, chan, msg, component_information->time_boot_ms, component_information->general_metadata_file_crc, component_information->general_metadata_uri, component_information->peripherals_metadata_file_crc, component_information->peripherals_metadata_uri); +} + +/** + * @brief Encode a component_information struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param component_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_component_information_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_component_information_t* component_information) +{ + return mavlink_msg_component_information_pack_status(system_id, component_id, _status, msg, component_information->time_boot_ms, component_information->general_metadata_file_crc, component_information->general_metadata_uri, component_information->peripherals_metadata_file_crc, component_information->peripherals_metadata_uri); } /** @@ -166,33 +214,30 @@ static inline uint16_t mavlink_msg_component_information_encode_chan(uint8_t sys * @param chan MAVLink channel to send the message * * @param time_boot_ms [ms] Timestamp (time since system boot). - * @param metadata_type The type of metadata being requested. - * @param metadata_uid Unique uid for this metadata which a gcs can use for created cached metadata and understanding whether it's cache it up to date or whether it needs to download new data. - * @param metadata_uri Component definition URI. If prefix mavlinkftp:// file is downloaded from vehicle over mavlink ftp protocol. If prefix http[s]:// file is downloaded over internet. Files are json format. They can end in .gz to indicate file is in gzip format. - * @param translation_uid Unique uid for the translation file associated with the metadata. - * @param translation_uri The translations for strings within the metadata file. If null the either do not exist or may be included in the metadata file itself. The translations specified here supercede any which may be in the metadata file itself. The uri format is the same as component_metadata_uri . Files are in Json Translation spec format. Empty string indicates no tranlsation file. + * @param general_metadata_file_crc CRC32 of the general metadata file (general_metadata_uri). + * @param general_metadata_uri MAVLink FTP URI for the general metadata file (COMP_METADATA_TYPE_GENERAL), which may be compressed with xz. The file contains general component metadata, and may contain URI links for additional metadata (see COMP_METADATA_TYPE). The information is static from boot, and may be generated at compile time. The string needs to be zero terminated. + * @param peripherals_metadata_file_crc CRC32 of peripherals metadata file (peripherals_metadata_uri). + * @param peripherals_metadata_uri (Optional) MAVLink FTP URI for the peripherals metadata file (COMP_METADATA_TYPE_PERIPHERALS), which may be compressed with xz. This contains data about "attached components" such as UAVCAN nodes. The peripherals are in a separate file because the information must be generated dynamically at runtime. The string needs to be zero terminated. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_component_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t metadata_type, uint32_t metadata_uid, const char *metadata_uri, uint32_t translation_uid, const char *translation_uri) +static inline void mavlink_msg_component_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t general_metadata_file_crc, const char *general_metadata_uri, uint32_t peripherals_metadata_file_crc, const char *peripherals_metadata_uri) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN]; _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint32_t(buf, 4, metadata_type); - _mav_put_uint32_t(buf, 8, metadata_uid); - _mav_put_uint32_t(buf, 12, translation_uid); - _mav_put_char_array(buf, 16, metadata_uri, 70); - _mav_put_char_array(buf, 86, translation_uri, 70); + _mav_put_uint32_t(buf, 4, general_metadata_file_crc); + _mav_put_uint32_t(buf, 8, peripherals_metadata_file_crc); + _mav_put_char_array(buf, 12, general_metadata_uri, 100); + _mav_put_char_array(buf, 112, peripherals_metadata_uri, 100); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION, buf, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); #else mavlink_component_information_t packet; packet.time_boot_ms = time_boot_ms; - packet.metadata_type = metadata_type; - packet.metadata_uid = metadata_uid; - packet.translation_uid = translation_uid; - mav_array_memcpy(packet.metadata_uri, metadata_uri, sizeof(char)*70); - mav_array_memcpy(packet.translation_uri, translation_uri, sizeof(char)*70); + packet.general_metadata_file_crc = general_metadata_file_crc; + packet.peripherals_metadata_file_crc = peripherals_metadata_file_crc; + mav_array_assign_char(packet.general_metadata_uri, general_metadata_uri, 100); + mav_array_assign_char(packet.peripherals_metadata_uri, peripherals_metadata_uri, 100); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); #endif } @@ -205,7 +250,7 @@ static inline void mavlink_msg_component_information_send(mavlink_channel_t chan static inline void mavlink_msg_component_information_send_struct(mavlink_channel_t chan, const mavlink_component_information_t* component_information) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_component_information_send(chan, component_information->time_boot_ms, component_information->metadata_type, component_information->metadata_uid, component_information->metadata_uri, component_information->translation_uid, component_information->translation_uri); + mavlink_msg_component_information_send(chan, component_information->time_boot_ms, component_information->general_metadata_file_crc, component_information->general_metadata_uri, component_information->peripherals_metadata_file_crc, component_information->peripherals_metadata_uri); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION, (const char *)component_information, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); #endif @@ -213,31 +258,29 @@ static inline void mavlink_msg_component_information_send_struct(mavlink_channel #if MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_component_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t metadata_type, uint32_t metadata_uid, const char *metadata_uri, uint32_t translation_uid, const char *translation_uri) +static inline void mavlink_msg_component_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t general_metadata_file_crc, const char *general_metadata_uri, uint32_t peripherals_metadata_file_crc, const char *peripherals_metadata_uri) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint32_t(buf, 0, time_boot_ms); - _mav_put_uint32_t(buf, 4, metadata_type); - _mav_put_uint32_t(buf, 8, metadata_uid); - _mav_put_uint32_t(buf, 12, translation_uid); - _mav_put_char_array(buf, 16, metadata_uri, 70); - _mav_put_char_array(buf, 86, translation_uri, 70); + _mav_put_uint32_t(buf, 4, general_metadata_file_crc); + _mav_put_uint32_t(buf, 8, peripherals_metadata_file_crc); + _mav_put_char_array(buf, 12, general_metadata_uri, 100); + _mav_put_char_array(buf, 112, peripherals_metadata_uri, 100); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION, buf, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); #else mavlink_component_information_t *packet = (mavlink_component_information_t *)msgbuf; packet->time_boot_ms = time_boot_ms; - packet->metadata_type = metadata_type; - packet->metadata_uid = metadata_uid; - packet->translation_uid = translation_uid; - mav_array_memcpy(packet->metadata_uri, metadata_uri, sizeof(char)*70); - mav_array_memcpy(packet->translation_uri, translation_uri, sizeof(char)*70); + packet->general_metadata_file_crc = general_metadata_file_crc; + packet->peripherals_metadata_file_crc = peripherals_metadata_file_crc; + mav_array_assign_char(packet->general_metadata_uri, general_metadata_uri, 100); + mav_array_assign_char(packet->peripherals_metadata_uri, peripherals_metadata_uri, 100); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); #endif } @@ -259,53 +302,43 @@ static inline uint32_t mavlink_msg_component_information_get_time_boot_ms(const } /** - * @brief Get field metadata_type from component_information message + * @brief Get field general_metadata_file_crc from component_information message * - * @return The type of metadata being requested. + * @return CRC32 of the general metadata file (general_metadata_uri). */ -static inline uint32_t mavlink_msg_component_information_get_metadata_type(const mavlink_message_t* msg) +static inline uint32_t mavlink_msg_component_information_get_general_metadata_file_crc(const mavlink_message_t* msg) { return _MAV_RETURN_uint32_t(msg, 4); } /** - * @brief Get field metadata_uid from component_information message - * - * @return Unique uid for this metadata which a gcs can use for created cached metadata and understanding whether it's cache it up to date or whether it needs to download new data. - */ -static inline uint32_t mavlink_msg_component_information_get_metadata_uid(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 8); -} - -/** - * @brief Get field metadata_uri from component_information message + * @brief Get field general_metadata_uri from component_information message * - * @return Component definition URI. If prefix mavlinkftp:// file is downloaded from vehicle over mavlink ftp protocol. If prefix http[s]:// file is downloaded over internet. Files are json format. They can end in .gz to indicate file is in gzip format. + * @return MAVLink FTP URI for the general metadata file (COMP_METADATA_TYPE_GENERAL), which may be compressed with xz. The file contains general component metadata, and may contain URI links for additional metadata (see COMP_METADATA_TYPE). The information is static from boot, and may be generated at compile time. The string needs to be zero terminated. */ -static inline uint16_t mavlink_msg_component_information_get_metadata_uri(const mavlink_message_t* msg, char *metadata_uri) +static inline uint16_t mavlink_msg_component_information_get_general_metadata_uri(const mavlink_message_t* msg, char *general_metadata_uri) { - return _MAV_RETURN_char_array(msg, metadata_uri, 70, 16); + return _MAV_RETURN_char_array(msg, general_metadata_uri, 100, 12); } /** - * @brief Get field translation_uid from component_information message + * @brief Get field peripherals_metadata_file_crc from component_information message * - * @return Unique uid for the translation file associated with the metadata. + * @return CRC32 of peripherals metadata file (peripherals_metadata_uri). */ -static inline uint32_t mavlink_msg_component_information_get_translation_uid(const mavlink_message_t* msg) +static inline uint32_t mavlink_msg_component_information_get_peripherals_metadata_file_crc(const mavlink_message_t* msg) { - return _MAV_RETURN_uint32_t(msg, 12); + return _MAV_RETURN_uint32_t(msg, 8); } /** - * @brief Get field translation_uri from component_information message + * @brief Get field peripherals_metadata_uri from component_information message * - * @return The translations for strings within the metadata file. If null the either do not exist or may be included in the metadata file itself. The translations specified here supercede any which may be in the metadata file itself. The uri format is the same as component_metadata_uri . Files are in Json Translation spec format. Empty string indicates no tranlsation file. + * @return (Optional) MAVLink FTP URI for the peripherals metadata file (COMP_METADATA_TYPE_PERIPHERALS), which may be compressed with xz. This contains data about "attached components" such as UAVCAN nodes. The peripherals are in a separate file because the information must be generated dynamically at runtime. The string needs to be zero terminated. */ -static inline uint16_t mavlink_msg_component_information_get_translation_uri(const mavlink_message_t* msg, char *translation_uri) +static inline uint16_t mavlink_msg_component_information_get_peripherals_metadata_uri(const mavlink_message_t* msg, char *peripherals_metadata_uri) { - return _MAV_RETURN_char_array(msg, translation_uri, 70, 86); + return _MAV_RETURN_char_array(msg, peripherals_metadata_uri, 100, 112); } /** @@ -318,11 +351,10 @@ static inline void mavlink_msg_component_information_decode(const mavlink_messag { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS component_information->time_boot_ms = mavlink_msg_component_information_get_time_boot_ms(msg); - component_information->metadata_type = mavlink_msg_component_information_get_metadata_type(msg); - component_information->metadata_uid = mavlink_msg_component_information_get_metadata_uid(msg); - component_information->translation_uid = mavlink_msg_component_information_get_translation_uid(msg); - mavlink_msg_component_information_get_metadata_uri(msg, component_information->metadata_uri); - mavlink_msg_component_information_get_translation_uri(msg, component_information->translation_uri); + component_information->general_metadata_file_crc = mavlink_msg_component_information_get_general_metadata_file_crc(msg); + component_information->peripherals_metadata_file_crc = mavlink_msg_component_information_get_peripherals_metadata_file_crc(msg); + mavlink_msg_component_information_get_general_metadata_uri(msg, component_information->general_metadata_uri); + mavlink_msg_component_information_get_peripherals_metadata_uri(msg, component_information->peripherals_metadata_uri); #else uint8_t len = msg->len < MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN; memset(component_information, 0, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_component_information_basic.h b/lib/main/MAVLink/common/mavlink_msg_component_information_basic.h new file mode 100644 index 00000000000..bbe75c1a7c3 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_component_information_basic.h @@ -0,0 +1,450 @@ +#pragma once +// MESSAGE COMPONENT_INFORMATION_BASIC PACKING + +#define MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC 396 + + +typedef struct __mavlink_component_information_basic_t { + uint64_t capabilities; /*< Component capability flags*/ + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + uint32_t time_manufacture_s; /*< [s] Date of manufacture as a UNIX Epoch time (since 1.1.1970) in seconds.*/ + char vendor_name[32]; /*< Name of the component vendor. Needs to be zero terminated. The field is optional and can be empty/all zeros.*/ + char model_name[32]; /*< Name of the component model. Needs to be zero terminated. The field is optional and can be empty/all zeros.*/ + char software_version[24]; /*< Software version. The recommended format is SEMVER: 'major.minor.patch' (any format may be used). The field must be zero terminated if it has a value. The field is optional and can be empty/all zeros.*/ + char hardware_version[24]; /*< Hardware version. The recommended format is SEMVER: 'major.minor.patch' (any format may be used). The field must be zero terminated if it has a value. The field is optional and can be empty/all zeros.*/ + char serial_number[32]; /*< Hardware serial number. The field must be zero terminated if it has a value. The field is optional and can be empty/all zeros.*/ +} mavlink_component_information_basic_t; + +#define MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN 160 +#define MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_MIN_LEN 160 +#define MAVLINK_MSG_ID_396_LEN 160 +#define MAVLINK_MSG_ID_396_MIN_LEN 160 + +#define MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_CRC 50 +#define MAVLINK_MSG_ID_396_CRC 50 + +#define MAVLINK_MSG_COMPONENT_INFORMATION_BASIC_FIELD_VENDOR_NAME_LEN 32 +#define MAVLINK_MSG_COMPONENT_INFORMATION_BASIC_FIELD_MODEL_NAME_LEN 32 +#define MAVLINK_MSG_COMPONENT_INFORMATION_BASIC_FIELD_SOFTWARE_VERSION_LEN 24 +#define MAVLINK_MSG_COMPONENT_INFORMATION_BASIC_FIELD_HARDWARE_VERSION_LEN 24 +#define MAVLINK_MSG_COMPONENT_INFORMATION_BASIC_FIELD_SERIAL_NUMBER_LEN 32 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_COMPONENT_INFORMATION_BASIC { \ + 396, \ + "COMPONENT_INFORMATION_BASIC", \ + 8, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_component_information_basic_t, time_boot_ms) }, \ + { "capabilities", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_component_information_basic_t, capabilities) }, \ + { "time_manufacture_s", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_component_information_basic_t, time_manufacture_s) }, \ + { "vendor_name", NULL, MAVLINK_TYPE_CHAR, 32, 16, offsetof(mavlink_component_information_basic_t, vendor_name) }, \ + { "model_name", NULL, MAVLINK_TYPE_CHAR, 32, 48, offsetof(mavlink_component_information_basic_t, model_name) }, \ + { "software_version", NULL, MAVLINK_TYPE_CHAR, 24, 80, offsetof(mavlink_component_information_basic_t, software_version) }, \ + { "hardware_version", NULL, MAVLINK_TYPE_CHAR, 24, 104, offsetof(mavlink_component_information_basic_t, hardware_version) }, \ + { "serial_number", NULL, MAVLINK_TYPE_CHAR, 32, 128, offsetof(mavlink_component_information_basic_t, serial_number) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_COMPONENT_INFORMATION_BASIC { \ + "COMPONENT_INFORMATION_BASIC", \ + 8, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_component_information_basic_t, time_boot_ms) }, \ + { "capabilities", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_component_information_basic_t, capabilities) }, \ + { "time_manufacture_s", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_component_information_basic_t, time_manufacture_s) }, \ + { "vendor_name", NULL, MAVLINK_TYPE_CHAR, 32, 16, offsetof(mavlink_component_information_basic_t, vendor_name) }, \ + { "model_name", NULL, MAVLINK_TYPE_CHAR, 32, 48, offsetof(mavlink_component_information_basic_t, model_name) }, \ + { "software_version", NULL, MAVLINK_TYPE_CHAR, 24, 80, offsetof(mavlink_component_information_basic_t, software_version) }, \ + { "hardware_version", NULL, MAVLINK_TYPE_CHAR, 24, 104, offsetof(mavlink_component_information_basic_t, hardware_version) }, \ + { "serial_number", NULL, MAVLINK_TYPE_CHAR, 32, 128, offsetof(mavlink_component_information_basic_t, serial_number) }, \ + } \ +} +#endif + +/** + * @brief Pack a component_information_basic message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param capabilities Component capability flags + * @param time_manufacture_s [s] Date of manufacture as a UNIX Epoch time (since 1.1.1970) in seconds. + * @param vendor_name Name of the component vendor. Needs to be zero terminated. The field is optional and can be empty/all zeros. + * @param model_name Name of the component model. Needs to be zero terminated. The field is optional and can be empty/all zeros. + * @param software_version Software version. The recommended format is SEMVER: 'major.minor.patch' (any format may be used). The field must be zero terminated if it has a value. The field is optional and can be empty/all zeros. + * @param hardware_version Hardware version. The recommended format is SEMVER: 'major.minor.patch' (any format may be used). The field must be zero terminated if it has a value. The field is optional and can be empty/all zeros. + * @param serial_number Hardware serial number. The field must be zero terminated if it has a value. The field is optional and can be empty/all zeros. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_component_information_basic_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint64_t capabilities, uint32_t time_manufacture_s, const char *vendor_name, const char *model_name, const char *software_version, const char *hardware_version, const char *serial_number) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN]; + _mav_put_uint64_t(buf, 0, capabilities); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_uint32_t(buf, 12, time_manufacture_s); + _mav_put_char_array(buf, 16, vendor_name, 32); + _mav_put_char_array(buf, 48, model_name, 32); + _mav_put_char_array(buf, 80, software_version, 24); + _mav_put_char_array(buf, 104, hardware_version, 24); + _mav_put_char_array(buf, 128, serial_number, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN); +#else + mavlink_component_information_basic_t packet; + packet.capabilities = capabilities; + packet.time_boot_ms = time_boot_ms; + packet.time_manufacture_s = time_manufacture_s; + mav_array_assign_char(packet.vendor_name, vendor_name, 32); + mav_array_assign_char(packet.model_name, model_name, 32); + mav_array_assign_char(packet.software_version, software_version, 24); + mav_array_assign_char(packet.hardware_version, hardware_version, 24); + mav_array_assign_char(packet.serial_number, serial_number, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_CRC); +} + +/** + * @brief Pack a component_information_basic message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param capabilities Component capability flags + * @param time_manufacture_s [s] Date of manufacture as a UNIX Epoch time (since 1.1.1970) in seconds. + * @param vendor_name Name of the component vendor. Needs to be zero terminated. The field is optional and can be empty/all zeros. + * @param model_name Name of the component model. Needs to be zero terminated. The field is optional and can be empty/all zeros. + * @param software_version Software version. The recommended format is SEMVER: 'major.minor.patch' (any format may be used). The field must be zero terminated if it has a value. The field is optional and can be empty/all zeros. + * @param hardware_version Hardware version. The recommended format is SEMVER: 'major.minor.patch' (any format may be used). The field must be zero terminated if it has a value. The field is optional and can be empty/all zeros. + * @param serial_number Hardware serial number. The field must be zero terminated if it has a value. The field is optional and can be empty/all zeros. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_component_information_basic_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint64_t capabilities, uint32_t time_manufacture_s, const char *vendor_name, const char *model_name, const char *software_version, const char *hardware_version, const char *serial_number) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN]; + _mav_put_uint64_t(buf, 0, capabilities); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_uint32_t(buf, 12, time_manufacture_s); + _mav_put_char_array(buf, 16, vendor_name, 32); + _mav_put_char_array(buf, 48, model_name, 32); + _mav_put_char_array(buf, 80, software_version, 24); + _mav_put_char_array(buf, 104, hardware_version, 24); + _mav_put_char_array(buf, 128, serial_number, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN); +#else + mavlink_component_information_basic_t packet; + packet.capabilities = capabilities; + packet.time_boot_ms = time_boot_ms; + packet.time_manufacture_s = time_manufacture_s; + mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(char)*32); + mav_array_memcpy(packet.model_name, model_name, sizeof(char)*32); + mav_array_memcpy(packet.software_version, software_version, sizeof(char)*24); + mav_array_memcpy(packet.hardware_version, hardware_version, sizeof(char)*24); + mav_array_memcpy(packet.serial_number, serial_number, sizeof(char)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN); +#endif +} + +/** + * @brief Pack a component_information_basic message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param capabilities Component capability flags + * @param time_manufacture_s [s] Date of manufacture as a UNIX Epoch time (since 1.1.1970) in seconds. + * @param vendor_name Name of the component vendor. Needs to be zero terminated. The field is optional and can be empty/all zeros. + * @param model_name Name of the component model. Needs to be zero terminated. The field is optional and can be empty/all zeros. + * @param software_version Software version. The recommended format is SEMVER: 'major.minor.patch' (any format may be used). The field must be zero terminated if it has a value. The field is optional and can be empty/all zeros. + * @param hardware_version Hardware version. The recommended format is SEMVER: 'major.minor.patch' (any format may be used). The field must be zero terminated if it has a value. The field is optional and can be empty/all zeros. + * @param serial_number Hardware serial number. The field must be zero terminated if it has a value. The field is optional and can be empty/all zeros. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_component_information_basic_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint64_t capabilities,uint32_t time_manufacture_s,const char *vendor_name,const char *model_name,const char *software_version,const char *hardware_version,const char *serial_number) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN]; + _mav_put_uint64_t(buf, 0, capabilities); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_uint32_t(buf, 12, time_manufacture_s); + _mav_put_char_array(buf, 16, vendor_name, 32); + _mav_put_char_array(buf, 48, model_name, 32); + _mav_put_char_array(buf, 80, software_version, 24); + _mav_put_char_array(buf, 104, hardware_version, 24); + _mav_put_char_array(buf, 128, serial_number, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN); +#else + mavlink_component_information_basic_t packet; + packet.capabilities = capabilities; + packet.time_boot_ms = time_boot_ms; + packet.time_manufacture_s = time_manufacture_s; + mav_array_assign_char(packet.vendor_name, vendor_name, 32); + mav_array_assign_char(packet.model_name, model_name, 32); + mav_array_assign_char(packet.software_version, software_version, 24); + mav_array_assign_char(packet.hardware_version, hardware_version, 24); + mav_array_assign_char(packet.serial_number, serial_number, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_CRC); +} + +/** + * @brief Encode a component_information_basic struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param component_information_basic C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_component_information_basic_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_component_information_basic_t* component_information_basic) +{ + return mavlink_msg_component_information_basic_pack(system_id, component_id, msg, component_information_basic->time_boot_ms, component_information_basic->capabilities, component_information_basic->time_manufacture_s, component_information_basic->vendor_name, component_information_basic->model_name, component_information_basic->software_version, component_information_basic->hardware_version, component_information_basic->serial_number); +} + +/** + * @brief Encode a component_information_basic struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param component_information_basic C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_component_information_basic_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_component_information_basic_t* component_information_basic) +{ + return mavlink_msg_component_information_basic_pack_chan(system_id, component_id, chan, msg, component_information_basic->time_boot_ms, component_information_basic->capabilities, component_information_basic->time_manufacture_s, component_information_basic->vendor_name, component_information_basic->model_name, component_information_basic->software_version, component_information_basic->hardware_version, component_information_basic->serial_number); +} + +/** + * @brief Encode a component_information_basic struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param component_information_basic C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_component_information_basic_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_component_information_basic_t* component_information_basic) +{ + return mavlink_msg_component_information_basic_pack_status(system_id, component_id, _status, msg, component_information_basic->time_boot_ms, component_information_basic->capabilities, component_information_basic->time_manufacture_s, component_information_basic->vendor_name, component_information_basic->model_name, component_information_basic->software_version, component_information_basic->hardware_version, component_information_basic->serial_number); +} + +/** + * @brief Send a component_information_basic message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param capabilities Component capability flags + * @param time_manufacture_s [s] Date of manufacture as a UNIX Epoch time (since 1.1.1970) in seconds. + * @param vendor_name Name of the component vendor. Needs to be zero terminated. The field is optional and can be empty/all zeros. + * @param model_name Name of the component model. Needs to be zero terminated. The field is optional and can be empty/all zeros. + * @param software_version Software version. The recommended format is SEMVER: 'major.minor.patch' (any format may be used). The field must be zero terminated if it has a value. The field is optional and can be empty/all zeros. + * @param hardware_version Hardware version. The recommended format is SEMVER: 'major.minor.patch' (any format may be used). The field must be zero terminated if it has a value. The field is optional and can be empty/all zeros. + * @param serial_number Hardware serial number. The field must be zero terminated if it has a value. The field is optional and can be empty/all zeros. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_component_information_basic_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t capabilities, uint32_t time_manufacture_s, const char *vendor_name, const char *model_name, const char *software_version, const char *hardware_version, const char *serial_number) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN]; + _mav_put_uint64_t(buf, 0, capabilities); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_uint32_t(buf, 12, time_manufacture_s); + _mav_put_char_array(buf, 16, vendor_name, 32); + _mav_put_char_array(buf, 48, model_name, 32); + _mav_put_char_array(buf, 80, software_version, 24); + _mav_put_char_array(buf, 104, hardware_version, 24); + _mav_put_char_array(buf, 128, serial_number, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC, buf, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_CRC); +#else + mavlink_component_information_basic_t packet; + packet.capabilities = capabilities; + packet.time_boot_ms = time_boot_ms; + packet.time_manufacture_s = time_manufacture_s; + mav_array_assign_char(packet.vendor_name, vendor_name, 32); + mav_array_assign_char(packet.model_name, model_name, 32); + mav_array_assign_char(packet.software_version, software_version, 24); + mav_array_assign_char(packet.hardware_version, hardware_version, 24); + mav_array_assign_char(packet.serial_number, serial_number, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC, (const char *)&packet, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_CRC); +#endif +} + +/** + * @brief Send a component_information_basic message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_component_information_basic_send_struct(mavlink_channel_t chan, const mavlink_component_information_basic_t* component_information_basic) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_component_information_basic_send(chan, component_information_basic->time_boot_ms, component_information_basic->capabilities, component_information_basic->time_manufacture_s, component_information_basic->vendor_name, component_information_basic->model_name, component_information_basic->software_version, component_information_basic->hardware_version, component_information_basic->serial_number); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC, (const char *)component_information_basic, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_CRC); +#endif +} + +#if MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_component_information_basic_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t capabilities, uint32_t time_manufacture_s, const char *vendor_name, const char *model_name, const char *software_version, const char *hardware_version, const char *serial_number) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, capabilities); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_uint32_t(buf, 12, time_manufacture_s); + _mav_put_char_array(buf, 16, vendor_name, 32); + _mav_put_char_array(buf, 48, model_name, 32); + _mav_put_char_array(buf, 80, software_version, 24); + _mav_put_char_array(buf, 104, hardware_version, 24); + _mav_put_char_array(buf, 128, serial_number, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC, buf, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_CRC); +#else + mavlink_component_information_basic_t *packet = (mavlink_component_information_basic_t *)msgbuf; + packet->capabilities = capabilities; + packet->time_boot_ms = time_boot_ms; + packet->time_manufacture_s = time_manufacture_s; + mav_array_assign_char(packet->vendor_name, vendor_name, 32); + mav_array_assign_char(packet->model_name, model_name, 32); + mav_array_assign_char(packet->software_version, software_version, 24); + mav_array_assign_char(packet->hardware_version, hardware_version, 24); + mav_array_assign_char(packet->serial_number, serial_number, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC, (const char *)packet, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_CRC); +#endif +} +#endif + +#endif + +// MESSAGE COMPONENT_INFORMATION_BASIC UNPACKING + + +/** + * @brief Get field time_boot_ms from component_information_basic message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_component_information_basic_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field capabilities from component_information_basic message + * + * @return Component capability flags + */ +static inline uint64_t mavlink_msg_component_information_basic_get_capabilities(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field time_manufacture_s from component_information_basic message + * + * @return [s] Date of manufacture as a UNIX Epoch time (since 1.1.1970) in seconds. + */ +static inline uint32_t mavlink_msg_component_information_basic_get_time_manufacture_s(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 12); +} + +/** + * @brief Get field vendor_name from component_information_basic message + * + * @return Name of the component vendor. Needs to be zero terminated. The field is optional and can be empty/all zeros. + */ +static inline uint16_t mavlink_msg_component_information_basic_get_vendor_name(const mavlink_message_t* msg, char *vendor_name) +{ + return _MAV_RETURN_char_array(msg, vendor_name, 32, 16); +} + +/** + * @brief Get field model_name from component_information_basic message + * + * @return Name of the component model. Needs to be zero terminated. The field is optional and can be empty/all zeros. + */ +static inline uint16_t mavlink_msg_component_information_basic_get_model_name(const mavlink_message_t* msg, char *model_name) +{ + return _MAV_RETURN_char_array(msg, model_name, 32, 48); +} + +/** + * @brief Get field software_version from component_information_basic message + * + * @return Software version. The recommended format is SEMVER: 'major.minor.patch' (any format may be used). The field must be zero terminated if it has a value. The field is optional and can be empty/all zeros. + */ +static inline uint16_t mavlink_msg_component_information_basic_get_software_version(const mavlink_message_t* msg, char *software_version) +{ + return _MAV_RETURN_char_array(msg, software_version, 24, 80); +} + +/** + * @brief Get field hardware_version from component_information_basic message + * + * @return Hardware version. The recommended format is SEMVER: 'major.minor.patch' (any format may be used). The field must be zero terminated if it has a value. The field is optional and can be empty/all zeros. + */ +static inline uint16_t mavlink_msg_component_information_basic_get_hardware_version(const mavlink_message_t* msg, char *hardware_version) +{ + return _MAV_RETURN_char_array(msg, hardware_version, 24, 104); +} + +/** + * @brief Get field serial_number from component_information_basic message + * + * @return Hardware serial number. The field must be zero terminated if it has a value. The field is optional and can be empty/all zeros. + */ +static inline uint16_t mavlink_msg_component_information_basic_get_serial_number(const mavlink_message_t* msg, char *serial_number) +{ + return _MAV_RETURN_char_array(msg, serial_number, 32, 128); +} + +/** + * @brief Decode a component_information_basic message into a struct + * + * @param msg The message to decode + * @param component_information_basic C-struct to decode the message contents into + */ +static inline void mavlink_msg_component_information_basic_decode(const mavlink_message_t* msg, mavlink_component_information_basic_t* component_information_basic) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + component_information_basic->capabilities = mavlink_msg_component_information_basic_get_capabilities(msg); + component_information_basic->time_boot_ms = mavlink_msg_component_information_basic_get_time_boot_ms(msg); + component_information_basic->time_manufacture_s = mavlink_msg_component_information_basic_get_time_manufacture_s(msg); + mavlink_msg_component_information_basic_get_vendor_name(msg, component_information_basic->vendor_name); + mavlink_msg_component_information_basic_get_model_name(msg, component_information_basic->model_name); + mavlink_msg_component_information_basic_get_software_version(msg, component_information_basic->software_version); + mavlink_msg_component_information_basic_get_hardware_version(msg, component_information_basic->hardware_version); + mavlink_msg_component_information_basic_get_serial_number(msg, component_information_basic->serial_number); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN? msg->len : MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN; + memset(component_information_basic, 0, MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_LEN); + memcpy(component_information_basic, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_component_metadata.h b/lib/main/MAVLink/common/mavlink_msg_component_metadata.h new file mode 100644 index 00000000000..d51c2f847a5 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_component_metadata.h @@ -0,0 +1,306 @@ +#pragma once +// MESSAGE COMPONENT_METADATA PACKING + +#define MAVLINK_MSG_ID_COMPONENT_METADATA 397 + + +typedef struct __mavlink_component_metadata_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + uint32_t file_crc; /*< CRC32 of the general metadata file.*/ + char uri[100]; /*< MAVLink FTP URI for the general metadata file (COMP_METADATA_TYPE_GENERAL), which may be compressed with xz. The file contains general component metadata, and may contain URI links for additional metadata (see COMP_METADATA_TYPE). The information is static from boot, and may be generated at compile time. The string needs to be zero terminated.*/ +} mavlink_component_metadata_t; + +#define MAVLINK_MSG_ID_COMPONENT_METADATA_LEN 108 +#define MAVLINK_MSG_ID_COMPONENT_METADATA_MIN_LEN 108 +#define MAVLINK_MSG_ID_397_LEN 108 +#define MAVLINK_MSG_ID_397_MIN_LEN 108 + +#define MAVLINK_MSG_ID_COMPONENT_METADATA_CRC 182 +#define MAVLINK_MSG_ID_397_CRC 182 + +#define MAVLINK_MSG_COMPONENT_METADATA_FIELD_URI_LEN 100 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_COMPONENT_METADATA { \ + 397, \ + "COMPONENT_METADATA", \ + 3, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_component_metadata_t, time_boot_ms) }, \ + { "file_crc", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_component_metadata_t, file_crc) }, \ + { "uri", NULL, MAVLINK_TYPE_CHAR, 100, 8, offsetof(mavlink_component_metadata_t, uri) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_COMPONENT_METADATA { \ + "COMPONENT_METADATA", \ + 3, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_component_metadata_t, time_boot_ms) }, \ + { "file_crc", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_component_metadata_t, file_crc) }, \ + { "uri", NULL, MAVLINK_TYPE_CHAR, 100, 8, offsetof(mavlink_component_metadata_t, uri) }, \ + } \ +} +#endif + +/** + * @brief Pack a component_metadata message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param file_crc CRC32 of the general metadata file. + * @param uri MAVLink FTP URI for the general metadata file (COMP_METADATA_TYPE_GENERAL), which may be compressed with xz. The file contains general component metadata, and may contain URI links for additional metadata (see COMP_METADATA_TYPE). The information is static from boot, and may be generated at compile time. The string needs to be zero terminated. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_component_metadata_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint32_t file_crc, const char *uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPONENT_METADATA_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, file_crc); + _mav_put_char_array(buf, 8, uri, 100); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPONENT_METADATA_LEN); +#else + mavlink_component_metadata_t packet; + packet.time_boot_ms = time_boot_ms; + packet.file_crc = file_crc; + mav_array_assign_char(packet.uri, uri, 100); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPONENT_METADATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMPONENT_METADATA; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMPONENT_METADATA_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_METADATA_LEN, MAVLINK_MSG_ID_COMPONENT_METADATA_CRC); +} + +/** + * @brief Pack a component_metadata message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param file_crc CRC32 of the general metadata file. + * @param uri MAVLink FTP URI for the general metadata file (COMP_METADATA_TYPE_GENERAL), which may be compressed with xz. The file contains general component metadata, and may contain URI links for additional metadata (see COMP_METADATA_TYPE). The information is static from boot, and may be generated at compile time. The string needs to be zero terminated. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_component_metadata_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint32_t file_crc, const char *uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPONENT_METADATA_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, file_crc); + _mav_put_char_array(buf, 8, uri, 100); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPONENT_METADATA_LEN); +#else + mavlink_component_metadata_t packet; + packet.time_boot_ms = time_boot_ms; + packet.file_crc = file_crc; + mav_array_memcpy(packet.uri, uri, sizeof(char)*100); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPONENT_METADATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMPONENT_METADATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_COMPONENT_METADATA_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_METADATA_LEN, MAVLINK_MSG_ID_COMPONENT_METADATA_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_COMPONENT_METADATA_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_METADATA_LEN); +#endif +} + +/** + * @brief Pack a component_metadata message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param file_crc CRC32 of the general metadata file. + * @param uri MAVLink FTP URI for the general metadata file (COMP_METADATA_TYPE_GENERAL), which may be compressed with xz. The file contains general component metadata, and may contain URI links for additional metadata (see COMP_METADATA_TYPE). The information is static from boot, and may be generated at compile time. The string needs to be zero terminated. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_component_metadata_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint32_t file_crc,const char *uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPONENT_METADATA_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, file_crc); + _mav_put_char_array(buf, 8, uri, 100); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPONENT_METADATA_LEN); +#else + mavlink_component_metadata_t packet; + packet.time_boot_ms = time_boot_ms; + packet.file_crc = file_crc; + mav_array_assign_char(packet.uri, uri, 100); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPONENT_METADATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMPONENT_METADATA; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMPONENT_METADATA_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_METADATA_LEN, MAVLINK_MSG_ID_COMPONENT_METADATA_CRC); +} + +/** + * @brief Encode a component_metadata struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param component_metadata C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_component_metadata_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_component_metadata_t* component_metadata) +{ + return mavlink_msg_component_metadata_pack(system_id, component_id, msg, component_metadata->time_boot_ms, component_metadata->file_crc, component_metadata->uri); +} + +/** + * @brief Encode a component_metadata struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param component_metadata C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_component_metadata_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_component_metadata_t* component_metadata) +{ + return mavlink_msg_component_metadata_pack_chan(system_id, component_id, chan, msg, component_metadata->time_boot_ms, component_metadata->file_crc, component_metadata->uri); +} + +/** + * @brief Encode a component_metadata struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param component_metadata C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_component_metadata_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_component_metadata_t* component_metadata) +{ + return mavlink_msg_component_metadata_pack_status(system_id, component_id, _status, msg, component_metadata->time_boot_ms, component_metadata->file_crc, component_metadata->uri); +} + +/** + * @brief Send a component_metadata message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param file_crc CRC32 of the general metadata file. + * @param uri MAVLink FTP URI for the general metadata file (COMP_METADATA_TYPE_GENERAL), which may be compressed with xz. The file contains general component metadata, and may contain URI links for additional metadata (see COMP_METADATA_TYPE). The information is static from boot, and may be generated at compile time. The string needs to be zero terminated. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_component_metadata_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t file_crc, const char *uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPONENT_METADATA_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, file_crc); + _mav_put_char_array(buf, 8, uri, 100); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_METADATA, buf, MAVLINK_MSG_ID_COMPONENT_METADATA_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_METADATA_LEN, MAVLINK_MSG_ID_COMPONENT_METADATA_CRC); +#else + mavlink_component_metadata_t packet; + packet.time_boot_ms = time_boot_ms; + packet.file_crc = file_crc; + mav_array_assign_char(packet.uri, uri, 100); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_METADATA, (const char *)&packet, MAVLINK_MSG_ID_COMPONENT_METADATA_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_METADATA_LEN, MAVLINK_MSG_ID_COMPONENT_METADATA_CRC); +#endif +} + +/** + * @brief Send a component_metadata message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_component_metadata_send_struct(mavlink_channel_t chan, const mavlink_component_metadata_t* component_metadata) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_component_metadata_send(chan, component_metadata->time_boot_ms, component_metadata->file_crc, component_metadata->uri); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_METADATA, (const char *)component_metadata, MAVLINK_MSG_ID_COMPONENT_METADATA_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_METADATA_LEN, MAVLINK_MSG_ID_COMPONENT_METADATA_CRC); +#endif +} + +#if MAVLINK_MSG_ID_COMPONENT_METADATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_component_metadata_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t file_crc, const char *uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, file_crc); + _mav_put_char_array(buf, 8, uri, 100); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_METADATA, buf, MAVLINK_MSG_ID_COMPONENT_METADATA_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_METADATA_LEN, MAVLINK_MSG_ID_COMPONENT_METADATA_CRC); +#else + mavlink_component_metadata_t *packet = (mavlink_component_metadata_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->file_crc = file_crc; + mav_array_assign_char(packet->uri, uri, 100); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_METADATA, (const char *)packet, MAVLINK_MSG_ID_COMPONENT_METADATA_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_METADATA_LEN, MAVLINK_MSG_ID_COMPONENT_METADATA_CRC); +#endif +} +#endif + +#endif + +// MESSAGE COMPONENT_METADATA UNPACKING + + +/** + * @brief Get field time_boot_ms from component_metadata message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_component_metadata_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field file_crc from component_metadata message + * + * @return CRC32 of the general metadata file. + */ +static inline uint32_t mavlink_msg_component_metadata_get_file_crc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field uri from component_metadata message + * + * @return MAVLink FTP URI for the general metadata file (COMP_METADATA_TYPE_GENERAL), which may be compressed with xz. The file contains general component metadata, and may contain URI links for additional metadata (see COMP_METADATA_TYPE). The information is static from boot, and may be generated at compile time. The string needs to be zero terminated. + */ +static inline uint16_t mavlink_msg_component_metadata_get_uri(const mavlink_message_t* msg, char *uri) +{ + return _MAV_RETURN_char_array(msg, uri, 100, 8); +} + +/** + * @brief Decode a component_metadata message into a struct + * + * @param msg The message to decode + * @param component_metadata C-struct to decode the message contents into + */ +static inline void mavlink_msg_component_metadata_decode(const mavlink_message_t* msg, mavlink_component_metadata_t* component_metadata) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + component_metadata->time_boot_ms = mavlink_msg_component_metadata_get_time_boot_ms(msg); + component_metadata->file_crc = mavlink_msg_component_metadata_get_file_crc(msg); + mavlink_msg_component_metadata_get_uri(msg, component_metadata->uri); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_COMPONENT_METADATA_LEN? msg->len : MAVLINK_MSG_ID_COMPONENT_METADATA_LEN; + memset(component_metadata, 0, MAVLINK_MSG_ID_COMPONENT_METADATA_LEN); + memcpy(component_metadata, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_control_system_state.h b/lib/main/MAVLink/common/mavlink_msg_control_system_state.h index b4381cff4e2..118161a2445 100755 --- a/lib/main/MAVLink/common/mavlink_msg_control_system_state.h +++ b/lib/main/MAVLink/common/mavlink_msg_control_system_state.h @@ -113,6 +113,81 @@ typedef struct __mavlink_control_system_state_t { static inline uint16_t mavlink_msg_control_system_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_usec, float x_acc, float y_acc, float z_acc, float x_vel, float y_vel, float z_vel, float x_pos, float y_pos, float z_pos, float airspeed, const float *vel_variance, const float *pos_variance, const float *q, float roll_rate, float pitch_rate, float yaw_rate) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x_acc); + _mav_put_float(buf, 12, y_acc); + _mav_put_float(buf, 16, z_acc); + _mav_put_float(buf, 20, x_vel); + _mav_put_float(buf, 24, y_vel); + _mav_put_float(buf, 28, z_vel); + _mav_put_float(buf, 32, x_pos); + _mav_put_float(buf, 36, y_pos); + _mav_put_float(buf, 40, z_pos); + _mav_put_float(buf, 44, airspeed); + _mav_put_float(buf, 88, roll_rate); + _mav_put_float(buf, 92, pitch_rate); + _mav_put_float(buf, 96, yaw_rate); + _mav_put_float_array(buf, 48, vel_variance, 3); + _mav_put_float_array(buf, 60, pos_variance, 3); + _mav_put_float_array(buf, 72, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_LEN); +#else + mavlink_control_system_state_t packet; + packet.time_usec = time_usec; + packet.x_acc = x_acc; + packet.y_acc = y_acc; + packet.z_acc = z_acc; + packet.x_vel = x_vel; + packet.y_vel = y_vel; + packet.z_vel = z_vel; + packet.x_pos = x_pos; + packet.y_pos = y_pos; + packet.z_pos = z_pos; + packet.airspeed = airspeed; + packet.roll_rate = roll_rate; + packet.pitch_rate = pitch_rate; + packet.yaw_rate = yaw_rate; + mav_array_assign_float(packet.vel_variance, vel_variance, 3); + mav_array_assign_float(packet.pos_variance, pos_variance, 3); + mav_array_assign_float(packet.q, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_MIN_LEN, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_LEN, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_CRC); +} + +/** + * @brief Pack a control_system_state message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param x_acc [m/s/s] X acceleration in body frame + * @param y_acc [m/s/s] Y acceleration in body frame + * @param z_acc [m/s/s] Z acceleration in body frame + * @param x_vel [m/s] X velocity in body frame + * @param y_vel [m/s] Y velocity in body frame + * @param z_vel [m/s] Z velocity in body frame + * @param x_pos [m] X position in local frame + * @param y_pos [m] Y position in local frame + * @param z_pos [m] Z position in local frame + * @param airspeed [m/s] Airspeed, set to -1 if unknown + * @param vel_variance Variance of body velocity estimate + * @param pos_variance Variance in local position + * @param q The attitude, represented as Quaternion + * @param roll_rate [rad/s] Angular rate in roll axis + * @param pitch_rate [rad/s] Angular rate in pitch axis + * @param yaw_rate [rad/s] Angular rate in yaw axis + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_control_system_state_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, float x_acc, float y_acc, float z_acc, float x_vel, float y_vel, float z_vel, float x_pos, float y_pos, float z_pos, float airspeed, const float *vel_variance, const float *pos_variance, const float *q, float roll_rate, float pitch_rate, float yaw_rate) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_LEN]; _mav_put_uint64_t(buf, 0, time_usec); @@ -156,7 +231,11 @@ static inline uint16_t mavlink_msg_control_system_state_pack(uint8_t system_id, #endif msg->msgid = MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_MIN_LEN, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_LEN, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_MIN_LEN, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_LEN, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_MIN_LEN, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_LEN); +#endif } /** @@ -224,9 +303,9 @@ static inline uint16_t mavlink_msg_control_system_state_pack_chan(uint8_t system packet.roll_rate = roll_rate; packet.pitch_rate = pitch_rate; packet.yaw_rate = yaw_rate; - mav_array_memcpy(packet.vel_variance, vel_variance, sizeof(float)*3); - mav_array_memcpy(packet.pos_variance, pos_variance, sizeof(float)*3); - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.vel_variance, vel_variance, 3); + mav_array_assign_float(packet.pos_variance, pos_variance, 3); + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_LEN); #endif @@ -261,6 +340,20 @@ static inline uint16_t mavlink_msg_control_system_state_encode_chan(uint8_t syst return mavlink_msg_control_system_state_pack_chan(system_id, component_id, chan, msg, control_system_state->time_usec, control_system_state->x_acc, control_system_state->y_acc, control_system_state->z_acc, control_system_state->x_vel, control_system_state->y_vel, control_system_state->z_vel, control_system_state->x_pos, control_system_state->y_pos, control_system_state->z_pos, control_system_state->airspeed, control_system_state->vel_variance, control_system_state->pos_variance, control_system_state->q, control_system_state->roll_rate, control_system_state->pitch_rate, control_system_state->yaw_rate); } +/** + * @brief Encode a control_system_state struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param control_system_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_control_system_state_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_control_system_state_t* control_system_state) +{ + return mavlink_msg_control_system_state_pack_status(system_id, component_id, _status, msg, control_system_state->time_usec, control_system_state->x_acc, control_system_state->y_acc, control_system_state->z_acc, control_system_state->x_vel, control_system_state->y_vel, control_system_state->z_vel, control_system_state->x_pos, control_system_state->y_pos, control_system_state->z_pos, control_system_state->airspeed, control_system_state->vel_variance, control_system_state->pos_variance, control_system_state->q, control_system_state->roll_rate, control_system_state->pitch_rate, control_system_state->yaw_rate); +} + /** * @brief Send a control_system_state message * @param chan MAVLink channel to send the message @@ -323,9 +416,9 @@ static inline void mavlink_msg_control_system_state_send(mavlink_channel_t chan, packet.roll_rate = roll_rate; packet.pitch_rate = pitch_rate; packet.yaw_rate = yaw_rate; - mav_array_memcpy(packet.vel_variance, vel_variance, sizeof(float)*3); - mav_array_memcpy(packet.pos_variance, pos_variance, sizeof(float)*3); - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.vel_variance, vel_variance, 3); + mav_array_assign_float(packet.pos_variance, pos_variance, 3); + mav_array_assign_float(packet.q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE, (const char *)&packet, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_MIN_LEN, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_LEN, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_CRC); #endif } @@ -346,7 +439,7 @@ static inline void mavlink_msg_control_system_state_send_struct(mavlink_channel_ #if MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -390,9 +483,9 @@ static inline void mavlink_msg_control_system_state_send_buf(mavlink_message_t * packet->roll_rate = roll_rate; packet->pitch_rate = pitch_rate; packet->yaw_rate = yaw_rate; - mav_array_memcpy(packet->vel_variance, vel_variance, sizeof(float)*3); - mav_array_memcpy(packet->pos_variance, pos_variance, sizeof(float)*3); - mav_array_memcpy(packet->q, q, sizeof(float)*4); + mav_array_assign_float(packet->vel_variance, vel_variance, 3); + mav_array_assign_float(packet->pos_variance, pos_variance, 3); + mav_array_assign_float(packet->q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE, (const char *)packet, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_MIN_LEN, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_LEN, MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_current_event_sequence.h b/lib/main/MAVLink/common/mavlink_msg_current_event_sequence.h new file mode 100644 index 00000000000..bc3a6157139 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_current_event_sequence.h @@ -0,0 +1,288 @@ +#pragma once +// MESSAGE CURRENT_EVENT_SEQUENCE PACKING + +#define MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE 411 + + +typedef struct __mavlink_current_event_sequence_t { + uint16_t sequence; /*< Sequence number.*/ + uint8_t flags; /*< Flag bitset.*/ +} mavlink_current_event_sequence_t; + +#define MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN 3 +#define MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_MIN_LEN 3 +#define MAVLINK_MSG_ID_411_LEN 3 +#define MAVLINK_MSG_ID_411_MIN_LEN 3 + +#define MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_CRC 106 +#define MAVLINK_MSG_ID_411_CRC 106 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CURRENT_EVENT_SEQUENCE { \ + 411, \ + "CURRENT_EVENT_SEQUENCE", \ + 2, \ + { { "sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_current_event_sequence_t, sequence) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_current_event_sequence_t, flags) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CURRENT_EVENT_SEQUENCE { \ + "CURRENT_EVENT_SEQUENCE", \ + 2, \ + { { "sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_current_event_sequence_t, sequence) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_current_event_sequence_t, flags) }, \ + } \ +} +#endif + +/** + * @brief Pack a current_event_sequence message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param sequence Sequence number. + * @param flags Flag bitset. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_current_event_sequence_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t sequence, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN); +#else + mavlink_current_event_sequence_t packet; + packet.sequence = sequence; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_MIN_LEN, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_CRC); +} + +/** + * @brief Pack a current_event_sequence message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param sequence Sequence number. + * @param flags Flag bitset. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_current_event_sequence_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint16_t sequence, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN); +#else + mavlink_current_event_sequence_t packet; + packet.sequence = sequence; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_MIN_LEN, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_MIN_LEN, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN); +#endif +} + +/** + * @brief Pack a current_event_sequence message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param sequence Sequence number. + * @param flags Flag bitset. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_current_event_sequence_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t sequence,uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN); +#else + mavlink_current_event_sequence_t packet; + packet.sequence = sequence; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_MIN_LEN, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_CRC); +} + +/** + * @brief Encode a current_event_sequence struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param current_event_sequence C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_current_event_sequence_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_current_event_sequence_t* current_event_sequence) +{ + return mavlink_msg_current_event_sequence_pack(system_id, component_id, msg, current_event_sequence->sequence, current_event_sequence->flags); +} + +/** + * @brief Encode a current_event_sequence struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param current_event_sequence C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_current_event_sequence_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_current_event_sequence_t* current_event_sequence) +{ + return mavlink_msg_current_event_sequence_pack_chan(system_id, component_id, chan, msg, current_event_sequence->sequence, current_event_sequence->flags); +} + +/** + * @brief Encode a current_event_sequence struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param current_event_sequence C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_current_event_sequence_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_current_event_sequence_t* current_event_sequence) +{ + return mavlink_msg_current_event_sequence_pack_status(system_id, component_id, _status, msg, current_event_sequence->sequence, current_event_sequence->flags); +} + +/** + * @brief Send a current_event_sequence message + * @param chan MAVLink channel to send the message + * + * @param sequence Sequence number. + * @param flags Flag bitset. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_current_event_sequence_send(mavlink_channel_t chan, uint16_t sequence, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, flags); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE, buf, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_MIN_LEN, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_CRC); +#else + mavlink_current_event_sequence_t packet; + packet.sequence = sequence; + packet.flags = flags; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE, (const char *)&packet, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_MIN_LEN, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_CRC); +#endif +} + +/** + * @brief Send a current_event_sequence message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_current_event_sequence_send_struct(mavlink_channel_t chan, const mavlink_current_event_sequence_t* current_event_sequence) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_current_event_sequence_send(chan, current_event_sequence->sequence, current_event_sequence->flags); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE, (const char *)current_event_sequence, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_MIN_LEN, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_current_event_sequence_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t sequence, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, flags); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE, buf, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_MIN_LEN, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_CRC); +#else + mavlink_current_event_sequence_t *packet = (mavlink_current_event_sequence_t *)msgbuf; + packet->sequence = sequence; + packet->flags = flags; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE, (const char *)packet, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_MIN_LEN, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CURRENT_EVENT_SEQUENCE UNPACKING + + +/** + * @brief Get field sequence from current_event_sequence message + * + * @return Sequence number. + */ +static inline uint16_t mavlink_msg_current_event_sequence_get_sequence(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field flags from current_event_sequence message + * + * @return Flag bitset. + */ +static inline uint8_t mavlink_msg_current_event_sequence_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Decode a current_event_sequence message into a struct + * + * @param msg The message to decode + * @param current_event_sequence C-struct to decode the message contents into + */ +static inline void mavlink_msg_current_event_sequence_decode(const mavlink_message_t* msg, mavlink_current_event_sequence_t* current_event_sequence) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + current_event_sequence->sequence = mavlink_msg_current_event_sequence_get_sequence(msg); + current_event_sequence->flags = mavlink_msg_current_event_sequence_get_flags(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN? msg->len : MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN; + memset(current_event_sequence, 0, MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN); + memcpy(current_event_sequence, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_current_mode.h b/lib/main/MAVLink/common/mavlink_msg_current_mode.h new file mode 100644 index 00000000000..a3dc44e5698 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_current_mode.h @@ -0,0 +1,316 @@ +#pragma once +// MESSAGE CURRENT_MODE PACKING + +#define MAVLINK_MSG_ID_CURRENT_MODE 436 + + +typedef struct __mavlink_current_mode_t { + uint32_t custom_mode; /*< A bitfield for use for autopilot-specific flags*/ + uint32_t intended_custom_mode; /*< The custom_mode of the mode that was last commanded by the user (for example, with MAV_CMD_DO_SET_STANDARD_MODE, MAV_CMD_DO_SET_MODE or via RC). This should usually be the same as custom_mode. It will be different if the vehicle is unable to enter the intended mode, or has left that mode due to a failsafe condition. 0 indicates the intended custom mode is unknown/not supplied*/ + uint8_t standard_mode; /*< Standard mode.*/ +} mavlink_current_mode_t; + +#define MAVLINK_MSG_ID_CURRENT_MODE_LEN 9 +#define MAVLINK_MSG_ID_CURRENT_MODE_MIN_LEN 9 +#define MAVLINK_MSG_ID_436_LEN 9 +#define MAVLINK_MSG_ID_436_MIN_LEN 9 + +#define MAVLINK_MSG_ID_CURRENT_MODE_CRC 193 +#define MAVLINK_MSG_ID_436_CRC 193 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CURRENT_MODE { \ + 436, \ + "CURRENT_MODE", \ + 3, \ + { { "standard_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_current_mode_t, standard_mode) }, \ + { "custom_mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_current_mode_t, custom_mode) }, \ + { "intended_custom_mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_current_mode_t, intended_custom_mode) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CURRENT_MODE { \ + "CURRENT_MODE", \ + 3, \ + { { "standard_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_current_mode_t, standard_mode) }, \ + { "custom_mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_current_mode_t, custom_mode) }, \ + { "intended_custom_mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_current_mode_t, intended_custom_mode) }, \ + } \ +} +#endif + +/** + * @brief Pack a current_mode message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param standard_mode Standard mode. + * @param custom_mode A bitfield for use for autopilot-specific flags + * @param intended_custom_mode The custom_mode of the mode that was last commanded by the user (for example, with MAV_CMD_DO_SET_STANDARD_MODE, MAV_CMD_DO_SET_MODE or via RC). This should usually be the same as custom_mode. It will be different if the vehicle is unable to enter the intended mode, or has left that mode due to a failsafe condition. 0 indicates the intended custom mode is unknown/not supplied + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_current_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t standard_mode, uint32_t custom_mode, uint32_t intended_custom_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CURRENT_MODE_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint32_t(buf, 4, intended_custom_mode); + _mav_put_uint8_t(buf, 8, standard_mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CURRENT_MODE_LEN); +#else + mavlink_current_mode_t packet; + packet.custom_mode = custom_mode; + packet.intended_custom_mode = intended_custom_mode; + packet.standard_mode = standard_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CURRENT_MODE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CURRENT_MODE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CURRENT_MODE_MIN_LEN, MAVLINK_MSG_ID_CURRENT_MODE_LEN, MAVLINK_MSG_ID_CURRENT_MODE_CRC); +} + +/** + * @brief Pack a current_mode message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param standard_mode Standard mode. + * @param custom_mode A bitfield for use for autopilot-specific flags + * @param intended_custom_mode The custom_mode of the mode that was last commanded by the user (for example, with MAV_CMD_DO_SET_STANDARD_MODE, MAV_CMD_DO_SET_MODE or via RC). This should usually be the same as custom_mode. It will be different if the vehicle is unable to enter the intended mode, or has left that mode due to a failsafe condition. 0 indicates the intended custom mode is unknown/not supplied + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_current_mode_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t standard_mode, uint32_t custom_mode, uint32_t intended_custom_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CURRENT_MODE_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint32_t(buf, 4, intended_custom_mode); + _mav_put_uint8_t(buf, 8, standard_mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CURRENT_MODE_LEN); +#else + mavlink_current_mode_t packet; + packet.custom_mode = custom_mode; + packet.intended_custom_mode = intended_custom_mode; + packet.standard_mode = standard_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CURRENT_MODE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CURRENT_MODE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CURRENT_MODE_MIN_LEN, MAVLINK_MSG_ID_CURRENT_MODE_LEN, MAVLINK_MSG_ID_CURRENT_MODE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_CURRENT_MODE_MIN_LEN, MAVLINK_MSG_ID_CURRENT_MODE_LEN); +#endif +} + +/** + * @brief Pack a current_mode message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param standard_mode Standard mode. + * @param custom_mode A bitfield for use for autopilot-specific flags + * @param intended_custom_mode The custom_mode of the mode that was last commanded by the user (for example, with MAV_CMD_DO_SET_STANDARD_MODE, MAV_CMD_DO_SET_MODE or via RC). This should usually be the same as custom_mode. It will be different if the vehicle is unable to enter the intended mode, or has left that mode due to a failsafe condition. 0 indicates the intended custom mode is unknown/not supplied + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_current_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t standard_mode,uint32_t custom_mode,uint32_t intended_custom_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CURRENT_MODE_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint32_t(buf, 4, intended_custom_mode); + _mav_put_uint8_t(buf, 8, standard_mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CURRENT_MODE_LEN); +#else + mavlink_current_mode_t packet; + packet.custom_mode = custom_mode; + packet.intended_custom_mode = intended_custom_mode; + packet.standard_mode = standard_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CURRENT_MODE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CURRENT_MODE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CURRENT_MODE_MIN_LEN, MAVLINK_MSG_ID_CURRENT_MODE_LEN, MAVLINK_MSG_ID_CURRENT_MODE_CRC); +} + +/** + * @brief Encode a current_mode struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param current_mode C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_current_mode_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_current_mode_t* current_mode) +{ + return mavlink_msg_current_mode_pack(system_id, component_id, msg, current_mode->standard_mode, current_mode->custom_mode, current_mode->intended_custom_mode); +} + +/** + * @brief Encode a current_mode struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param current_mode C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_current_mode_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_current_mode_t* current_mode) +{ + return mavlink_msg_current_mode_pack_chan(system_id, component_id, chan, msg, current_mode->standard_mode, current_mode->custom_mode, current_mode->intended_custom_mode); +} + +/** + * @brief Encode a current_mode struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param current_mode C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_current_mode_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_current_mode_t* current_mode) +{ + return mavlink_msg_current_mode_pack_status(system_id, component_id, _status, msg, current_mode->standard_mode, current_mode->custom_mode, current_mode->intended_custom_mode); +} + +/** + * @brief Send a current_mode message + * @param chan MAVLink channel to send the message + * + * @param standard_mode Standard mode. + * @param custom_mode A bitfield for use for autopilot-specific flags + * @param intended_custom_mode The custom_mode of the mode that was last commanded by the user (for example, with MAV_CMD_DO_SET_STANDARD_MODE, MAV_CMD_DO_SET_MODE or via RC). This should usually be the same as custom_mode. It will be different if the vehicle is unable to enter the intended mode, or has left that mode due to a failsafe condition. 0 indicates the intended custom mode is unknown/not supplied + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_current_mode_send(mavlink_channel_t chan, uint8_t standard_mode, uint32_t custom_mode, uint32_t intended_custom_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CURRENT_MODE_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint32_t(buf, 4, intended_custom_mode); + _mav_put_uint8_t(buf, 8, standard_mode); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CURRENT_MODE, buf, MAVLINK_MSG_ID_CURRENT_MODE_MIN_LEN, MAVLINK_MSG_ID_CURRENT_MODE_LEN, MAVLINK_MSG_ID_CURRENT_MODE_CRC); +#else + mavlink_current_mode_t packet; + packet.custom_mode = custom_mode; + packet.intended_custom_mode = intended_custom_mode; + packet.standard_mode = standard_mode; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CURRENT_MODE, (const char *)&packet, MAVLINK_MSG_ID_CURRENT_MODE_MIN_LEN, MAVLINK_MSG_ID_CURRENT_MODE_LEN, MAVLINK_MSG_ID_CURRENT_MODE_CRC); +#endif +} + +/** + * @brief Send a current_mode message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_current_mode_send_struct(mavlink_channel_t chan, const mavlink_current_mode_t* current_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_current_mode_send(chan, current_mode->standard_mode, current_mode->custom_mode, current_mode->intended_custom_mode); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CURRENT_MODE, (const char *)current_mode, MAVLINK_MSG_ID_CURRENT_MODE_MIN_LEN, MAVLINK_MSG_ID_CURRENT_MODE_LEN, MAVLINK_MSG_ID_CURRENT_MODE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CURRENT_MODE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_current_mode_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t standard_mode, uint32_t custom_mode, uint32_t intended_custom_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint32_t(buf, 4, intended_custom_mode); + _mav_put_uint8_t(buf, 8, standard_mode); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CURRENT_MODE, buf, MAVLINK_MSG_ID_CURRENT_MODE_MIN_LEN, MAVLINK_MSG_ID_CURRENT_MODE_LEN, MAVLINK_MSG_ID_CURRENT_MODE_CRC); +#else + mavlink_current_mode_t *packet = (mavlink_current_mode_t *)msgbuf; + packet->custom_mode = custom_mode; + packet->intended_custom_mode = intended_custom_mode; + packet->standard_mode = standard_mode; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CURRENT_MODE, (const char *)packet, MAVLINK_MSG_ID_CURRENT_MODE_MIN_LEN, MAVLINK_MSG_ID_CURRENT_MODE_LEN, MAVLINK_MSG_ID_CURRENT_MODE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CURRENT_MODE UNPACKING + + +/** + * @brief Get field standard_mode from current_mode message + * + * @return Standard mode. + */ +static inline uint8_t mavlink_msg_current_mode_get_standard_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field custom_mode from current_mode message + * + * @return A bitfield for use for autopilot-specific flags + */ +static inline uint32_t mavlink_msg_current_mode_get_custom_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field intended_custom_mode from current_mode message + * + * @return The custom_mode of the mode that was last commanded by the user (for example, with MAV_CMD_DO_SET_STANDARD_MODE, MAV_CMD_DO_SET_MODE or via RC). This should usually be the same as custom_mode. It will be different if the vehicle is unable to enter the intended mode, or has left that mode due to a failsafe condition. 0 indicates the intended custom mode is unknown/not supplied + */ +static inline uint32_t mavlink_msg_current_mode_get_intended_custom_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Decode a current_mode message into a struct + * + * @param msg The message to decode + * @param current_mode C-struct to decode the message contents into + */ +static inline void mavlink_msg_current_mode_decode(const mavlink_message_t* msg, mavlink_current_mode_t* current_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + current_mode->custom_mode = mavlink_msg_current_mode_get_custom_mode(msg); + current_mode->intended_custom_mode = mavlink_msg_current_mode_get_intended_custom_mode(msg); + current_mode->standard_mode = mavlink_msg_current_mode_get_standard_mode(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CURRENT_MODE_LEN? msg->len : MAVLINK_MSG_ID_CURRENT_MODE_LEN; + memset(current_mode, 0, MAVLINK_MSG_ID_CURRENT_MODE_LEN); + memcpy(current_mode, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_data_stream.h b/lib/main/MAVLink/common/mavlink_msg_data_stream.h index 36b688cbeeb..9436e645cd2 100755 --- a/lib/main/MAVLink/common/mavlink_msg_data_stream.h +++ b/lib/main/MAVLink/common/mavlink_msg_data_stream.h @@ -75,6 +75,45 @@ static inline uint16_t mavlink_msg_data_stream_pack(uint8_t system_id, uint8_t c return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_STREAM_MIN_LEN, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); } +/** + * @brief Pack a data_stream message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param stream_id The ID of the requested data stream + * @param message_rate [Hz] The message rate + * @param on_off 1 stream is enabled, 0 stream is stopped. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_data_stream_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t stream_id, uint16_t message_rate, uint8_t on_off) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA_STREAM_LEN]; + _mav_put_uint16_t(buf, 0, message_rate); + _mav_put_uint8_t(buf, 2, stream_id); + _mav_put_uint8_t(buf, 3, on_off); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#else + mavlink_data_stream_t packet; + packet.message_rate = message_rate; + packet.stream_id = stream_id; + packet.on_off = on_off; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DATA_STREAM; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_DATA_STREAM_MIN_LEN, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_DATA_STREAM_MIN_LEN, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#endif +} + /** * @brief Pack a data_stream message on a channel * @param system_id ID of this system @@ -137,6 +176,20 @@ static inline uint16_t mavlink_msg_data_stream_encode_chan(uint8_t system_id, ui return mavlink_msg_data_stream_pack_chan(system_id, component_id, chan, msg, data_stream->stream_id, data_stream->message_rate, data_stream->on_off); } +/** + * @brief Encode a data_stream struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param data_stream C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data_stream_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_data_stream_t* data_stream) +{ + return mavlink_msg_data_stream_pack_status(system_id, component_id, _status, msg, data_stream->stream_id, data_stream->message_rate, data_stream->on_off); +} + /** * @brief Send a data_stream message * @param chan MAVLink channel to send the message @@ -182,7 +235,7 @@ static inline void mavlink_msg_data_stream_send_struct(mavlink_channel_t chan, c #if MAVLINK_MSG_ID_DATA_STREAM_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_data_transmission_handshake.h b/lib/main/MAVLink/common/mavlink_msg_data_transmission_handshake.h index 45fe60680a1..aa6d551ee47 100755 --- a/lib/main/MAVLink/common/mavlink_msg_data_transmission_handshake.h +++ b/lib/main/MAVLink/common/mavlink_msg_data_transmission_handshake.h @@ -99,6 +99,57 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t syst return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_MIN_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); } +/** + * @brief Pack a data_transmission_handshake message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param type Type of requested/acknowledged data. + * @param size [bytes] total data size (set on ACK only). + * @param width Width of a matrix or image. + * @param height Height of a matrix or image. + * @param packets Number of packets being sent (set on ACK only). + * @param payload [bytes] Payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only). + * @param jpg_quality [%] JPEG quality. Values: [1-100]. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_data_transmission_handshake_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint16_t packets, uint8_t payload, uint8_t jpg_quality) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN]; + _mav_put_uint32_t(buf, 0, size); + _mav_put_uint16_t(buf, 4, width); + _mav_put_uint16_t(buf, 6, height); + _mav_put_uint16_t(buf, 8, packets); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, payload); + _mav_put_uint8_t(buf, 12, jpg_quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#else + mavlink_data_transmission_handshake_t packet; + packet.size = size; + packet.width = width; + packet.height = height; + packet.packets = packets; + packet.type = type; + packet.payload = payload; + packet.jpg_quality = jpg_quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_MIN_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_MIN_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#endif +} + /** * @brief Pack a data_transmission_handshake message on a channel * @param system_id ID of this system @@ -173,6 +224,20 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_encode_chan(uint8 return mavlink_msg_data_transmission_handshake_pack_chan(system_id, component_id, chan, msg, data_transmission_handshake->type, data_transmission_handshake->size, data_transmission_handshake->width, data_transmission_handshake->height, data_transmission_handshake->packets, data_transmission_handshake->payload, data_transmission_handshake->jpg_quality); } +/** + * @brief Encode a data_transmission_handshake struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param data_transmission_handshake C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data_transmission_handshake_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_data_transmission_handshake_t* data_transmission_handshake) +{ + return mavlink_msg_data_transmission_handshake_pack_status(system_id, component_id, _status, msg, data_transmission_handshake->type, data_transmission_handshake->size, data_transmission_handshake->width, data_transmission_handshake->height, data_transmission_handshake->packets, data_transmission_handshake->payload, data_transmission_handshake->jpg_quality); +} + /** * @brief Send a data_transmission_handshake message * @param chan MAVLink channel to send the message @@ -230,7 +295,7 @@ static inline void mavlink_msg_data_transmission_handshake_send_struct(mavlink_c #if MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_debug.h b/lib/main/MAVLink/common/mavlink_msg_debug.h index 85814e366cd..ffb11509a59 100755 --- a/lib/main/MAVLink/common/mavlink_msg_debug.h +++ b/lib/main/MAVLink/common/mavlink_msg_debug.h @@ -75,6 +75,45 @@ static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t compone return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_MIN_LEN, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); } +/** + * @brief Pack a debug message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param ind index of debug variable + * @param value DEBUG value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_debug_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t ind, float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEBUG_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_uint8_t(buf, 8, ind); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_LEN); +#else + mavlink_debug_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + packet.ind = ind; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEBUG; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_DEBUG_MIN_LEN, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_DEBUG_MIN_LEN, MAVLINK_MSG_ID_DEBUG_LEN); +#endif +} + /** * @brief Pack a debug message on a channel * @param system_id ID of this system @@ -137,6 +176,20 @@ static inline uint16_t mavlink_msg_debug_encode_chan(uint8_t system_id, uint8_t return mavlink_msg_debug_pack_chan(system_id, component_id, chan, msg, debug->time_boot_ms, debug->ind, debug->value); } +/** + * @brief Encode a debug struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param debug C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_debug_t* debug) +{ + return mavlink_msg_debug_pack_status(system_id, component_id, _status, msg, debug->time_boot_ms, debug->ind, debug->value); +} + /** * @brief Send a debug message * @param chan MAVLink channel to send the message @@ -182,7 +235,7 @@ static inline void mavlink_msg_debug_send_struct(mavlink_channel_t chan, const m #if MAVLINK_MSG_ID_DEBUG_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_debug_float_array.h b/lib/main/MAVLink/common/mavlink_msg_debug_float_array.h index c3a8b31fb8a..c17a0de627c 100644 --- a/lib/main/MAVLink/common/mavlink_msg_debug_float_array.h +++ b/lib/main/MAVLink/common/mavlink_msg_debug_float_array.h @@ -60,6 +60,42 @@ typedef struct __mavlink_debug_float_array_t { static inline uint16_t mavlink_msg_debug_float_array_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_usec, const char *name, uint16_t array_id, const float *data) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, array_id); + _mav_put_char_array(buf, 10, name, 10); + _mav_put_float_array(buf, 20, data, 58); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN); +#else + mavlink_debug_float_array_t packet; + packet.time_usec = time_usec; + packet.array_id = array_id; + mav_array_assign_char(packet.name, name, 10); + mav_array_assign_float(packet.data, data, 58); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); +} + +/** + * @brief Pack a debug_float_array message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param name Name, for human-friendly display in a Ground Control Station + * @param array_id Unique ID used to discriminate between arrays + * @param data data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_debug_float_array_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, const char *name, uint16_t array_id, const float *data) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN]; _mav_put_uint64_t(buf, 0, time_usec); @@ -77,7 +113,11 @@ static inline uint16_t mavlink_msg_debug_float_array_pack(uint8_t system_id, uin #endif msg->msgid = MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN); +#endif } /** @@ -107,8 +147,8 @@ static inline uint16_t mavlink_msg_debug_float_array_pack_chan(uint8_t system_id mavlink_debug_float_array_t packet; packet.time_usec = time_usec; packet.array_id = array_id; - mav_array_memcpy(packet.name, name, sizeof(char)*10); - mav_array_memcpy(packet.data, data, sizeof(float)*58); + mav_array_assign_char(packet.name, name, 10); + mav_array_assign_float(packet.data, data, 58); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN); #endif @@ -143,6 +183,20 @@ static inline uint16_t mavlink_msg_debug_float_array_encode_chan(uint8_t system_ return mavlink_msg_debug_float_array_pack_chan(system_id, component_id, chan, msg, debug_float_array->time_usec, debug_float_array->name, debug_float_array->array_id, debug_float_array->data); } +/** + * @brief Encode a debug_float_array struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param debug_float_array C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_float_array_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_debug_float_array_t* debug_float_array) +{ + return mavlink_msg_debug_float_array_pack_status(system_id, component_id, _status, msg, debug_float_array->time_usec, debug_float_array->name, debug_float_array->array_id, debug_float_array->data); +} + /** * @brief Send a debug_float_array message * @param chan MAVLink channel to send the message @@ -167,8 +221,8 @@ static inline void mavlink_msg_debug_float_array_send(mavlink_channel_t chan, ui mavlink_debug_float_array_t packet; packet.time_usec = time_usec; packet.array_id = array_id; - mav_array_memcpy(packet.name, name, sizeof(char)*10); - mav_array_memcpy(packet.data, data, sizeof(float)*58); + mav_array_assign_char(packet.name, name, 10); + mav_array_assign_float(packet.data, data, 58); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY, (const char *)&packet, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); #endif } @@ -189,7 +243,7 @@ static inline void mavlink_msg_debug_float_array_send_struct(mavlink_channel_t c #if MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -208,8 +262,8 @@ static inline void mavlink_msg_debug_float_array_send_buf(mavlink_message_t *msg mavlink_debug_float_array_t *packet = (mavlink_debug_float_array_t *)msgbuf; packet->time_usec = time_usec; packet->array_id = array_id; - mav_array_memcpy(packet->name, name, sizeof(char)*10); - mav_array_memcpy(packet->data, data, sizeof(float)*58); + mav_array_assign_char(packet->name, name, 10); + mav_array_assign_float(packet->data, data, 58); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY, (const char *)packet, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_debug_vect.h b/lib/main/MAVLink/common/mavlink_msg_debug_vect.h index 1cb8d77ab57..3bd4a5102d0 100755 --- a/lib/main/MAVLink/common/mavlink_msg_debug_vect.h +++ b/lib/main/MAVLink/common/mavlink_msg_debug_vect.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t co packet.x = x; packet.y = y; packet.z = z; - mav_array_memcpy(packet.name, name, sizeof(char)*10); + mav_array_assign_char(packet.name, name, 10); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN); #endif @@ -85,6 +85,49 @@ static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t co return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_VECT_MIN_LEN, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); } +/** + * @brief Pack a debug_vect message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param name Name + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param x x + * @param y y + * @param z z + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_debug_vect_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + const char *name, uint64_t time_usec, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEBUG_VECT_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_char_array(buf, 20, name, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#else + mavlink_debug_vect_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_DEBUG_VECT_MIN_LEN, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_DEBUG_VECT_MIN_LEN, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#endif +} + /** * @brief Pack a debug_vect message on a channel * @param system_id ID of this system @@ -116,7 +159,7 @@ static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8 packet.x = x; packet.y = y; packet.z = z; - mav_array_memcpy(packet.name, name, sizeof(char)*10); + mav_array_assign_char(packet.name, name, 10); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN); #endif @@ -151,6 +194,20 @@ static inline uint16_t mavlink_msg_debug_vect_encode_chan(uint8_t system_id, uin return mavlink_msg_debug_vect_pack_chan(system_id, component_id, chan, msg, debug_vect->name, debug_vect->time_usec, debug_vect->x, debug_vect->y, debug_vect->z); } +/** + * @brief Encode a debug_vect struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param debug_vect C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_vect_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_debug_vect_t* debug_vect) +{ + return mavlink_msg_debug_vect_pack_status(system_id, component_id, _status, msg, debug_vect->name, debug_vect->time_usec, debug_vect->x, debug_vect->y, debug_vect->z); +} + /** * @brief Send a debug_vect message * @param chan MAVLink channel to send the message @@ -179,7 +236,7 @@ static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const cha packet.x = x; packet.y = y; packet.z = z; - mav_array_memcpy(packet.name, name, sizeof(char)*10); + mav_array_assign_char(packet.name, name, 10); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)&packet, MAVLINK_MSG_ID_DEBUG_VECT_MIN_LEN, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); #endif } @@ -200,7 +257,7 @@ static inline void mavlink_msg_debug_vect_send_struct(mavlink_channel_t chan, co #if MAVLINK_MSG_ID_DEBUG_VECT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -222,7 +279,7 @@ static inline void mavlink_msg_debug_vect_send_buf(mavlink_message_t *msgbuf, ma packet->x = x; packet->y = y; packet->z = z; - mav_array_memcpy(packet->name, name, sizeof(char)*10); + mav_array_assign_char(packet->name, name, 10); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)packet, MAVLINK_MSG_ID_DEBUG_VECT_MIN_LEN, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_distance_sensor.h b/lib/main/MAVLink/common/mavlink_msg_distance_sensor.h index 7e4e8504086..adf30f78a9d 100755 --- a/lib/main/MAVLink/common/mavlink_msg_distance_sensor.h +++ b/lib/main/MAVLink/common/mavlink_msg_distance_sensor.h @@ -12,7 +12,7 @@ typedef struct __mavlink_distance_sensor_t { uint8_t type; /*< Type of distance sensor.*/ uint8_t id; /*< Onboard ID of the sensor*/ uint8_t orientation; /*< Direction the sensor faces. downward-facing: ROTATION_PITCH_270, upward-facing: ROTATION_PITCH_90, backward-facing: ROTATION_PITCH_180, forward-facing: ROTATION_NONE, left-facing: ROTATION_YAW_90, right-facing: ROTATION_YAW_270*/ - uint8_t covariance; /*< [cm^2] Measurement variance. Max standard deviation is 6cm. 255 if unknown.*/ + uint8_t covariance; /*< [cm^2] Measurement variance. Max standard deviation is 6cm. UINT8_MAX if unknown.*/ float horizontal_fov; /*< [rad] Horizontal Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0.*/ float vertical_fov; /*< [rad] Vertical Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0.*/ float quaternion[4]; /*< Quaternion of the sensor orientation in vehicle body frame (w, x, y, z order, zero-rotation is 1, 0, 0, 0). Zero-rotation is along the vehicle body x-axis. This field is required if the orientation is set to MAV_SENSOR_ROTATION_CUSTOM. Set it to 0 if invalid."*/ @@ -81,7 +81,7 @@ typedef struct __mavlink_distance_sensor_t { * @param type Type of distance sensor. * @param id Onboard ID of the sensor * @param orientation Direction the sensor faces. downward-facing: ROTATION_PITCH_270, upward-facing: ROTATION_PITCH_90, backward-facing: ROTATION_PITCH_180, forward-facing: ROTATION_NONE, left-facing: ROTATION_YAW_90, right-facing: ROTATION_YAW_270 - * @param covariance [cm^2] Measurement variance. Max standard deviation is 6cm. 255 if unknown. + * @param covariance [cm^2] Measurement variance. Max standard deviation is 6cm. UINT8_MAX if unknown. * @param horizontal_fov [rad] Horizontal Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. * @param vertical_fov [rad] Vertical Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. * @param quaternion Quaternion of the sensor orientation in vehicle body frame (w, x, y, z order, zero-rotation is 1, 0, 0, 0). Zero-rotation is along the vehicle body x-axis. This field is required if the orientation is set to MAV_SENSOR_ROTATION_CUSTOM. Set it to 0 if invalid." @@ -119,7 +119,7 @@ static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8 packet.horizontal_fov = horizontal_fov; packet.vertical_fov = vertical_fov; packet.signal_quality = signal_quality; - mav_array_memcpy(packet.quaternion, quaternion, sizeof(float)*4); + mav_array_assign_float(packet.quaternion, quaternion, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); #endif @@ -127,6 +127,70 @@ static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DISTANCE_SENSOR_MIN_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); } +/** + * @brief Pack a distance_sensor message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param min_distance [cm] Minimum distance the sensor can measure + * @param max_distance [cm] Maximum distance the sensor can measure + * @param current_distance [cm] Current distance reading + * @param type Type of distance sensor. + * @param id Onboard ID of the sensor + * @param orientation Direction the sensor faces. downward-facing: ROTATION_PITCH_270, upward-facing: ROTATION_PITCH_90, backward-facing: ROTATION_PITCH_180, forward-facing: ROTATION_NONE, left-facing: ROTATION_YAW_90, right-facing: ROTATION_YAW_270 + * @param covariance [cm^2] Measurement variance. Max standard deviation is 6cm. UINT8_MAX if unknown. + * @param horizontal_fov [rad] Horizontal Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + * @param vertical_fov [rad] Vertical Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + * @param quaternion Quaternion of the sensor orientation in vehicle body frame (w, x, y, z order, zero-rotation is 1, 0, 0, 0). Zero-rotation is along the vehicle body x-axis. This field is required if the orientation is set to MAV_SENSOR_ROTATION_CUSTOM. Set it to 0 if invalid." + * @param signal_quality [%] Signal quality of the sensor. Specific to each sensor type, representing the relation of the signal strength with the target reflectivity, distance, size or aspect, but normalised as a percentage. 0 = unknown/unset signal quality, 1 = invalid signal, 100 = perfect signal. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_distance_sensor_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance, float horizontal_fov, float vertical_fov, const float *quaternion, uint8_t signal_quality) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, min_distance); + _mav_put_uint16_t(buf, 6, max_distance); + _mav_put_uint16_t(buf, 8, current_distance); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, id); + _mav_put_uint8_t(buf, 12, orientation); + _mav_put_uint8_t(buf, 13, covariance); + _mav_put_float(buf, 14, horizontal_fov); + _mav_put_float(buf, 18, vertical_fov); + _mav_put_uint8_t(buf, 38, signal_quality); + _mav_put_float_array(buf, 22, quaternion, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#else + mavlink_distance_sensor_t packet; + packet.time_boot_ms = time_boot_ms; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.current_distance = current_distance; + packet.type = type; + packet.id = id; + packet.orientation = orientation; + packet.covariance = covariance; + packet.horizontal_fov = horizontal_fov; + packet.vertical_fov = vertical_fov; + packet.signal_quality = signal_quality; + mav_array_memcpy(packet.quaternion, quaternion, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DISTANCE_SENSOR; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_DISTANCE_SENSOR_MIN_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_DISTANCE_SENSOR_MIN_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +} + /** * @brief Pack a distance_sensor message on a channel * @param system_id ID of this system @@ -140,7 +204,7 @@ static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8 * @param type Type of distance sensor. * @param id Onboard ID of the sensor * @param orientation Direction the sensor faces. downward-facing: ROTATION_PITCH_270, upward-facing: ROTATION_PITCH_90, backward-facing: ROTATION_PITCH_180, forward-facing: ROTATION_NONE, left-facing: ROTATION_YAW_90, right-facing: ROTATION_YAW_270 - * @param covariance [cm^2] Measurement variance. Max standard deviation is 6cm. 255 if unknown. + * @param covariance [cm^2] Measurement variance. Max standard deviation is 6cm. UINT8_MAX if unknown. * @param horizontal_fov [rad] Horizontal Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. * @param vertical_fov [rad] Vertical Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. * @param quaternion Quaternion of the sensor orientation in vehicle body frame (w, x, y, z order, zero-rotation is 1, 0, 0, 0). Zero-rotation is along the vehicle body x-axis. This field is required if the orientation is set to MAV_SENSOR_ROTATION_CUSTOM. Set it to 0 if invalid." @@ -179,7 +243,7 @@ static inline uint16_t mavlink_msg_distance_sensor_pack_chan(uint8_t system_id, packet.horizontal_fov = horizontal_fov; packet.vertical_fov = vertical_fov; packet.signal_quality = signal_quality; - mav_array_memcpy(packet.quaternion, quaternion, sizeof(float)*4); + mav_array_assign_float(packet.quaternion, quaternion, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); #endif @@ -214,6 +278,20 @@ static inline uint16_t mavlink_msg_distance_sensor_encode_chan(uint8_t system_id return mavlink_msg_distance_sensor_pack_chan(system_id, component_id, chan, msg, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance, distance_sensor->horizontal_fov, distance_sensor->vertical_fov, distance_sensor->quaternion, distance_sensor->signal_quality); } +/** + * @brief Encode a distance_sensor struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param distance_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_distance_sensor_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor) +{ + return mavlink_msg_distance_sensor_pack_status(system_id, component_id, _status, msg, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance, distance_sensor->horizontal_fov, distance_sensor->vertical_fov, distance_sensor->quaternion, distance_sensor->signal_quality); +} + /** * @brief Send a distance_sensor message * @param chan MAVLink channel to send the message @@ -225,7 +303,7 @@ static inline uint16_t mavlink_msg_distance_sensor_encode_chan(uint8_t system_id * @param type Type of distance sensor. * @param id Onboard ID of the sensor * @param orientation Direction the sensor faces. downward-facing: ROTATION_PITCH_270, upward-facing: ROTATION_PITCH_90, backward-facing: ROTATION_PITCH_180, forward-facing: ROTATION_NONE, left-facing: ROTATION_YAW_90, right-facing: ROTATION_YAW_270 - * @param covariance [cm^2] Measurement variance. Max standard deviation is 6cm. 255 if unknown. + * @param covariance [cm^2] Measurement variance. Max standard deviation is 6cm. UINT8_MAX if unknown. * @param horizontal_fov [rad] Horizontal Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. * @param vertical_fov [rad] Vertical Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. * @param quaternion Quaternion of the sensor orientation in vehicle body frame (w, x, y, z order, zero-rotation is 1, 0, 0, 0). Zero-rotation is along the vehicle body x-axis. This field is required if the orientation is set to MAV_SENSOR_ROTATION_CUSTOM. Set it to 0 if invalid." @@ -263,7 +341,7 @@ static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint packet.horizontal_fov = horizontal_fov; packet.vertical_fov = vertical_fov; packet.signal_quality = signal_quality; - mav_array_memcpy(packet.quaternion, quaternion, sizeof(float)*4); + mav_array_assign_float(packet.quaternion, quaternion, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_MIN_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); #endif } @@ -284,7 +362,7 @@ static inline void mavlink_msg_distance_sensor_send_struct(mavlink_channel_t cha #if MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -320,7 +398,7 @@ static inline void mavlink_msg_distance_sensor_send_buf(mavlink_message_t *msgbu packet->horizontal_fov = horizontal_fov; packet->vertical_fov = vertical_fov; packet->signal_quality = signal_quality; - mav_array_memcpy(packet->quaternion, quaternion, sizeof(float)*4); + mav_array_assign_float(packet->quaternion, quaternion, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_MIN_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); #endif } @@ -404,7 +482,7 @@ static inline uint8_t mavlink_msg_distance_sensor_get_orientation(const mavlink_ /** * @brief Get field covariance from distance_sensor message * - * @return [cm^2] Measurement variance. Max standard deviation is 6cm. 255 if unknown. + * @return [cm^2] Measurement variance. Max standard deviation is 6cm. UINT8_MAX if unknown. */ static inline uint8_t mavlink_msg_distance_sensor_get_covariance(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_efi_status.h b/lib/main/MAVLink/common/mavlink_msg_efi_status.h index d6310240446..70d3f5ce25f 100644 --- a/lib/main/MAVLink/common/mavlink_msg_efi_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_efi_status.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_EFI_STATUS 225 - +MAVPACKED( typedef struct __mavlink_efi_status_t { float ecu_index; /*< ECU index*/ float rpm; /*< RPM*/ @@ -22,11 +22,13 @@ typedef struct __mavlink_efi_status_t { float throttle_out; /*< [%] Output throttle*/ float pt_compensation; /*< Pressure/temperature compensation*/ uint8_t health; /*< EFI health status*/ -} mavlink_efi_status_t; + float ignition_voltage; /*< [V] Supply voltage to EFI sparking system. Zero in this value means "unknown", so if the supply voltage really is zero volts use 0.0001 instead.*/ + float fuel_pressure; /*< [kPa] Fuel pressure. Zero in this value means "unknown", so if the fuel pressure really is zero kPa use 0.0001 instead.*/ +}) mavlink_efi_status_t; -#define MAVLINK_MSG_ID_EFI_STATUS_LEN 65 +#define MAVLINK_MSG_ID_EFI_STATUS_LEN 73 #define MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN 65 -#define MAVLINK_MSG_ID_225_LEN 65 +#define MAVLINK_MSG_ID_225_LEN 73 #define MAVLINK_MSG_ID_225_MIN_LEN 65 #define MAVLINK_MSG_ID_EFI_STATUS_CRC 208 @@ -38,7 +40,7 @@ typedef struct __mavlink_efi_status_t { #define MAVLINK_MESSAGE_INFO_EFI_STATUS { \ 225, \ "EFI_STATUS", \ - 17, \ + 19, \ { { "health", NULL, MAVLINK_TYPE_UINT8_T, 0, 64, offsetof(mavlink_efi_status_t, health) }, \ { "ecu_index", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_efi_status_t, ecu_index) }, \ { "rpm", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_efi_status_t, rpm) }, \ @@ -56,12 +58,14 @@ typedef struct __mavlink_efi_status_t { { "exhaust_gas_temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_efi_status_t, exhaust_gas_temperature) }, \ { "throttle_out", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_efi_status_t, throttle_out) }, \ { "pt_compensation", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_efi_status_t, pt_compensation) }, \ + { "ignition_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 65, offsetof(mavlink_efi_status_t, ignition_voltage) }, \ + { "fuel_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 69, offsetof(mavlink_efi_status_t, fuel_pressure) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_EFI_STATUS { \ "EFI_STATUS", \ - 17, \ + 19, \ { { "health", NULL, MAVLINK_TYPE_UINT8_T, 0, 64, offsetof(mavlink_efi_status_t, health) }, \ { "ecu_index", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_efi_status_t, ecu_index) }, \ { "rpm", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_efi_status_t, rpm) }, \ @@ -79,6 +83,8 @@ typedef struct __mavlink_efi_status_t { { "exhaust_gas_temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_efi_status_t, exhaust_gas_temperature) }, \ { "throttle_out", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_efi_status_t, throttle_out) }, \ { "pt_compensation", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_efi_status_t, pt_compensation) }, \ + { "ignition_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 65, offsetof(mavlink_efi_status_t, ignition_voltage) }, \ + { "fuel_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 69, offsetof(mavlink_efi_status_t, fuel_pressure) }, \ } \ } #endif @@ -106,10 +112,12 @@ typedef struct __mavlink_efi_status_t { * @param exhaust_gas_temperature [degC] Exhaust gas temperature * @param throttle_out [%] Output throttle * @param pt_compensation Pressure/temperature compensation + * @param ignition_voltage [V] Supply voltage to EFI sparking system. Zero in this value means "unknown", so if the supply voltage really is zero volts use 0.0001 instead. + * @param fuel_pressure [kPa] Fuel pressure. Zero in this value means "unknown", so if the fuel pressure really is zero kPa use 0.0001 instead. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_efi_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t health, float ecu_index, float rpm, float fuel_consumed, float fuel_flow, float engine_load, float throttle_position, float spark_dwell_time, float barometric_pressure, float intake_manifold_pressure, float intake_manifold_temperature, float cylinder_head_temperature, float ignition_timing, float injection_time, float exhaust_gas_temperature, float throttle_out, float pt_compensation) + uint8_t health, float ecu_index, float rpm, float fuel_consumed, float fuel_flow, float engine_load, float throttle_position, float spark_dwell_time, float barometric_pressure, float intake_manifold_pressure, float intake_manifold_temperature, float cylinder_head_temperature, float ignition_timing, float injection_time, float exhaust_gas_temperature, float throttle_out, float pt_compensation, float ignition_voltage, float fuel_pressure) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_EFI_STATUS_LEN]; @@ -130,6 +138,8 @@ static inline uint16_t mavlink_msg_efi_status_pack(uint8_t system_id, uint8_t co _mav_put_float(buf, 56, throttle_out); _mav_put_float(buf, 60, pt_compensation); _mav_put_uint8_t(buf, 64, health); + _mav_put_float(buf, 65, ignition_voltage); + _mav_put_float(buf, 69, fuel_pressure); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_EFI_STATUS_LEN); #else @@ -151,6 +161,8 @@ static inline uint16_t mavlink_msg_efi_status_pack(uint8_t system_id, uint8_t co packet.throttle_out = throttle_out; packet.pt_compensation = pt_compensation; packet.health = health; + packet.ignition_voltage = ignition_voltage; + packet.fuel_pressure = fuel_pressure; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_EFI_STATUS_LEN); #endif @@ -159,6 +171,93 @@ static inline uint16_t mavlink_msg_efi_status_pack(uint8_t system_id, uint8_t co return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); } +/** + * @brief Pack a efi_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param health EFI health status + * @param ecu_index ECU index + * @param rpm RPM + * @param fuel_consumed [cm^3] Fuel consumed + * @param fuel_flow [cm^3/min] Fuel flow rate + * @param engine_load [%] Engine load + * @param throttle_position [%] Throttle position + * @param spark_dwell_time [ms] Spark dwell time + * @param barometric_pressure [kPa] Barometric pressure + * @param intake_manifold_pressure [kPa] Intake manifold pressure( + * @param intake_manifold_temperature [degC] Intake manifold temperature + * @param cylinder_head_temperature [degC] Cylinder head temperature + * @param ignition_timing [deg] Ignition timing (Crank angle degrees) + * @param injection_time [ms] Injection time + * @param exhaust_gas_temperature [degC] Exhaust gas temperature + * @param throttle_out [%] Output throttle + * @param pt_compensation Pressure/temperature compensation + * @param ignition_voltage [V] Supply voltage to EFI sparking system. Zero in this value means "unknown", so if the supply voltage really is zero volts use 0.0001 instead. + * @param fuel_pressure [kPa] Fuel pressure. Zero in this value means "unknown", so if the fuel pressure really is zero kPa use 0.0001 instead. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_efi_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t health, float ecu_index, float rpm, float fuel_consumed, float fuel_flow, float engine_load, float throttle_position, float spark_dwell_time, float barometric_pressure, float intake_manifold_pressure, float intake_manifold_temperature, float cylinder_head_temperature, float ignition_timing, float injection_time, float exhaust_gas_temperature, float throttle_out, float pt_compensation, float ignition_voltage, float fuel_pressure) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_EFI_STATUS_LEN]; + _mav_put_float(buf, 0, ecu_index); + _mav_put_float(buf, 4, rpm); + _mav_put_float(buf, 8, fuel_consumed); + _mav_put_float(buf, 12, fuel_flow); + _mav_put_float(buf, 16, engine_load); + _mav_put_float(buf, 20, throttle_position); + _mav_put_float(buf, 24, spark_dwell_time); + _mav_put_float(buf, 28, barometric_pressure); + _mav_put_float(buf, 32, intake_manifold_pressure); + _mav_put_float(buf, 36, intake_manifold_temperature); + _mav_put_float(buf, 40, cylinder_head_temperature); + _mav_put_float(buf, 44, ignition_timing); + _mav_put_float(buf, 48, injection_time); + _mav_put_float(buf, 52, exhaust_gas_temperature); + _mav_put_float(buf, 56, throttle_out); + _mav_put_float(buf, 60, pt_compensation); + _mav_put_uint8_t(buf, 64, health); + _mav_put_float(buf, 65, ignition_voltage); + _mav_put_float(buf, 69, fuel_pressure); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_EFI_STATUS_LEN); +#else + mavlink_efi_status_t packet; + packet.ecu_index = ecu_index; + packet.rpm = rpm; + packet.fuel_consumed = fuel_consumed; + packet.fuel_flow = fuel_flow; + packet.engine_load = engine_load; + packet.throttle_position = throttle_position; + packet.spark_dwell_time = spark_dwell_time; + packet.barometric_pressure = barometric_pressure; + packet.intake_manifold_pressure = intake_manifold_pressure; + packet.intake_manifold_temperature = intake_manifold_temperature; + packet.cylinder_head_temperature = cylinder_head_temperature; + packet.ignition_timing = ignition_timing; + packet.injection_time = injection_time; + packet.exhaust_gas_temperature = exhaust_gas_temperature; + packet.throttle_out = throttle_out; + packet.pt_compensation = pt_compensation; + packet.health = health; + packet.ignition_voltage = ignition_voltage; + packet.fuel_pressure = fuel_pressure; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_EFI_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_EFI_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN); +#endif +} + /** * @brief Pack a efi_status message on a channel * @param system_id ID of this system @@ -182,11 +281,13 @@ static inline uint16_t mavlink_msg_efi_status_pack(uint8_t system_id, uint8_t co * @param exhaust_gas_temperature [degC] Exhaust gas temperature * @param throttle_out [%] Output throttle * @param pt_compensation Pressure/temperature compensation + * @param ignition_voltage [V] Supply voltage to EFI sparking system. Zero in this value means "unknown", so if the supply voltage really is zero volts use 0.0001 instead. + * @param fuel_pressure [kPa] Fuel pressure. Zero in this value means "unknown", so if the fuel pressure really is zero kPa use 0.0001 instead. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_efi_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t health,float ecu_index,float rpm,float fuel_consumed,float fuel_flow,float engine_load,float throttle_position,float spark_dwell_time,float barometric_pressure,float intake_manifold_pressure,float intake_manifold_temperature,float cylinder_head_temperature,float ignition_timing,float injection_time,float exhaust_gas_temperature,float throttle_out,float pt_compensation) + uint8_t health,float ecu_index,float rpm,float fuel_consumed,float fuel_flow,float engine_load,float throttle_position,float spark_dwell_time,float barometric_pressure,float intake_manifold_pressure,float intake_manifold_temperature,float cylinder_head_temperature,float ignition_timing,float injection_time,float exhaust_gas_temperature,float throttle_out,float pt_compensation,float ignition_voltage,float fuel_pressure) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_EFI_STATUS_LEN]; @@ -207,6 +308,8 @@ static inline uint16_t mavlink_msg_efi_status_pack_chan(uint8_t system_id, uint8 _mav_put_float(buf, 56, throttle_out); _mav_put_float(buf, 60, pt_compensation); _mav_put_uint8_t(buf, 64, health); + _mav_put_float(buf, 65, ignition_voltage); + _mav_put_float(buf, 69, fuel_pressure); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_EFI_STATUS_LEN); #else @@ -228,6 +331,8 @@ static inline uint16_t mavlink_msg_efi_status_pack_chan(uint8_t system_id, uint8 packet.throttle_out = throttle_out; packet.pt_compensation = pt_compensation; packet.health = health; + packet.ignition_voltage = ignition_voltage; + packet.fuel_pressure = fuel_pressure; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_EFI_STATUS_LEN); #endif @@ -246,7 +351,7 @@ static inline uint16_t mavlink_msg_efi_status_pack_chan(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_efi_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_efi_status_t* efi_status) { - return mavlink_msg_efi_status_pack(system_id, component_id, msg, efi_status->health, efi_status->ecu_index, efi_status->rpm, efi_status->fuel_consumed, efi_status->fuel_flow, efi_status->engine_load, efi_status->throttle_position, efi_status->spark_dwell_time, efi_status->barometric_pressure, efi_status->intake_manifold_pressure, efi_status->intake_manifold_temperature, efi_status->cylinder_head_temperature, efi_status->ignition_timing, efi_status->injection_time, efi_status->exhaust_gas_temperature, efi_status->throttle_out, efi_status->pt_compensation); + return mavlink_msg_efi_status_pack(system_id, component_id, msg, efi_status->health, efi_status->ecu_index, efi_status->rpm, efi_status->fuel_consumed, efi_status->fuel_flow, efi_status->engine_load, efi_status->throttle_position, efi_status->spark_dwell_time, efi_status->barometric_pressure, efi_status->intake_manifold_pressure, efi_status->intake_manifold_temperature, efi_status->cylinder_head_temperature, efi_status->ignition_timing, efi_status->injection_time, efi_status->exhaust_gas_temperature, efi_status->throttle_out, efi_status->pt_compensation, efi_status->ignition_voltage, efi_status->fuel_pressure); } /** @@ -260,7 +365,21 @@ static inline uint16_t mavlink_msg_efi_status_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_efi_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_efi_status_t* efi_status) { - return mavlink_msg_efi_status_pack_chan(system_id, component_id, chan, msg, efi_status->health, efi_status->ecu_index, efi_status->rpm, efi_status->fuel_consumed, efi_status->fuel_flow, efi_status->engine_load, efi_status->throttle_position, efi_status->spark_dwell_time, efi_status->barometric_pressure, efi_status->intake_manifold_pressure, efi_status->intake_manifold_temperature, efi_status->cylinder_head_temperature, efi_status->ignition_timing, efi_status->injection_time, efi_status->exhaust_gas_temperature, efi_status->throttle_out, efi_status->pt_compensation); + return mavlink_msg_efi_status_pack_chan(system_id, component_id, chan, msg, efi_status->health, efi_status->ecu_index, efi_status->rpm, efi_status->fuel_consumed, efi_status->fuel_flow, efi_status->engine_load, efi_status->throttle_position, efi_status->spark_dwell_time, efi_status->barometric_pressure, efi_status->intake_manifold_pressure, efi_status->intake_manifold_temperature, efi_status->cylinder_head_temperature, efi_status->ignition_timing, efi_status->injection_time, efi_status->exhaust_gas_temperature, efi_status->throttle_out, efi_status->pt_compensation, efi_status->ignition_voltage, efi_status->fuel_pressure); +} + +/** + * @brief Encode a efi_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param efi_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_efi_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_efi_status_t* efi_status) +{ + return mavlink_msg_efi_status_pack_status(system_id, component_id, _status, msg, efi_status->health, efi_status->ecu_index, efi_status->rpm, efi_status->fuel_consumed, efi_status->fuel_flow, efi_status->engine_load, efi_status->throttle_position, efi_status->spark_dwell_time, efi_status->barometric_pressure, efi_status->intake_manifold_pressure, efi_status->intake_manifold_temperature, efi_status->cylinder_head_temperature, efi_status->ignition_timing, efi_status->injection_time, efi_status->exhaust_gas_temperature, efi_status->throttle_out, efi_status->pt_compensation, efi_status->ignition_voltage, efi_status->fuel_pressure); } /** @@ -284,10 +403,12 @@ static inline uint16_t mavlink_msg_efi_status_encode_chan(uint8_t system_id, uin * @param exhaust_gas_temperature [degC] Exhaust gas temperature * @param throttle_out [%] Output throttle * @param pt_compensation Pressure/temperature compensation + * @param ignition_voltage [V] Supply voltage to EFI sparking system. Zero in this value means "unknown", so if the supply voltage really is zero volts use 0.0001 instead. + * @param fuel_pressure [kPa] Fuel pressure. Zero in this value means "unknown", so if the fuel pressure really is zero kPa use 0.0001 instead. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_efi_status_send(mavlink_channel_t chan, uint8_t health, float ecu_index, float rpm, float fuel_consumed, float fuel_flow, float engine_load, float throttle_position, float spark_dwell_time, float barometric_pressure, float intake_manifold_pressure, float intake_manifold_temperature, float cylinder_head_temperature, float ignition_timing, float injection_time, float exhaust_gas_temperature, float throttle_out, float pt_compensation) +static inline void mavlink_msg_efi_status_send(mavlink_channel_t chan, uint8_t health, float ecu_index, float rpm, float fuel_consumed, float fuel_flow, float engine_load, float throttle_position, float spark_dwell_time, float barometric_pressure, float intake_manifold_pressure, float intake_manifold_temperature, float cylinder_head_temperature, float ignition_timing, float injection_time, float exhaust_gas_temperature, float throttle_out, float pt_compensation, float ignition_voltage, float fuel_pressure) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_EFI_STATUS_LEN]; @@ -308,6 +429,8 @@ static inline void mavlink_msg_efi_status_send(mavlink_channel_t chan, uint8_t h _mav_put_float(buf, 56, throttle_out); _mav_put_float(buf, 60, pt_compensation); _mav_put_uint8_t(buf, 64, health); + _mav_put_float(buf, 65, ignition_voltage); + _mav_put_float(buf, 69, fuel_pressure); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EFI_STATUS, buf, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); #else @@ -329,6 +452,8 @@ static inline void mavlink_msg_efi_status_send(mavlink_channel_t chan, uint8_t h packet.throttle_out = throttle_out; packet.pt_compensation = pt_compensation; packet.health = health; + packet.ignition_voltage = ignition_voltage; + packet.fuel_pressure = fuel_pressure; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EFI_STATUS, (const char *)&packet, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); #endif @@ -342,7 +467,7 @@ static inline void mavlink_msg_efi_status_send(mavlink_channel_t chan, uint8_t h static inline void mavlink_msg_efi_status_send_struct(mavlink_channel_t chan, const mavlink_efi_status_t* efi_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_efi_status_send(chan, efi_status->health, efi_status->ecu_index, efi_status->rpm, efi_status->fuel_consumed, efi_status->fuel_flow, efi_status->engine_load, efi_status->throttle_position, efi_status->spark_dwell_time, efi_status->barometric_pressure, efi_status->intake_manifold_pressure, efi_status->intake_manifold_temperature, efi_status->cylinder_head_temperature, efi_status->ignition_timing, efi_status->injection_time, efi_status->exhaust_gas_temperature, efi_status->throttle_out, efi_status->pt_compensation); + mavlink_msg_efi_status_send(chan, efi_status->health, efi_status->ecu_index, efi_status->rpm, efi_status->fuel_consumed, efi_status->fuel_flow, efi_status->engine_load, efi_status->throttle_position, efi_status->spark_dwell_time, efi_status->barometric_pressure, efi_status->intake_manifold_pressure, efi_status->intake_manifold_temperature, efi_status->cylinder_head_temperature, efi_status->ignition_timing, efi_status->injection_time, efi_status->exhaust_gas_temperature, efi_status->throttle_out, efi_status->pt_compensation, efi_status->ignition_voltage, efi_status->fuel_pressure); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EFI_STATUS, (const char *)efi_status, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); #endif @@ -350,13 +475,13 @@ static inline void mavlink_msg_efi_status_send_struct(mavlink_channel_t chan, co #if MAVLINK_MSG_ID_EFI_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_efi_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t health, float ecu_index, float rpm, float fuel_consumed, float fuel_flow, float engine_load, float throttle_position, float spark_dwell_time, float barometric_pressure, float intake_manifold_pressure, float intake_manifold_temperature, float cylinder_head_temperature, float ignition_timing, float injection_time, float exhaust_gas_temperature, float throttle_out, float pt_compensation) +static inline void mavlink_msg_efi_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t health, float ecu_index, float rpm, float fuel_consumed, float fuel_flow, float engine_load, float throttle_position, float spark_dwell_time, float barometric_pressure, float intake_manifold_pressure, float intake_manifold_temperature, float cylinder_head_temperature, float ignition_timing, float injection_time, float exhaust_gas_temperature, float throttle_out, float pt_compensation, float ignition_voltage, float fuel_pressure) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -377,6 +502,8 @@ static inline void mavlink_msg_efi_status_send_buf(mavlink_message_t *msgbuf, ma _mav_put_float(buf, 56, throttle_out); _mav_put_float(buf, 60, pt_compensation); _mav_put_uint8_t(buf, 64, health); + _mav_put_float(buf, 65, ignition_voltage); + _mav_put_float(buf, 69, fuel_pressure); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EFI_STATUS, buf, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); #else @@ -398,6 +525,8 @@ static inline void mavlink_msg_efi_status_send_buf(mavlink_message_t *msgbuf, ma packet->throttle_out = throttle_out; packet->pt_compensation = pt_compensation; packet->health = health; + packet->ignition_voltage = ignition_voltage; + packet->fuel_pressure = fuel_pressure; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EFI_STATUS, (const char *)packet, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); #endif @@ -579,6 +708,26 @@ static inline float mavlink_msg_efi_status_get_pt_compensation(const mavlink_mes return _MAV_RETURN_float(msg, 60); } +/** + * @brief Get field ignition_voltage from efi_status message + * + * @return [V] Supply voltage to EFI sparking system. Zero in this value means "unknown", so if the supply voltage really is zero volts use 0.0001 instead. + */ +static inline float mavlink_msg_efi_status_get_ignition_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 65); +} + +/** + * @brief Get field fuel_pressure from efi_status message + * + * @return [kPa] Fuel pressure. Zero in this value means "unknown", so if the fuel pressure really is zero kPa use 0.0001 instead. + */ +static inline float mavlink_msg_efi_status_get_fuel_pressure(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 69); +} + /** * @brief Decode a efi_status message into a struct * @@ -605,6 +754,8 @@ static inline void mavlink_msg_efi_status_decode(const mavlink_message_t* msg, m efi_status->throttle_out = mavlink_msg_efi_status_get_throttle_out(msg); efi_status->pt_compensation = mavlink_msg_efi_status_get_pt_compensation(msg); efi_status->health = mavlink_msg_efi_status_get_health(msg); + efi_status->ignition_voltage = mavlink_msg_efi_status_get_ignition_voltage(msg); + efi_status->fuel_pressure = mavlink_msg_efi_status_get_fuel_pressure(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_EFI_STATUS_LEN? msg->len : MAVLINK_MSG_ID_EFI_STATUS_LEN; memset(efi_status, 0, MAVLINK_MSG_ID_EFI_STATUS_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_encapsulated_data.h b/lib/main/MAVLink/common/mavlink_msg_encapsulated_data.h index bfcf511b921..eef3c3da14e 100755 --- a/lib/main/MAVLink/common/mavlink_msg_encapsulated_data.h +++ b/lib/main/MAVLink/common/mavlink_msg_encapsulated_data.h @@ -59,7 +59,7 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uin #else mavlink_encapsulated_data_t packet; packet.seqnr = seqnr; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*253); + mav_array_assign_uint8_t(packet.data, data, 253); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); #endif @@ -67,6 +67,40 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uin return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ENCAPSULATED_DATA_MIN_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); } +/** + * @brief Pack a encapsulated_data message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param seqnr sequence number (starting with 0 on every transmission) + * @param data image data bytes + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_encapsulated_data_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint16_t seqnr, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN]; + _mav_put_uint16_t(buf, 0, seqnr); + _mav_put_uint8_t_array(buf, 2, data, 253); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#else + mavlink_encapsulated_data_t packet; + packet.seqnr = seqnr; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*253); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ENCAPSULATED_DATA_MIN_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ENCAPSULATED_DATA_MIN_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#endif +} + /** * @brief Pack a encapsulated_data message on a channel * @param system_id ID of this system @@ -89,7 +123,7 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack_chan(uint8_t system_id #else mavlink_encapsulated_data_t packet; packet.seqnr = seqnr; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*253); + mav_array_assign_uint8_t(packet.data, data, 253); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); #endif @@ -124,6 +158,20 @@ static inline uint16_t mavlink_msg_encapsulated_data_encode_chan(uint8_t system_ return mavlink_msg_encapsulated_data_pack_chan(system_id, component_id, chan, msg, encapsulated_data->seqnr, encapsulated_data->data); } +/** + * @brief Encode a encapsulated_data struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param encapsulated_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_encapsulated_data_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_encapsulated_data_t* encapsulated_data) +{ + return mavlink_msg_encapsulated_data_pack_status(system_id, component_id, _status, msg, encapsulated_data->seqnr, encapsulated_data->data); +} + /** * @brief Send a encapsulated_data message * @param chan MAVLink channel to send the message @@ -143,7 +191,7 @@ static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, ui #else mavlink_encapsulated_data_t packet; packet.seqnr = seqnr; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*253); + mav_array_assign_uint8_t(packet.data, data, 253); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)&packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_MIN_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); #endif } @@ -164,7 +212,7 @@ static inline void mavlink_msg_encapsulated_data_send_struct(mavlink_channel_t c #if MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -180,7 +228,7 @@ static inline void mavlink_msg_encapsulated_data_send_buf(mavlink_message_t *msg #else mavlink_encapsulated_data_t *packet = (mavlink_encapsulated_data_t *)msgbuf; packet->seqnr = seqnr; - mav_array_memcpy(packet->data, data, sizeof(uint8_t)*253); + mav_array_assign_uint8_t(packet->data, data, 253); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_MIN_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_esc_info.h b/lib/main/MAVLink/common/mavlink_msg_esc_info.h index d8a8bbcfe1d..463e46abb4b 100644 --- a/lib/main/MAVLink/common/mavlink_msg_esc_info.h +++ b/lib/main/MAVLink/common/mavlink_msg_esc_info.h @@ -9,20 +9,20 @@ typedef struct __mavlink_esc_info_t { uint32_t error_count[4]; /*< Number of reported errors by each ESC since boot.*/ uint16_t counter; /*< Counter of data packets received.*/ uint16_t failure_flags[4]; /*< Bitmap of ESC failure flags.*/ + int16_t temperature[4]; /*< [cdegC] Temperature of each ESC. INT16_MAX: if data not supplied by ESC.*/ uint8_t index; /*< Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4.*/ uint8_t count; /*< Total number of ESCs in all messages of this type. Message fields with an index higher than this should be ignored because they contain invalid data.*/ uint8_t connection_type; /*< Connection type protocol for all ESC.*/ uint8_t info; /*< Information regarding online/offline status of each ESC.*/ - uint8_t temperature[4]; /*< [degC] Temperature measured by each ESC. UINT8_MAX if data not supplied by ESC.*/ } mavlink_esc_info_t; -#define MAVLINK_MSG_ID_ESC_INFO_LEN 42 -#define MAVLINK_MSG_ID_ESC_INFO_MIN_LEN 42 -#define MAVLINK_MSG_ID_290_LEN 42 -#define MAVLINK_MSG_ID_290_MIN_LEN 42 +#define MAVLINK_MSG_ID_ESC_INFO_LEN 46 +#define MAVLINK_MSG_ID_ESC_INFO_MIN_LEN 46 +#define MAVLINK_MSG_ID_290_LEN 46 +#define MAVLINK_MSG_ID_290_MIN_LEN 46 -#define MAVLINK_MSG_ID_ESC_INFO_CRC 221 -#define MAVLINK_MSG_ID_290_CRC 221 +#define MAVLINK_MSG_ID_ESC_INFO_CRC 251 +#define MAVLINK_MSG_ID_290_CRC 251 #define MAVLINK_MSG_ESC_INFO_FIELD_ERROR_COUNT_LEN 4 #define MAVLINK_MSG_ESC_INFO_FIELD_FAILURE_FLAGS_LEN 4 @@ -33,30 +33,30 @@ typedef struct __mavlink_esc_info_t { 290, \ "ESC_INFO", \ 9, \ - { { "index", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_esc_info_t, index) }, \ + { { "index", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_esc_info_t, index) }, \ { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_esc_info_t, time_usec) }, \ { "counter", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_esc_info_t, counter) }, \ - { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_esc_info_t, count) }, \ - { "connection_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_esc_info_t, connection_type) }, \ - { "info", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_esc_info_t, info) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_esc_info_t, count) }, \ + { "connection_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 44, offsetof(mavlink_esc_info_t, connection_type) }, \ + { "info", NULL, MAVLINK_TYPE_UINT8_T, 0, 45, offsetof(mavlink_esc_info_t, info) }, \ { "failure_flags", NULL, MAVLINK_TYPE_UINT16_T, 4, 26, offsetof(mavlink_esc_info_t, failure_flags) }, \ { "error_count", NULL, MAVLINK_TYPE_UINT32_T, 4, 8, offsetof(mavlink_esc_info_t, error_count) }, \ - { "temperature", NULL, MAVLINK_TYPE_UINT8_T, 4, 38, offsetof(mavlink_esc_info_t, temperature) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 4, 34, offsetof(mavlink_esc_info_t, temperature) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_ESC_INFO { \ "ESC_INFO", \ 9, \ - { { "index", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_esc_info_t, index) }, \ + { { "index", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_esc_info_t, index) }, \ { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_esc_info_t, time_usec) }, \ { "counter", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_esc_info_t, counter) }, \ - { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_esc_info_t, count) }, \ - { "connection_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_esc_info_t, connection_type) }, \ - { "info", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_esc_info_t, info) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_esc_info_t, count) }, \ + { "connection_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 44, offsetof(mavlink_esc_info_t, connection_type) }, \ + { "info", NULL, MAVLINK_TYPE_UINT8_T, 0, 45, offsetof(mavlink_esc_info_t, info) }, \ { "failure_flags", NULL, MAVLINK_TYPE_UINT16_T, 4, 26, offsetof(mavlink_esc_info_t, failure_flags) }, \ { "error_count", NULL, MAVLINK_TYPE_UINT32_T, 4, 8, offsetof(mavlink_esc_info_t, error_count) }, \ - { "temperature", NULL, MAVLINK_TYPE_UINT8_T, 4, 38, offsetof(mavlink_esc_info_t, temperature) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 4, 34, offsetof(mavlink_esc_info_t, temperature) }, \ } \ } #endif @@ -75,23 +75,74 @@ typedef struct __mavlink_esc_info_t { * @param info Information regarding online/offline status of each ESC. * @param failure_flags Bitmap of ESC failure flags. * @param error_count Number of reported errors by each ESC since boot. - * @param temperature [degC] Temperature measured by each ESC. UINT8_MAX if data not supplied by ESC. + * @param temperature [cdegC] Temperature of each ESC. INT16_MAX: if data not supplied by ESC. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_esc_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t index, uint64_t time_usec, uint16_t counter, uint8_t count, uint8_t connection_type, uint8_t info, const uint16_t *failure_flags, const uint32_t *error_count, const uint8_t *temperature) + uint8_t index, uint64_t time_usec, uint16_t counter, uint8_t count, uint8_t connection_type, uint8_t info, const uint16_t *failure_flags, const uint32_t *error_count, const int16_t *temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ESC_INFO_LEN]; _mav_put_uint64_t(buf, 0, time_usec); _mav_put_uint16_t(buf, 24, counter); - _mav_put_uint8_t(buf, 34, index); - _mav_put_uint8_t(buf, 35, count); - _mav_put_uint8_t(buf, 36, connection_type); - _mav_put_uint8_t(buf, 37, info); + _mav_put_uint8_t(buf, 42, index); + _mav_put_uint8_t(buf, 43, count); + _mav_put_uint8_t(buf, 44, connection_type); + _mav_put_uint8_t(buf, 45, info); _mav_put_uint32_t_array(buf, 8, error_count, 4); _mav_put_uint16_t_array(buf, 26, failure_flags, 4); - _mav_put_uint8_t_array(buf, 38, temperature, 4); + _mav_put_int16_t_array(buf, 34, temperature, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ESC_INFO_LEN); +#else + mavlink_esc_info_t packet; + packet.time_usec = time_usec; + packet.counter = counter; + packet.index = index; + packet.count = count; + packet.connection_type = connection_type; + packet.info = info; + mav_array_assign_uint32_t(packet.error_count, error_count, 4); + mav_array_assign_uint16_t(packet.failure_flags, failure_flags, 4); + mav_array_assign_int16_t(packet.temperature, temperature, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ESC_INFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ESC_INFO; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); +} + +/** + * @brief Pack a esc_info message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param index Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + * @param counter Counter of data packets received. + * @param count Total number of ESCs in all messages of this type. Message fields with an index higher than this should be ignored because they contain invalid data. + * @param connection_type Connection type protocol for all ESC. + * @param info Information regarding online/offline status of each ESC. + * @param failure_flags Bitmap of ESC failure flags. + * @param error_count Number of reported errors by each ESC since boot. + * @param temperature [cdegC] Temperature of each ESC. INT16_MAX: if data not supplied by ESC. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_esc_info_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t index, uint64_t time_usec, uint16_t counter, uint8_t count, uint8_t connection_type, uint8_t info, const uint16_t *failure_flags, const uint32_t *error_count, const int16_t *temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ESC_INFO_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 24, counter); + _mav_put_uint8_t(buf, 42, index); + _mav_put_uint8_t(buf, 43, count); + _mav_put_uint8_t(buf, 44, connection_type); + _mav_put_uint8_t(buf, 45, info); + _mav_put_uint32_t_array(buf, 8, error_count, 4); + _mav_put_uint16_t_array(buf, 26, failure_flags, 4); + _mav_put_int16_t_array(buf, 34, temperature, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ESC_INFO_LEN); #else mavlink_esc_info_t packet; @@ -103,12 +154,16 @@ static inline uint16_t mavlink_msg_esc_info_pack(uint8_t system_id, uint8_t comp packet.info = info; mav_array_memcpy(packet.error_count, error_count, sizeof(uint32_t)*4); mav_array_memcpy(packet.failure_flags, failure_flags, sizeof(uint16_t)*4); - mav_array_memcpy(packet.temperature, temperature, sizeof(uint8_t)*4); + mav_array_memcpy(packet.temperature, temperature, sizeof(int16_t)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ESC_INFO_LEN); #endif msg->msgid = MAVLINK_MSG_ID_ESC_INFO; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN); +#endif } /** @@ -125,24 +180,24 @@ static inline uint16_t mavlink_msg_esc_info_pack(uint8_t system_id, uint8_t comp * @param info Information regarding online/offline status of each ESC. * @param failure_flags Bitmap of ESC failure flags. * @param error_count Number of reported errors by each ESC since boot. - * @param temperature [degC] Temperature measured by each ESC. UINT8_MAX if data not supplied by ESC. + * @param temperature [cdegC] Temperature of each ESC. INT16_MAX: if data not supplied by ESC. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_esc_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t index,uint64_t time_usec,uint16_t counter,uint8_t count,uint8_t connection_type,uint8_t info,const uint16_t *failure_flags,const uint32_t *error_count,const uint8_t *temperature) + uint8_t index,uint64_t time_usec,uint16_t counter,uint8_t count,uint8_t connection_type,uint8_t info,const uint16_t *failure_flags,const uint32_t *error_count,const int16_t *temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ESC_INFO_LEN]; _mav_put_uint64_t(buf, 0, time_usec); _mav_put_uint16_t(buf, 24, counter); - _mav_put_uint8_t(buf, 34, index); - _mav_put_uint8_t(buf, 35, count); - _mav_put_uint8_t(buf, 36, connection_type); - _mav_put_uint8_t(buf, 37, info); + _mav_put_uint8_t(buf, 42, index); + _mav_put_uint8_t(buf, 43, count); + _mav_put_uint8_t(buf, 44, connection_type); + _mav_put_uint8_t(buf, 45, info); _mav_put_uint32_t_array(buf, 8, error_count, 4); _mav_put_uint16_t_array(buf, 26, failure_flags, 4); - _mav_put_uint8_t_array(buf, 38, temperature, 4); + _mav_put_int16_t_array(buf, 34, temperature, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ESC_INFO_LEN); #else mavlink_esc_info_t packet; @@ -152,9 +207,9 @@ static inline uint16_t mavlink_msg_esc_info_pack_chan(uint8_t system_id, uint8_t packet.count = count; packet.connection_type = connection_type; packet.info = info; - mav_array_memcpy(packet.error_count, error_count, sizeof(uint32_t)*4); - mav_array_memcpy(packet.failure_flags, failure_flags, sizeof(uint16_t)*4); - mav_array_memcpy(packet.temperature, temperature, sizeof(uint8_t)*4); + mav_array_assign_uint32_t(packet.error_count, error_count, 4); + mav_array_assign_uint16_t(packet.failure_flags, failure_flags, 4); + mav_array_assign_int16_t(packet.temperature, temperature, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ESC_INFO_LEN); #endif @@ -189,6 +244,20 @@ static inline uint16_t mavlink_msg_esc_info_encode_chan(uint8_t system_id, uint8 return mavlink_msg_esc_info_pack_chan(system_id, component_id, chan, msg, esc_info->index, esc_info->time_usec, esc_info->counter, esc_info->count, esc_info->connection_type, esc_info->info, esc_info->failure_flags, esc_info->error_count, esc_info->temperature); } +/** + * @brief Encode a esc_info struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param esc_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_esc_info_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_esc_info_t* esc_info) +{ + return mavlink_msg_esc_info_pack_status(system_id, component_id, _status, msg, esc_info->index, esc_info->time_usec, esc_info->counter, esc_info->count, esc_info->connection_type, esc_info->info, esc_info->failure_flags, esc_info->error_count, esc_info->temperature); +} + /** * @brief Send a esc_info message * @param chan MAVLink channel to send the message @@ -201,23 +270,23 @@ static inline uint16_t mavlink_msg_esc_info_encode_chan(uint8_t system_id, uint8 * @param info Information regarding online/offline status of each ESC. * @param failure_flags Bitmap of ESC failure flags. * @param error_count Number of reported errors by each ESC since boot. - * @param temperature [degC] Temperature measured by each ESC. UINT8_MAX if data not supplied by ESC. + * @param temperature [cdegC] Temperature of each ESC. INT16_MAX: if data not supplied by ESC. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_esc_info_send(mavlink_channel_t chan, uint8_t index, uint64_t time_usec, uint16_t counter, uint8_t count, uint8_t connection_type, uint8_t info, const uint16_t *failure_flags, const uint32_t *error_count, const uint8_t *temperature) +static inline void mavlink_msg_esc_info_send(mavlink_channel_t chan, uint8_t index, uint64_t time_usec, uint16_t counter, uint8_t count, uint8_t connection_type, uint8_t info, const uint16_t *failure_flags, const uint32_t *error_count, const int16_t *temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ESC_INFO_LEN]; _mav_put_uint64_t(buf, 0, time_usec); _mav_put_uint16_t(buf, 24, counter); - _mav_put_uint8_t(buf, 34, index); - _mav_put_uint8_t(buf, 35, count); - _mav_put_uint8_t(buf, 36, connection_type); - _mav_put_uint8_t(buf, 37, info); + _mav_put_uint8_t(buf, 42, index); + _mav_put_uint8_t(buf, 43, count); + _mav_put_uint8_t(buf, 44, connection_type); + _mav_put_uint8_t(buf, 45, info); _mav_put_uint32_t_array(buf, 8, error_count, 4); _mav_put_uint16_t_array(buf, 26, failure_flags, 4); - _mav_put_uint8_t_array(buf, 38, temperature, 4); + _mav_put_int16_t_array(buf, 34, temperature, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_INFO, buf, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); #else mavlink_esc_info_t packet; @@ -227,9 +296,9 @@ static inline void mavlink_msg_esc_info_send(mavlink_channel_t chan, uint8_t ind packet.count = count; packet.connection_type = connection_type; packet.info = info; - mav_array_memcpy(packet.error_count, error_count, sizeof(uint32_t)*4); - mav_array_memcpy(packet.failure_flags, failure_flags, sizeof(uint16_t)*4); - mav_array_memcpy(packet.temperature, temperature, sizeof(uint8_t)*4); + mav_array_assign_uint32_t(packet.error_count, error_count, 4); + mav_array_assign_uint16_t(packet.failure_flags, failure_flags, 4); + mav_array_assign_int16_t(packet.temperature, temperature, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_INFO, (const char *)&packet, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); #endif } @@ -250,25 +319,25 @@ static inline void mavlink_msg_esc_info_send_struct(mavlink_channel_t chan, cons #if MAVLINK_MSG_ID_ESC_INFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_esc_info_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t index, uint64_t time_usec, uint16_t counter, uint8_t count, uint8_t connection_type, uint8_t info, const uint16_t *failure_flags, const uint32_t *error_count, const uint8_t *temperature) +static inline void mavlink_msg_esc_info_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t index, uint64_t time_usec, uint16_t counter, uint8_t count, uint8_t connection_type, uint8_t info, const uint16_t *failure_flags, const uint32_t *error_count, const int16_t *temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint64_t(buf, 0, time_usec); _mav_put_uint16_t(buf, 24, counter); - _mav_put_uint8_t(buf, 34, index); - _mav_put_uint8_t(buf, 35, count); - _mav_put_uint8_t(buf, 36, connection_type); - _mav_put_uint8_t(buf, 37, info); + _mav_put_uint8_t(buf, 42, index); + _mav_put_uint8_t(buf, 43, count); + _mav_put_uint8_t(buf, 44, connection_type); + _mav_put_uint8_t(buf, 45, info); _mav_put_uint32_t_array(buf, 8, error_count, 4); _mav_put_uint16_t_array(buf, 26, failure_flags, 4); - _mav_put_uint8_t_array(buf, 38, temperature, 4); + _mav_put_int16_t_array(buf, 34, temperature, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_INFO, buf, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); #else mavlink_esc_info_t *packet = (mavlink_esc_info_t *)msgbuf; @@ -278,9 +347,9 @@ static inline void mavlink_msg_esc_info_send_buf(mavlink_message_t *msgbuf, mavl packet->count = count; packet->connection_type = connection_type; packet->info = info; - mav_array_memcpy(packet->error_count, error_count, sizeof(uint32_t)*4); - mav_array_memcpy(packet->failure_flags, failure_flags, sizeof(uint16_t)*4); - mav_array_memcpy(packet->temperature, temperature, sizeof(uint8_t)*4); + mav_array_assign_uint32_t(packet->error_count, error_count, 4); + mav_array_assign_uint16_t(packet->failure_flags, failure_flags, 4); + mav_array_assign_int16_t(packet->temperature, temperature, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_INFO, (const char *)packet, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); #endif } @@ -298,7 +367,7 @@ static inline void mavlink_msg_esc_info_send_buf(mavlink_message_t *msgbuf, mavl */ static inline uint8_t mavlink_msg_esc_info_get_index(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 34); + return _MAV_RETURN_uint8_t(msg, 42); } /** @@ -328,7 +397,7 @@ static inline uint16_t mavlink_msg_esc_info_get_counter(const mavlink_message_t* */ static inline uint8_t mavlink_msg_esc_info_get_count(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 35); + return _MAV_RETURN_uint8_t(msg, 43); } /** @@ -338,7 +407,7 @@ static inline uint8_t mavlink_msg_esc_info_get_count(const mavlink_message_t* ms */ static inline uint8_t mavlink_msg_esc_info_get_connection_type(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 36); + return _MAV_RETURN_uint8_t(msg, 44); } /** @@ -348,7 +417,7 @@ static inline uint8_t mavlink_msg_esc_info_get_connection_type(const mavlink_mes */ static inline uint8_t mavlink_msg_esc_info_get_info(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 37); + return _MAV_RETURN_uint8_t(msg, 45); } /** @@ -374,11 +443,11 @@ static inline uint16_t mavlink_msg_esc_info_get_error_count(const mavlink_messag /** * @brief Get field temperature from esc_info message * - * @return [degC] Temperature measured by each ESC. UINT8_MAX if data not supplied by ESC. + * @return [cdegC] Temperature of each ESC. INT16_MAX: if data not supplied by ESC. */ -static inline uint16_t mavlink_msg_esc_info_get_temperature(const mavlink_message_t* msg, uint8_t *temperature) +static inline uint16_t mavlink_msg_esc_info_get_temperature(const mavlink_message_t* msg, int16_t *temperature) { - return _MAV_RETURN_uint8_t_array(msg, temperature, 4, 38); + return _MAV_RETURN_int16_t_array(msg, temperature, 4, 34); } /** @@ -394,11 +463,11 @@ static inline void mavlink_msg_esc_info_decode(const mavlink_message_t* msg, mav mavlink_msg_esc_info_get_error_count(msg, esc_info->error_count); esc_info->counter = mavlink_msg_esc_info_get_counter(msg); mavlink_msg_esc_info_get_failure_flags(msg, esc_info->failure_flags); + mavlink_msg_esc_info_get_temperature(msg, esc_info->temperature); esc_info->index = mavlink_msg_esc_info_get_index(msg); esc_info->count = mavlink_msg_esc_info_get_count(msg); esc_info->connection_type = mavlink_msg_esc_info_get_connection_type(msg); esc_info->info = mavlink_msg_esc_info_get_info(msg); - mavlink_msg_esc_info_get_temperature(msg, esc_info->temperature); #else uint8_t len = msg->len < MAVLINK_MSG_ID_ESC_INFO_LEN? msg->len : MAVLINK_MSG_ID_ESC_INFO_LEN; memset(esc_info, 0, MAVLINK_MSG_ID_ESC_INFO_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_esc_status.h b/lib/main/MAVLink/common/mavlink_msg_esc_status.h index 3a5fefe20ac..1fc33dda0a1 100644 --- a/lib/main/MAVLink/common/mavlink_msg_esc_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_esc_status.h @@ -65,6 +65,45 @@ typedef struct __mavlink_esc_status_t { static inline uint16_t mavlink_msg_esc_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t index, uint64_t time_usec, const int32_t *rpm, const float *voltage, const float *current) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ESC_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 56, index); + _mav_put_int32_t_array(buf, 8, rpm, 4); + _mav_put_float_array(buf, 24, voltage, 4); + _mav_put_float_array(buf, 40, current, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ESC_STATUS_LEN); +#else + mavlink_esc_status_t packet; + packet.time_usec = time_usec; + packet.index = index; + mav_array_assign_int32_t(packet.rpm, rpm, 4); + mav_array_assign_float(packet.voltage, voltage, 4); + mav_array_assign_float(packet.current, current, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ESC_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ESC_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); +} + +/** + * @brief Pack a esc_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param index Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + * @param rpm [rpm] Reported motor RPM from each ESC (negative for reverse rotation). + * @param voltage [V] Voltage measured from each ESC. + * @param current [A] Current measured from each ESC. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_esc_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t index, uint64_t time_usec, const int32_t *rpm, const float *voltage, const float *current) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ESC_STATUS_LEN]; _mav_put_uint64_t(buf, 0, time_usec); @@ -84,7 +123,11 @@ static inline uint16_t mavlink_msg_esc_status_pack(uint8_t system_id, uint8_t co #endif msg->msgid = MAVLINK_MSG_ID_ESC_STATUS; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN); +#endif } /** @@ -116,9 +159,9 @@ static inline uint16_t mavlink_msg_esc_status_pack_chan(uint8_t system_id, uint8 mavlink_esc_status_t packet; packet.time_usec = time_usec; packet.index = index; - mav_array_memcpy(packet.rpm, rpm, sizeof(int32_t)*4); - mav_array_memcpy(packet.voltage, voltage, sizeof(float)*4); - mav_array_memcpy(packet.current, current, sizeof(float)*4); + mav_array_assign_int32_t(packet.rpm, rpm, 4); + mav_array_assign_float(packet.voltage, voltage, 4); + mav_array_assign_float(packet.current, current, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ESC_STATUS_LEN); #endif @@ -153,6 +196,20 @@ static inline uint16_t mavlink_msg_esc_status_encode_chan(uint8_t system_id, uin return mavlink_msg_esc_status_pack_chan(system_id, component_id, chan, msg, esc_status->index, esc_status->time_usec, esc_status->rpm, esc_status->voltage, esc_status->current); } +/** + * @brief Encode a esc_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param esc_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_esc_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_esc_status_t* esc_status) +{ + return mavlink_msg_esc_status_pack_status(system_id, component_id, _status, msg, esc_status->index, esc_status->time_usec, esc_status->rpm, esc_status->voltage, esc_status->current); +} + /** * @brief Send a esc_status message * @param chan MAVLink channel to send the message @@ -179,9 +236,9 @@ static inline void mavlink_msg_esc_status_send(mavlink_channel_t chan, uint8_t i mavlink_esc_status_t packet; packet.time_usec = time_usec; packet.index = index; - mav_array_memcpy(packet.rpm, rpm, sizeof(int32_t)*4); - mav_array_memcpy(packet.voltage, voltage, sizeof(float)*4); - mav_array_memcpy(packet.current, current, sizeof(float)*4); + mav_array_assign_int32_t(packet.rpm, rpm, 4); + mav_array_assign_float(packet.voltage, voltage, 4); + mav_array_assign_float(packet.current, current, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); #endif } @@ -202,7 +259,7 @@ static inline void mavlink_msg_esc_status_send_struct(mavlink_channel_t chan, co #if MAVLINK_MSG_ID_ESC_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -222,9 +279,9 @@ static inline void mavlink_msg_esc_status_send_buf(mavlink_message_t *msgbuf, ma mavlink_esc_status_t *packet = (mavlink_esc_status_t *)msgbuf; packet->time_usec = time_usec; packet->index = index; - mav_array_memcpy(packet->rpm, rpm, sizeof(int32_t)*4); - mav_array_memcpy(packet->voltage, voltage, sizeof(float)*4); - mav_array_memcpy(packet->current, current, sizeof(float)*4); + mav_array_assign_int32_t(packet->rpm, rpm, 4); + mav_array_assign_float(packet->voltage, voltage, 4); + mav_array_assign_float(packet->current, current, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_STATUS, (const char *)packet, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_estimator_status.h b/lib/main/MAVLink/common/mavlink_msg_estimator_status.h index bc5499b920a..4a53e069717 100755 --- a/lib/main/MAVLink/common/mavlink_msg_estimator_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_estimator_status.h @@ -117,6 +117,66 @@ static inline uint16_t mavlink_msg_estimator_status_pack(uint8_t system_id, uint return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ESTIMATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESTIMATOR_STATUS_LEN, MAVLINK_MSG_ID_ESTIMATOR_STATUS_CRC); } +/** + * @brief Pack a estimator_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param flags Bitmap indicating which EKF outputs are valid. + * @param vel_ratio Velocity innovation test ratio + * @param pos_horiz_ratio Horizontal position innovation test ratio + * @param pos_vert_ratio Vertical position innovation test ratio + * @param mag_ratio Magnetometer innovation test ratio + * @param hagl_ratio Height above terrain innovation test ratio + * @param tas_ratio True airspeed innovation test ratio + * @param pos_horiz_accuracy [m] Horizontal position 1-STD accuracy relative to the EKF local origin + * @param pos_vert_accuracy [m] Vertical position 1-STD accuracy relative to the EKF local origin + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_estimator_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint16_t flags, float vel_ratio, float pos_horiz_ratio, float pos_vert_ratio, float mag_ratio, float hagl_ratio, float tas_ratio, float pos_horiz_accuracy, float pos_vert_accuracy) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ESTIMATOR_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, vel_ratio); + _mav_put_float(buf, 12, pos_horiz_ratio); + _mav_put_float(buf, 16, pos_vert_ratio); + _mav_put_float(buf, 20, mag_ratio); + _mav_put_float(buf, 24, hagl_ratio); + _mav_put_float(buf, 28, tas_ratio); + _mav_put_float(buf, 32, pos_horiz_accuracy); + _mav_put_float(buf, 36, pos_vert_accuracy); + _mav_put_uint16_t(buf, 40, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ESTIMATOR_STATUS_LEN); +#else + mavlink_estimator_status_t packet; + packet.time_usec = time_usec; + packet.vel_ratio = vel_ratio; + packet.pos_horiz_ratio = pos_horiz_ratio; + packet.pos_vert_ratio = pos_vert_ratio; + packet.mag_ratio = mag_ratio; + packet.hagl_ratio = hagl_ratio; + packet.tas_ratio = tas_ratio; + packet.pos_horiz_accuracy = pos_horiz_accuracy; + packet.pos_vert_accuracy = pos_vert_accuracy; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ESTIMATOR_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ESTIMATOR_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ESTIMATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESTIMATOR_STATUS_LEN, MAVLINK_MSG_ID_ESTIMATOR_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ESTIMATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESTIMATOR_STATUS_LEN); +#endif +} + /** * @brief Pack a estimator_status message on a channel * @param system_id ID of this system @@ -200,6 +260,20 @@ static inline uint16_t mavlink_msg_estimator_status_encode_chan(uint8_t system_i return mavlink_msg_estimator_status_pack_chan(system_id, component_id, chan, msg, estimator_status->time_usec, estimator_status->flags, estimator_status->vel_ratio, estimator_status->pos_horiz_ratio, estimator_status->pos_vert_ratio, estimator_status->mag_ratio, estimator_status->hagl_ratio, estimator_status->tas_ratio, estimator_status->pos_horiz_accuracy, estimator_status->pos_vert_accuracy); } +/** + * @brief Encode a estimator_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param estimator_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_estimator_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_estimator_status_t* estimator_status) +{ + return mavlink_msg_estimator_status_pack_status(system_id, component_id, _status, msg, estimator_status->time_usec, estimator_status->flags, estimator_status->vel_ratio, estimator_status->pos_horiz_ratio, estimator_status->pos_vert_ratio, estimator_status->mag_ratio, estimator_status->hagl_ratio, estimator_status->tas_ratio, estimator_status->pos_horiz_accuracy, estimator_status->pos_vert_accuracy); +} + /** * @brief Send a estimator_status message * @param chan MAVLink channel to send the message @@ -266,7 +340,7 @@ static inline void mavlink_msg_estimator_status_send_struct(mavlink_channel_t ch #if MAVLINK_MSG_ID_ESTIMATOR_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_event.h b/lib/main/MAVLink/common/mavlink_msg_event.h new file mode 100644 index 00000000000..0567433144d --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_event.h @@ -0,0 +1,418 @@ +#pragma once +// MESSAGE EVENT PACKING + +#define MAVLINK_MSG_ID_EVENT 410 + + +typedef struct __mavlink_event_t { + uint32_t id; /*< Event ID (as defined in the component metadata)*/ + uint32_t event_time_boot_ms; /*< [ms] Timestamp (time since system boot when the event happened).*/ + uint16_t sequence; /*< Sequence number.*/ + uint8_t destination_component; /*< Component ID*/ + uint8_t destination_system; /*< System ID*/ + uint8_t log_levels; /*< Log levels: 4 bits MSB: internal (for logging purposes), 4 bits LSB: external. Levels: Emergency = 0, Alert = 1, Critical = 2, Error = 3, Warning = 4, Notice = 5, Info = 6, Debug = 7, Protocol = 8, Disabled = 9*/ + uint8_t arguments[40]; /*< Arguments (depend on event ID).*/ +} mavlink_event_t; + +#define MAVLINK_MSG_ID_EVENT_LEN 53 +#define MAVLINK_MSG_ID_EVENT_MIN_LEN 53 +#define MAVLINK_MSG_ID_410_LEN 53 +#define MAVLINK_MSG_ID_410_MIN_LEN 53 + +#define MAVLINK_MSG_ID_EVENT_CRC 160 +#define MAVLINK_MSG_ID_410_CRC 160 + +#define MAVLINK_MSG_EVENT_FIELD_ARGUMENTS_LEN 40 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_EVENT { \ + 410, \ + "EVENT", \ + 7, \ + { { "destination_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_event_t, destination_component) }, \ + { "destination_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_event_t, destination_system) }, \ + { "id", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_event_t, id) }, \ + { "event_time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_event_t, event_time_boot_ms) }, \ + { "sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_event_t, sequence) }, \ + { "log_levels", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_event_t, log_levels) }, \ + { "arguments", NULL, MAVLINK_TYPE_UINT8_T, 40, 13, offsetof(mavlink_event_t, arguments) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_EVENT { \ + "EVENT", \ + 7, \ + { { "destination_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_event_t, destination_component) }, \ + { "destination_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_event_t, destination_system) }, \ + { "id", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_event_t, id) }, \ + { "event_time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_event_t, event_time_boot_ms) }, \ + { "sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_event_t, sequence) }, \ + { "log_levels", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_event_t, log_levels) }, \ + { "arguments", NULL, MAVLINK_TYPE_UINT8_T, 40, 13, offsetof(mavlink_event_t, arguments) }, \ + } \ +} +#endif + +/** + * @brief Pack a event message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param destination_component Component ID + * @param destination_system System ID + * @param id Event ID (as defined in the component metadata) + * @param event_time_boot_ms [ms] Timestamp (time since system boot when the event happened). + * @param sequence Sequence number. + * @param log_levels Log levels: 4 bits MSB: internal (for logging purposes), 4 bits LSB: external. Levels: Emergency = 0, Alert = 1, Critical = 2, Error = 3, Warning = 4, Notice = 5, Info = 6, Debug = 7, Protocol = 8, Disabled = 9 + * @param arguments Arguments (depend on event ID). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_event_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t destination_component, uint8_t destination_system, uint32_t id, uint32_t event_time_boot_ms, uint16_t sequence, uint8_t log_levels, const uint8_t *arguments) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_EVENT_LEN]; + _mav_put_uint32_t(buf, 0, id); + _mav_put_uint32_t(buf, 4, event_time_boot_ms); + _mav_put_uint16_t(buf, 8, sequence); + _mav_put_uint8_t(buf, 10, destination_component); + _mav_put_uint8_t(buf, 11, destination_system); + _mav_put_uint8_t(buf, 12, log_levels); + _mav_put_uint8_t_array(buf, 13, arguments, 40); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_EVENT_LEN); +#else + mavlink_event_t packet; + packet.id = id; + packet.event_time_boot_ms = event_time_boot_ms; + packet.sequence = sequence; + packet.destination_component = destination_component; + packet.destination_system = destination_system; + packet.log_levels = log_levels; + mav_array_assign_uint8_t(packet.arguments, arguments, 40); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_EVENT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_EVENT; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_EVENT_MIN_LEN, MAVLINK_MSG_ID_EVENT_LEN, MAVLINK_MSG_ID_EVENT_CRC); +} + +/** + * @brief Pack a event message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param destination_component Component ID + * @param destination_system System ID + * @param id Event ID (as defined in the component metadata) + * @param event_time_boot_ms [ms] Timestamp (time since system boot when the event happened). + * @param sequence Sequence number. + * @param log_levels Log levels: 4 bits MSB: internal (for logging purposes), 4 bits LSB: external. Levels: Emergency = 0, Alert = 1, Critical = 2, Error = 3, Warning = 4, Notice = 5, Info = 6, Debug = 7, Protocol = 8, Disabled = 9 + * @param arguments Arguments (depend on event ID). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_event_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t destination_component, uint8_t destination_system, uint32_t id, uint32_t event_time_boot_ms, uint16_t sequence, uint8_t log_levels, const uint8_t *arguments) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_EVENT_LEN]; + _mav_put_uint32_t(buf, 0, id); + _mav_put_uint32_t(buf, 4, event_time_boot_ms); + _mav_put_uint16_t(buf, 8, sequence); + _mav_put_uint8_t(buf, 10, destination_component); + _mav_put_uint8_t(buf, 11, destination_system); + _mav_put_uint8_t(buf, 12, log_levels); + _mav_put_uint8_t_array(buf, 13, arguments, 40); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_EVENT_LEN); +#else + mavlink_event_t packet; + packet.id = id; + packet.event_time_boot_ms = event_time_boot_ms; + packet.sequence = sequence; + packet.destination_component = destination_component; + packet.destination_system = destination_system; + packet.log_levels = log_levels; + mav_array_memcpy(packet.arguments, arguments, sizeof(uint8_t)*40); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_EVENT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_EVENT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_EVENT_MIN_LEN, MAVLINK_MSG_ID_EVENT_LEN, MAVLINK_MSG_ID_EVENT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_EVENT_MIN_LEN, MAVLINK_MSG_ID_EVENT_LEN); +#endif +} + +/** + * @brief Pack a event message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param destination_component Component ID + * @param destination_system System ID + * @param id Event ID (as defined in the component metadata) + * @param event_time_boot_ms [ms] Timestamp (time since system boot when the event happened). + * @param sequence Sequence number. + * @param log_levels Log levels: 4 bits MSB: internal (for logging purposes), 4 bits LSB: external. Levels: Emergency = 0, Alert = 1, Critical = 2, Error = 3, Warning = 4, Notice = 5, Info = 6, Debug = 7, Protocol = 8, Disabled = 9 + * @param arguments Arguments (depend on event ID). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_event_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t destination_component,uint8_t destination_system,uint32_t id,uint32_t event_time_boot_ms,uint16_t sequence,uint8_t log_levels,const uint8_t *arguments) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_EVENT_LEN]; + _mav_put_uint32_t(buf, 0, id); + _mav_put_uint32_t(buf, 4, event_time_boot_ms); + _mav_put_uint16_t(buf, 8, sequence); + _mav_put_uint8_t(buf, 10, destination_component); + _mav_put_uint8_t(buf, 11, destination_system); + _mav_put_uint8_t(buf, 12, log_levels); + _mav_put_uint8_t_array(buf, 13, arguments, 40); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_EVENT_LEN); +#else + mavlink_event_t packet; + packet.id = id; + packet.event_time_boot_ms = event_time_boot_ms; + packet.sequence = sequence; + packet.destination_component = destination_component; + packet.destination_system = destination_system; + packet.log_levels = log_levels; + mav_array_assign_uint8_t(packet.arguments, arguments, 40); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_EVENT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_EVENT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_EVENT_MIN_LEN, MAVLINK_MSG_ID_EVENT_LEN, MAVLINK_MSG_ID_EVENT_CRC); +} + +/** + * @brief Encode a event struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param event C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_event_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_event_t* event) +{ + return mavlink_msg_event_pack(system_id, component_id, msg, event->destination_component, event->destination_system, event->id, event->event_time_boot_ms, event->sequence, event->log_levels, event->arguments); +} + +/** + * @brief Encode a event struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param event C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_event_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_event_t* event) +{ + return mavlink_msg_event_pack_chan(system_id, component_id, chan, msg, event->destination_component, event->destination_system, event->id, event->event_time_boot_ms, event->sequence, event->log_levels, event->arguments); +} + +/** + * @brief Encode a event struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param event C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_event_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_event_t* event) +{ + return mavlink_msg_event_pack_status(system_id, component_id, _status, msg, event->destination_component, event->destination_system, event->id, event->event_time_boot_ms, event->sequence, event->log_levels, event->arguments); +} + +/** + * @brief Send a event message + * @param chan MAVLink channel to send the message + * + * @param destination_component Component ID + * @param destination_system System ID + * @param id Event ID (as defined in the component metadata) + * @param event_time_boot_ms [ms] Timestamp (time since system boot when the event happened). + * @param sequence Sequence number. + * @param log_levels Log levels: 4 bits MSB: internal (for logging purposes), 4 bits LSB: external. Levels: Emergency = 0, Alert = 1, Critical = 2, Error = 3, Warning = 4, Notice = 5, Info = 6, Debug = 7, Protocol = 8, Disabled = 9 + * @param arguments Arguments (depend on event ID). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_event_send(mavlink_channel_t chan, uint8_t destination_component, uint8_t destination_system, uint32_t id, uint32_t event_time_boot_ms, uint16_t sequence, uint8_t log_levels, const uint8_t *arguments) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_EVENT_LEN]; + _mav_put_uint32_t(buf, 0, id); + _mav_put_uint32_t(buf, 4, event_time_boot_ms); + _mav_put_uint16_t(buf, 8, sequence); + _mav_put_uint8_t(buf, 10, destination_component); + _mav_put_uint8_t(buf, 11, destination_system); + _mav_put_uint8_t(buf, 12, log_levels); + _mav_put_uint8_t_array(buf, 13, arguments, 40); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EVENT, buf, MAVLINK_MSG_ID_EVENT_MIN_LEN, MAVLINK_MSG_ID_EVENT_LEN, MAVLINK_MSG_ID_EVENT_CRC); +#else + mavlink_event_t packet; + packet.id = id; + packet.event_time_boot_ms = event_time_boot_ms; + packet.sequence = sequence; + packet.destination_component = destination_component; + packet.destination_system = destination_system; + packet.log_levels = log_levels; + mav_array_assign_uint8_t(packet.arguments, arguments, 40); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EVENT, (const char *)&packet, MAVLINK_MSG_ID_EVENT_MIN_LEN, MAVLINK_MSG_ID_EVENT_LEN, MAVLINK_MSG_ID_EVENT_CRC); +#endif +} + +/** + * @brief Send a event message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_event_send_struct(mavlink_channel_t chan, const mavlink_event_t* event) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_event_send(chan, event->destination_component, event->destination_system, event->id, event->event_time_boot_ms, event->sequence, event->log_levels, event->arguments); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EVENT, (const char *)event, MAVLINK_MSG_ID_EVENT_MIN_LEN, MAVLINK_MSG_ID_EVENT_LEN, MAVLINK_MSG_ID_EVENT_CRC); +#endif +} + +#if MAVLINK_MSG_ID_EVENT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_event_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t destination_component, uint8_t destination_system, uint32_t id, uint32_t event_time_boot_ms, uint16_t sequence, uint8_t log_levels, const uint8_t *arguments) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, id); + _mav_put_uint32_t(buf, 4, event_time_boot_ms); + _mav_put_uint16_t(buf, 8, sequence); + _mav_put_uint8_t(buf, 10, destination_component); + _mav_put_uint8_t(buf, 11, destination_system); + _mav_put_uint8_t(buf, 12, log_levels); + _mav_put_uint8_t_array(buf, 13, arguments, 40); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EVENT, buf, MAVLINK_MSG_ID_EVENT_MIN_LEN, MAVLINK_MSG_ID_EVENT_LEN, MAVLINK_MSG_ID_EVENT_CRC); +#else + mavlink_event_t *packet = (mavlink_event_t *)msgbuf; + packet->id = id; + packet->event_time_boot_ms = event_time_boot_ms; + packet->sequence = sequence; + packet->destination_component = destination_component; + packet->destination_system = destination_system; + packet->log_levels = log_levels; + mav_array_assign_uint8_t(packet->arguments, arguments, 40); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EVENT, (const char *)packet, MAVLINK_MSG_ID_EVENT_MIN_LEN, MAVLINK_MSG_ID_EVENT_LEN, MAVLINK_MSG_ID_EVENT_CRC); +#endif +} +#endif + +#endif + +// MESSAGE EVENT UNPACKING + + +/** + * @brief Get field destination_component from event message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_event_get_destination_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 10); +} + +/** + * @brief Get field destination_system from event message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_event_get_destination_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 11); +} + +/** + * @brief Get field id from event message + * + * @return Event ID (as defined in the component metadata) + */ +static inline uint32_t mavlink_msg_event_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field event_time_boot_ms from event message + * + * @return [ms] Timestamp (time since system boot when the event happened). + */ +static inline uint32_t mavlink_msg_event_get_event_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field sequence from event message + * + * @return Sequence number. + */ +static inline uint16_t mavlink_msg_event_get_sequence(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field log_levels from event message + * + * @return Log levels: 4 bits MSB: internal (for logging purposes), 4 bits LSB: external. Levels: Emergency = 0, Alert = 1, Critical = 2, Error = 3, Warning = 4, Notice = 5, Info = 6, Debug = 7, Protocol = 8, Disabled = 9 + */ +static inline uint8_t mavlink_msg_event_get_log_levels(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Get field arguments from event message + * + * @return Arguments (depend on event ID). + */ +static inline uint16_t mavlink_msg_event_get_arguments(const mavlink_message_t* msg, uint8_t *arguments) +{ + return _MAV_RETURN_uint8_t_array(msg, arguments, 40, 13); +} + +/** + * @brief Decode a event message into a struct + * + * @param msg The message to decode + * @param event C-struct to decode the message contents into + */ +static inline void mavlink_msg_event_decode(const mavlink_message_t* msg, mavlink_event_t* event) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + event->id = mavlink_msg_event_get_id(msg); + event->event_time_boot_ms = mavlink_msg_event_get_event_time_boot_ms(msg); + event->sequence = mavlink_msg_event_get_sequence(msg); + event->destination_component = mavlink_msg_event_get_destination_component(msg); + event->destination_system = mavlink_msg_event_get_destination_system(msg); + event->log_levels = mavlink_msg_event_get_log_levels(msg); + mavlink_msg_event_get_arguments(msg, event->arguments); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_EVENT_LEN? msg->len : MAVLINK_MSG_ID_EVENT_LEN; + memset(event, 0, MAVLINK_MSG_ID_EVENT_LEN); + memcpy(event, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_extended_sys_state.h b/lib/main/MAVLink/common/mavlink_msg_extended_sys_state.h index 2eeeed0c0bd..a56ba8f7887 100755 --- a/lib/main/MAVLink/common/mavlink_msg_extended_sys_state.h +++ b/lib/main/MAVLink/common/mavlink_msg_extended_sys_state.h @@ -69,6 +69,42 @@ static inline uint16_t mavlink_msg_extended_sys_state_pack(uint8_t system_id, ui return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_EXTENDED_SYS_STATE_MIN_LEN, MAVLINK_MSG_ID_EXTENDED_SYS_STATE_LEN, MAVLINK_MSG_ID_EXTENDED_SYS_STATE_CRC); } +/** + * @brief Pack a extended_sys_state message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param vtol_state The VTOL state if applicable. Is set to MAV_VTOL_STATE_UNDEFINED if UAV is not in VTOL configuration. + * @param landed_state The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_extended_sys_state_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t vtol_state, uint8_t landed_state) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_EXTENDED_SYS_STATE_LEN]; + _mav_put_uint8_t(buf, 0, vtol_state); + _mav_put_uint8_t(buf, 1, landed_state); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_EXTENDED_SYS_STATE_LEN); +#else + mavlink_extended_sys_state_t packet; + packet.vtol_state = vtol_state; + packet.landed_state = landed_state; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_EXTENDED_SYS_STATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_EXTENDED_SYS_STATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_EXTENDED_SYS_STATE_MIN_LEN, MAVLINK_MSG_ID_EXTENDED_SYS_STATE_LEN, MAVLINK_MSG_ID_EXTENDED_SYS_STATE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_EXTENDED_SYS_STATE_MIN_LEN, MAVLINK_MSG_ID_EXTENDED_SYS_STATE_LEN); +#endif +} + /** * @brief Pack a extended_sys_state message on a channel * @param system_id ID of this system @@ -128,6 +164,20 @@ static inline uint16_t mavlink_msg_extended_sys_state_encode_chan(uint8_t system return mavlink_msg_extended_sys_state_pack_chan(system_id, component_id, chan, msg, extended_sys_state->vtol_state, extended_sys_state->landed_state); } +/** + * @brief Encode a extended_sys_state struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param extended_sys_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_extended_sys_state_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_extended_sys_state_t* extended_sys_state) +{ + return mavlink_msg_extended_sys_state_pack_status(system_id, component_id, _status, msg, extended_sys_state->vtol_state, extended_sys_state->landed_state); +} + /** * @brief Send a extended_sys_state message * @param chan MAVLink channel to send the message @@ -170,7 +220,7 @@ static inline void mavlink_msg_extended_sys_state_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_EXTENDED_SYS_STATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_fence_status.h b/lib/main/MAVLink/common/mavlink_msg_fence_status.h index 08ba1555f2a..61745bb0cb5 100644 --- a/lib/main/MAVLink/common/mavlink_msg_fence_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_fence_status.h @@ -87,6 +87,51 @@ static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FENCE_STATUS_MIN_LEN, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); } +/** + * @brief Pack a fence_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param breach_status Breach status (0 if currently inside fence, 1 if outside). + * @param breach_count Number of fence breaches. + * @param breach_type Last breach type. + * @param breach_time [ms] Time (since boot) of last breach. + * @param breach_mitigation Active action to prevent fence breach + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_fence_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time, uint8_t breach_mitigation) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, breach_time); + _mav_put_uint16_t(buf, 4, breach_count); + _mav_put_uint8_t(buf, 6, breach_status); + _mav_put_uint8_t(buf, 7, breach_type); + _mav_put_uint8_t(buf, 8, breach_mitigation); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN); +#else + mavlink_fence_status_t packet; + packet.breach_time = breach_time; + packet.breach_count = breach_count; + packet.breach_status = breach_status; + packet.breach_type = breach_type; + packet.breach_mitigation = breach_mitigation; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FENCE_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_FENCE_STATUS_MIN_LEN, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_FENCE_STATUS_MIN_LEN, MAVLINK_MSG_ID_FENCE_STATUS_LEN); +#endif +} + /** * @brief Pack a fence_status message on a channel * @param system_id ID of this system @@ -155,6 +200,20 @@ static inline uint16_t mavlink_msg_fence_status_encode_chan(uint8_t system_id, u return mavlink_msg_fence_status_pack_chan(system_id, component_id, chan, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time, fence_status->breach_mitigation); } +/** + * @brief Encode a fence_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param fence_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fence_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_fence_status_t* fence_status) +{ + return mavlink_msg_fence_status_pack_status(system_id, component_id, _status, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time, fence_status->breach_mitigation); +} + /** * @brief Send a fence_status message * @param chan MAVLink channel to send the message @@ -206,7 +265,7 @@ static inline void mavlink_msg_fence_status_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_FENCE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_file_transfer_protocol.h b/lib/main/MAVLink/common/mavlink_msg_file_transfer_protocol.h index 0dd4d0530d1..091aec65e66 100755 --- a/lib/main/MAVLink/common/mavlink_msg_file_transfer_protocol.h +++ b/lib/main/MAVLink/common/mavlink_msg_file_transfer_protocol.h @@ -8,7 +8,7 @@ typedef struct __mavlink_file_transfer_protocol_t { uint8_t target_network; /*< Network ID (0 for broadcast)*/ uint8_t target_system; /*< System ID (0 for broadcast)*/ uint8_t target_component; /*< Component ID (0 for broadcast)*/ - uint8_t payload[251]; /*< Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification.*/ + uint8_t payload[251]; /*< Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The content/format of this block is defined in https://mavlink.io/en/services/ftp.html.*/ } mavlink_file_transfer_protocol_t; #define MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN 254 @@ -53,7 +53,7 @@ typedef struct __mavlink_file_transfer_protocol_t { * @param target_network Network ID (0 for broadcast) * @param target_system System ID (0 for broadcast) * @param target_component Component ID (0 for broadcast) - * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The content/format of this block is defined in https://mavlink.io/en/services/ftp.html. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_file_transfer_protocol_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -71,7 +71,7 @@ static inline uint16_t mavlink_msg_file_transfer_protocol_pack(uint8_t system_id packet.target_network = target_network; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*251); + mav_array_assign_uint8_t(packet.payload, payload, 251); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); #endif @@ -79,6 +79,46 @@ static inline uint16_t mavlink_msg_file_transfer_protocol_pack(uint8_t system_id return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_MIN_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_CRC); } +/** + * @brief Pack a file_transfer_protocol message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_network Network ID (0 for broadcast) + * @param target_system System ID (0 for broadcast) + * @param target_component Component ID (0 for broadcast) + * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The content/format of this block is defined in https://mavlink.io/en/services/ftp.html. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_file_transfer_protocol_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_network, uint8_t target_system, uint8_t target_component, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN]; + _mav_put_uint8_t(buf, 0, target_network); + _mav_put_uint8_t(buf, 1, target_system); + _mav_put_uint8_t(buf, 2, target_component); + _mav_put_uint8_t_array(buf, 3, payload, 251); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#else + mavlink_file_transfer_protocol_t packet; + packet.target_network = target_network; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*251); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_MIN_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_MIN_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); +#endif +} + /** * @brief Pack a file_transfer_protocol message on a channel * @param system_id ID of this system @@ -88,7 +128,7 @@ static inline uint16_t mavlink_msg_file_transfer_protocol_pack(uint8_t system_id * @param target_network Network ID (0 for broadcast) * @param target_system System ID (0 for broadcast) * @param target_component Component ID (0 for broadcast) - * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The content/format of this block is defined in https://mavlink.io/en/services/ftp.html. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_file_transfer_protocol_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -107,7 +147,7 @@ static inline uint16_t mavlink_msg_file_transfer_protocol_pack_chan(uint8_t syst packet.target_network = target_network; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*251); + mav_array_assign_uint8_t(packet.payload, payload, 251); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN); #endif @@ -142,6 +182,20 @@ static inline uint16_t mavlink_msg_file_transfer_protocol_encode_chan(uint8_t sy return mavlink_msg_file_transfer_protocol_pack_chan(system_id, component_id, chan, msg, file_transfer_protocol->target_network, file_transfer_protocol->target_system, file_transfer_protocol->target_component, file_transfer_protocol->payload); } +/** + * @brief Encode a file_transfer_protocol struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param file_transfer_protocol C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_file_transfer_protocol_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_file_transfer_protocol_t* file_transfer_protocol) +{ + return mavlink_msg_file_transfer_protocol_pack_status(system_id, component_id, _status, msg, file_transfer_protocol->target_network, file_transfer_protocol->target_system, file_transfer_protocol->target_component, file_transfer_protocol->payload); +} + /** * @brief Send a file_transfer_protocol message * @param chan MAVLink channel to send the message @@ -149,7 +203,7 @@ static inline uint16_t mavlink_msg_file_transfer_protocol_encode_chan(uint8_t sy * @param target_network Network ID (0 for broadcast) * @param target_system System ID (0 for broadcast) * @param target_component Component ID (0 for broadcast) - * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + * @param payload Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The content/format of this block is defined in https://mavlink.io/en/services/ftp.html. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -167,7 +221,7 @@ static inline void mavlink_msg_file_transfer_protocol_send(mavlink_channel_t cha packet.target_network = target_network; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*251); + mav_array_assign_uint8_t(packet.payload, payload, 251); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, (const char *)&packet, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_MIN_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_CRC); #endif } @@ -188,7 +242,7 @@ static inline void mavlink_msg_file_transfer_protocol_send_struct(mavlink_channe #if MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -208,7 +262,7 @@ static inline void mavlink_msg_file_transfer_protocol_send_buf(mavlink_message_t packet->target_network = target_network; packet->target_system = target_system; packet->target_component = target_component; - mav_array_memcpy(packet->payload, payload, sizeof(uint8_t)*251); + mav_array_assign_uint8_t(packet->payload, payload, 251); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_MIN_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_CRC); #endif } @@ -252,7 +306,7 @@ static inline uint8_t mavlink_msg_file_transfer_protocol_get_target_component(co /** * @brief Get field payload from file_transfer_protocol message * - * @return Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The entire content of this block is opaque unless you understand any the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the mavlink specification. + * @return Variable length payload. The length is defined by the remaining message length when subtracting the header and other fields. The content/format of this block is defined in https://mavlink.io/en/services/ftp.html. */ static inline uint16_t mavlink_msg_file_transfer_protocol_get_payload(const mavlink_message_t* msg, uint8_t *payload) { diff --git a/lib/main/MAVLink/common/mavlink_msg_flight_information.h b/lib/main/MAVLink/common/mavlink_msg_flight_information.h index 78513ff593b..57e5d385a59 100644 --- a/lib/main/MAVLink/common/mavlink_msg_flight_information.h +++ b/lib/main/MAVLink/common/mavlink_msg_flight_information.h @@ -5,15 +5,16 @@ typedef struct __mavlink_flight_information_t { - uint64_t arming_time_utc; /*< [us] Timestamp at arming (time since UNIX epoch) in UTC, 0 for unknown*/ - uint64_t takeoff_time_utc; /*< [us] Timestamp at takeoff (time since UNIX epoch) in UTC, 0 for unknown*/ - uint64_t flight_uuid; /*< Universally unique identifier (UUID) of flight, should correspond to name of log files*/ + uint64_t arming_time_utc; /*< [us] Timestamp at arming (since system boot). Set to 0 on boot. Set value on arming. Note, field is misnamed UTC.*/ + uint64_t takeoff_time_utc; /*< [us] Timestamp at takeoff (since system boot). Set to 0 at boot and on arming. Note, field is misnamed UTC.*/ + uint64_t flight_uuid; /*< Flight number. Note, field is misnamed UUID.*/ uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + uint32_t landing_time; /*< [ms] Timestamp at landing (in ms since system boot). Set to 0 at boot and on arming.*/ } mavlink_flight_information_t; -#define MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN 28 +#define MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN 32 #define MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN 28 -#define MAVLINK_MSG_ID_264_LEN 28 +#define MAVLINK_MSG_ID_264_LEN 32 #define MAVLINK_MSG_ID_264_MIN_LEN 28 #define MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC 49 @@ -25,21 +26,23 @@ typedef struct __mavlink_flight_information_t { #define MAVLINK_MESSAGE_INFO_FLIGHT_INFORMATION { \ 264, \ "FLIGHT_INFORMATION", \ - 4, \ + 5, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 24, offsetof(mavlink_flight_information_t, time_boot_ms) }, \ { "arming_time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_flight_information_t, arming_time_utc) }, \ { "takeoff_time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_flight_information_t, takeoff_time_utc) }, \ { "flight_uuid", NULL, MAVLINK_TYPE_UINT64_T, 0, 16, offsetof(mavlink_flight_information_t, flight_uuid) }, \ + { "landing_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 28, offsetof(mavlink_flight_information_t, landing_time) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_FLIGHT_INFORMATION { \ "FLIGHT_INFORMATION", \ - 4, \ + 5, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 24, offsetof(mavlink_flight_information_t, time_boot_ms) }, \ { "arming_time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_flight_information_t, arming_time_utc) }, \ { "takeoff_time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_flight_information_t, takeoff_time_utc) }, \ { "flight_uuid", NULL, MAVLINK_TYPE_UINT64_T, 0, 16, offsetof(mavlink_flight_information_t, flight_uuid) }, \ + { "landing_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 28, offsetof(mavlink_flight_information_t, landing_time) }, \ } \ } #endif @@ -51,13 +54,14 @@ typedef struct __mavlink_flight_information_t { * @param msg The MAVLink message to compress the data into * * @param time_boot_ms [ms] Timestamp (time since system boot). - * @param arming_time_utc [us] Timestamp at arming (time since UNIX epoch) in UTC, 0 for unknown - * @param takeoff_time_utc [us] Timestamp at takeoff (time since UNIX epoch) in UTC, 0 for unknown - * @param flight_uuid Universally unique identifier (UUID) of flight, should correspond to name of log files + * @param arming_time_utc [us] Timestamp at arming (since system boot). Set to 0 on boot. Set value on arming. Note, field is misnamed UTC. + * @param takeoff_time_utc [us] Timestamp at takeoff (since system boot). Set to 0 at boot and on arming. Note, field is misnamed UTC. + * @param flight_uuid Flight number. Note, field is misnamed UUID. + * @param landing_time [ms] Timestamp at landing (in ms since system boot). Set to 0 at boot and on arming. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_flight_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, uint64_t arming_time_utc, uint64_t takeoff_time_utc, uint64_t flight_uuid) + uint32_t time_boot_ms, uint64_t arming_time_utc, uint64_t takeoff_time_utc, uint64_t flight_uuid, uint32_t landing_time) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN]; @@ -65,6 +69,7 @@ static inline uint16_t mavlink_msg_flight_information_pack(uint8_t system_id, ui _mav_put_uint64_t(buf, 8, takeoff_time_utc); _mav_put_uint64_t(buf, 16, flight_uuid); _mav_put_uint32_t(buf, 24, time_boot_ms); + _mav_put_uint32_t(buf, 28, landing_time); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN); #else @@ -73,6 +78,7 @@ static inline uint16_t mavlink_msg_flight_information_pack(uint8_t system_id, ui packet.takeoff_time_utc = takeoff_time_utc; packet.flight_uuid = flight_uuid; packet.time_boot_ms = time_boot_ms; + packet.landing_time = landing_time; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN); #endif @@ -81,6 +87,51 @@ static inline uint16_t mavlink_msg_flight_information_pack(uint8_t system_id, ui return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); } +/** + * @brief Pack a flight_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param arming_time_utc [us] Timestamp at arming (since system boot). Set to 0 on boot. Set value on arming. Note, field is misnamed UTC. + * @param takeoff_time_utc [us] Timestamp at takeoff (since system boot). Set to 0 at boot and on arming. Note, field is misnamed UTC. + * @param flight_uuid Flight number. Note, field is misnamed UUID. + * @param landing_time [ms] Timestamp at landing (in ms since system boot). Set to 0 at boot and on arming. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_flight_information_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint64_t arming_time_utc, uint64_t takeoff_time_utc, uint64_t flight_uuid, uint32_t landing_time) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN]; + _mav_put_uint64_t(buf, 0, arming_time_utc); + _mav_put_uint64_t(buf, 8, takeoff_time_utc); + _mav_put_uint64_t(buf, 16, flight_uuid); + _mav_put_uint32_t(buf, 24, time_boot_ms); + _mav_put_uint32_t(buf, 28, landing_time); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN); +#else + mavlink_flight_information_t packet; + packet.arming_time_utc = arming_time_utc; + packet.takeoff_time_utc = takeoff_time_utc; + packet.flight_uuid = flight_uuid; + packet.time_boot_ms = time_boot_ms; + packet.landing_time = landing_time; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FLIGHT_INFORMATION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN); +#endif +} + /** * @brief Pack a flight_information message on a channel * @param system_id ID of this system @@ -88,14 +139,15 @@ static inline uint16_t mavlink_msg_flight_information_pack(uint8_t system_id, ui * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms [ms] Timestamp (time since system boot). - * @param arming_time_utc [us] Timestamp at arming (time since UNIX epoch) in UTC, 0 for unknown - * @param takeoff_time_utc [us] Timestamp at takeoff (time since UNIX epoch) in UTC, 0 for unknown - * @param flight_uuid Universally unique identifier (UUID) of flight, should correspond to name of log files + * @param arming_time_utc [us] Timestamp at arming (since system boot). Set to 0 on boot. Set value on arming. Note, field is misnamed UTC. + * @param takeoff_time_utc [us] Timestamp at takeoff (since system boot). Set to 0 at boot and on arming. Note, field is misnamed UTC. + * @param flight_uuid Flight number. Note, field is misnamed UUID. + * @param landing_time [ms] Timestamp at landing (in ms since system boot). Set to 0 at boot and on arming. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_flight_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,uint64_t arming_time_utc,uint64_t takeoff_time_utc,uint64_t flight_uuid) + uint32_t time_boot_ms,uint64_t arming_time_utc,uint64_t takeoff_time_utc,uint64_t flight_uuid,uint32_t landing_time) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN]; @@ -103,6 +155,7 @@ static inline uint16_t mavlink_msg_flight_information_pack_chan(uint8_t system_i _mav_put_uint64_t(buf, 8, takeoff_time_utc); _mav_put_uint64_t(buf, 16, flight_uuid); _mav_put_uint32_t(buf, 24, time_boot_ms); + _mav_put_uint32_t(buf, 28, landing_time); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN); #else @@ -111,6 +164,7 @@ static inline uint16_t mavlink_msg_flight_information_pack_chan(uint8_t system_i packet.takeoff_time_utc = takeoff_time_utc; packet.flight_uuid = flight_uuid; packet.time_boot_ms = time_boot_ms; + packet.landing_time = landing_time; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN); #endif @@ -129,7 +183,7 @@ static inline uint16_t mavlink_msg_flight_information_pack_chan(uint8_t system_i */ static inline uint16_t mavlink_msg_flight_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flight_information_t* flight_information) { - return mavlink_msg_flight_information_pack(system_id, component_id, msg, flight_information->time_boot_ms, flight_information->arming_time_utc, flight_information->takeoff_time_utc, flight_information->flight_uuid); + return mavlink_msg_flight_information_pack(system_id, component_id, msg, flight_information->time_boot_ms, flight_information->arming_time_utc, flight_information->takeoff_time_utc, flight_information->flight_uuid, flight_information->landing_time); } /** @@ -143,7 +197,21 @@ static inline uint16_t mavlink_msg_flight_information_encode(uint8_t system_id, */ static inline uint16_t mavlink_msg_flight_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flight_information_t* flight_information) { - return mavlink_msg_flight_information_pack_chan(system_id, component_id, chan, msg, flight_information->time_boot_ms, flight_information->arming_time_utc, flight_information->takeoff_time_utc, flight_information->flight_uuid); + return mavlink_msg_flight_information_pack_chan(system_id, component_id, chan, msg, flight_information->time_boot_ms, flight_information->arming_time_utc, flight_information->takeoff_time_utc, flight_information->flight_uuid, flight_information->landing_time); +} + +/** + * @brief Encode a flight_information struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param flight_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flight_information_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_flight_information_t* flight_information) +{ + return mavlink_msg_flight_information_pack_status(system_id, component_id, _status, msg, flight_information->time_boot_ms, flight_information->arming_time_utc, flight_information->takeoff_time_utc, flight_information->flight_uuid, flight_information->landing_time); } /** @@ -151,13 +219,14 @@ static inline uint16_t mavlink_msg_flight_information_encode_chan(uint8_t system * @param chan MAVLink channel to send the message * * @param time_boot_ms [ms] Timestamp (time since system boot). - * @param arming_time_utc [us] Timestamp at arming (time since UNIX epoch) in UTC, 0 for unknown - * @param takeoff_time_utc [us] Timestamp at takeoff (time since UNIX epoch) in UTC, 0 for unknown - * @param flight_uuid Universally unique identifier (UUID) of flight, should correspond to name of log files + * @param arming_time_utc [us] Timestamp at arming (since system boot). Set to 0 on boot. Set value on arming. Note, field is misnamed UTC. + * @param takeoff_time_utc [us] Timestamp at takeoff (since system boot). Set to 0 at boot and on arming. Note, field is misnamed UTC. + * @param flight_uuid Flight number. Note, field is misnamed UUID. + * @param landing_time [ms] Timestamp at landing (in ms since system boot). Set to 0 at boot and on arming. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_flight_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t arming_time_utc, uint64_t takeoff_time_utc, uint64_t flight_uuid) +static inline void mavlink_msg_flight_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t arming_time_utc, uint64_t takeoff_time_utc, uint64_t flight_uuid, uint32_t landing_time) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN]; @@ -165,6 +234,7 @@ static inline void mavlink_msg_flight_information_send(mavlink_channel_t chan, u _mav_put_uint64_t(buf, 8, takeoff_time_utc); _mav_put_uint64_t(buf, 16, flight_uuid); _mav_put_uint32_t(buf, 24, time_boot_ms); + _mav_put_uint32_t(buf, 28, landing_time); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLIGHT_INFORMATION, buf, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); #else @@ -173,6 +243,7 @@ static inline void mavlink_msg_flight_information_send(mavlink_channel_t chan, u packet.takeoff_time_utc = takeoff_time_utc; packet.flight_uuid = flight_uuid; packet.time_boot_ms = time_boot_ms; + packet.landing_time = landing_time; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLIGHT_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); #endif @@ -186,7 +257,7 @@ static inline void mavlink_msg_flight_information_send(mavlink_channel_t chan, u static inline void mavlink_msg_flight_information_send_struct(mavlink_channel_t chan, const mavlink_flight_information_t* flight_information) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_flight_information_send(chan, flight_information->time_boot_ms, flight_information->arming_time_utc, flight_information->takeoff_time_utc, flight_information->flight_uuid); + mavlink_msg_flight_information_send(chan, flight_information->time_boot_ms, flight_information->arming_time_utc, flight_information->takeoff_time_utc, flight_information->flight_uuid, flight_information->landing_time); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLIGHT_INFORMATION, (const char *)flight_information, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); #endif @@ -194,13 +265,13 @@ static inline void mavlink_msg_flight_information_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_flight_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t arming_time_utc, uint64_t takeoff_time_utc, uint64_t flight_uuid) +static inline void mavlink_msg_flight_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t arming_time_utc, uint64_t takeoff_time_utc, uint64_t flight_uuid, uint32_t landing_time) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,6 +279,7 @@ static inline void mavlink_msg_flight_information_send_buf(mavlink_message_t *ms _mav_put_uint64_t(buf, 8, takeoff_time_utc); _mav_put_uint64_t(buf, 16, flight_uuid); _mav_put_uint32_t(buf, 24, time_boot_ms); + _mav_put_uint32_t(buf, 28, landing_time); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLIGHT_INFORMATION, buf, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); #else @@ -216,6 +288,7 @@ static inline void mavlink_msg_flight_information_send_buf(mavlink_message_t *ms packet->takeoff_time_utc = takeoff_time_utc; packet->flight_uuid = flight_uuid; packet->time_boot_ms = time_boot_ms; + packet->landing_time = landing_time; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLIGHT_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); #endif @@ -240,7 +313,7 @@ static inline uint32_t mavlink_msg_flight_information_get_time_boot_ms(const mav /** * @brief Get field arming_time_utc from flight_information message * - * @return [us] Timestamp at arming (time since UNIX epoch) in UTC, 0 for unknown + * @return [us] Timestamp at arming (since system boot). Set to 0 on boot. Set value on arming. Note, field is misnamed UTC. */ static inline uint64_t mavlink_msg_flight_information_get_arming_time_utc(const mavlink_message_t* msg) { @@ -250,7 +323,7 @@ static inline uint64_t mavlink_msg_flight_information_get_arming_time_utc(const /** * @brief Get field takeoff_time_utc from flight_information message * - * @return [us] Timestamp at takeoff (time since UNIX epoch) in UTC, 0 for unknown + * @return [us] Timestamp at takeoff (since system boot). Set to 0 at boot and on arming. Note, field is misnamed UTC. */ static inline uint64_t mavlink_msg_flight_information_get_takeoff_time_utc(const mavlink_message_t* msg) { @@ -260,13 +333,23 @@ static inline uint64_t mavlink_msg_flight_information_get_takeoff_time_utc(const /** * @brief Get field flight_uuid from flight_information message * - * @return Universally unique identifier (UUID) of flight, should correspond to name of log files + * @return Flight number. Note, field is misnamed UUID. */ static inline uint64_t mavlink_msg_flight_information_get_flight_uuid(const mavlink_message_t* msg) { return _MAV_RETURN_uint64_t(msg, 16); } +/** + * @brief Get field landing_time from flight_information message + * + * @return [ms] Timestamp at landing (in ms since system boot). Set to 0 at boot and on arming. + */ +static inline uint32_t mavlink_msg_flight_information_get_landing_time(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 28); +} + /** * @brief Decode a flight_information message into a struct * @@ -280,6 +363,7 @@ static inline void mavlink_msg_flight_information_decode(const mavlink_message_t flight_information->takeoff_time_utc = mavlink_msg_flight_information_get_takeoff_time_utc(msg); flight_information->flight_uuid = mavlink_msg_flight_information_get_flight_uuid(msg); flight_information->time_boot_ms = mavlink_msg_flight_information_get_time_boot_ms(msg); + flight_information->landing_time = mavlink_msg_flight_information_get_landing_time(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN; memset(flight_information, 0, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_follow_target.h b/lib/main/MAVLink/common/mavlink_msg_follow_target.h index 01d03ab510f..c8a9dbb869d 100755 --- a/lib/main/MAVLink/common/mavlink_msg_follow_target.h +++ b/lib/main/MAVLink/common/mavlink_msg_follow_target.h @@ -12,7 +12,7 @@ typedef struct __mavlink_follow_target_t { float alt; /*< [m] Altitude (MSL)*/ float vel[3]; /*< [m/s] target velocity (0,0,0) for unknown*/ float acc[3]; /*< [m/s/s] linear target acceleration (0,0,0) for unknown*/ - float attitude_q[4]; /*< (1 0 0 0 for unknown)*/ + float attitude_q[4]; /*< (0 0 0 0 for unknown)*/ float rates[3]; /*< (0 0 0 for unknown)*/ float position_cov[3]; /*< eph epv*/ uint8_t est_capabilities; /*< bit positions for tracker reporting capabilities (POS = 0, VEL = 1, ACCEL = 2, ATT + RATES = 3)*/ @@ -82,7 +82,7 @@ typedef struct __mavlink_follow_target_t { * @param alt [m] Altitude (MSL) * @param vel [m/s] target velocity (0,0,0) for unknown * @param acc [m/s/s] linear target acceleration (0,0,0) for unknown - * @param attitude_q (1 0 0 0 for unknown) + * @param attitude_q (0 0 0 0 for unknown) * @param rates (0 0 0 for unknown) * @param position_cov eph epv * @param custom_state button states or switches of a tracker device @@ -91,6 +91,63 @@ typedef struct __mavlink_follow_target_t { static inline uint16_t mavlink_msg_follow_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t timestamp, uint8_t est_capabilities, int32_t lat, int32_t lon, float alt, const float *vel, const float *acc, const float *attitude_q, const float *rates, const float *position_cov, uint64_t custom_state) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FOLLOW_TARGET_LEN]; + _mav_put_uint64_t(buf, 0, timestamp); + _mav_put_uint64_t(buf, 8, custom_state); + _mav_put_int32_t(buf, 16, lat); + _mav_put_int32_t(buf, 20, lon); + _mav_put_float(buf, 24, alt); + _mav_put_uint8_t(buf, 92, est_capabilities); + _mav_put_float_array(buf, 28, vel, 3); + _mav_put_float_array(buf, 40, acc, 3); + _mav_put_float_array(buf, 52, attitude_q, 4); + _mav_put_float_array(buf, 68, rates, 3); + _mav_put_float_array(buf, 80, position_cov, 3); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FOLLOW_TARGET_LEN); +#else + mavlink_follow_target_t packet; + packet.timestamp = timestamp; + packet.custom_state = custom_state; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.est_capabilities = est_capabilities; + mav_array_assign_float(packet.vel, vel, 3); + mav_array_assign_float(packet.acc, acc, 3); + mav_array_assign_float(packet.attitude_q, attitude_q, 4); + mav_array_assign_float(packet.rates, rates, 3); + mav_array_assign_float(packet.position_cov, position_cov, 3); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FOLLOW_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FOLLOW_TARGET; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FOLLOW_TARGET_MIN_LEN, MAVLINK_MSG_ID_FOLLOW_TARGET_LEN, MAVLINK_MSG_ID_FOLLOW_TARGET_CRC); +} + +/** + * @brief Pack a follow_target message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param timestamp [ms] Timestamp (time since system boot). + * @param est_capabilities bit positions for tracker reporting capabilities (POS = 0, VEL = 1, ACCEL = 2, ATT + RATES = 3) + * @param lat [degE7] Latitude (WGS84) + * @param lon [degE7] Longitude (WGS84) + * @param alt [m] Altitude (MSL) + * @param vel [m/s] target velocity (0,0,0) for unknown + * @param acc [m/s/s] linear target acceleration (0,0,0) for unknown + * @param attitude_q (0 0 0 0 for unknown) + * @param rates (0 0 0 for unknown) + * @param position_cov eph epv + * @param custom_state button states or switches of a tracker device + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_follow_target_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t timestamp, uint8_t est_capabilities, int32_t lat, int32_t lon, float alt, const float *vel, const float *acc, const float *attitude_q, const float *rates, const float *position_cov, uint64_t custom_state) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_FOLLOW_TARGET_LEN]; _mav_put_uint64_t(buf, 0, timestamp); @@ -122,7 +179,11 @@ static inline uint16_t mavlink_msg_follow_target_pack(uint8_t system_id, uint8_t #endif msg->msgid = MAVLINK_MSG_ID_FOLLOW_TARGET; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FOLLOW_TARGET_MIN_LEN, MAVLINK_MSG_ID_FOLLOW_TARGET_LEN, MAVLINK_MSG_ID_FOLLOW_TARGET_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_FOLLOW_TARGET_MIN_LEN, MAVLINK_MSG_ID_FOLLOW_TARGET_LEN, MAVLINK_MSG_ID_FOLLOW_TARGET_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_FOLLOW_TARGET_MIN_LEN, MAVLINK_MSG_ID_FOLLOW_TARGET_LEN); +#endif } /** @@ -138,7 +199,7 @@ static inline uint16_t mavlink_msg_follow_target_pack(uint8_t system_id, uint8_t * @param alt [m] Altitude (MSL) * @param vel [m/s] target velocity (0,0,0) for unknown * @param acc [m/s/s] linear target acceleration (0,0,0) for unknown - * @param attitude_q (1 0 0 0 for unknown) + * @param attitude_q (0 0 0 0 for unknown) * @param rates (0 0 0 for unknown) * @param position_cov eph epv * @param custom_state button states or switches of a tracker device @@ -170,11 +231,11 @@ static inline uint16_t mavlink_msg_follow_target_pack_chan(uint8_t system_id, ui packet.lon = lon; packet.alt = alt; packet.est_capabilities = est_capabilities; - mav_array_memcpy(packet.vel, vel, sizeof(float)*3); - mav_array_memcpy(packet.acc, acc, sizeof(float)*3); - mav_array_memcpy(packet.attitude_q, attitude_q, sizeof(float)*4); - mav_array_memcpy(packet.rates, rates, sizeof(float)*3); - mav_array_memcpy(packet.position_cov, position_cov, sizeof(float)*3); + mav_array_assign_float(packet.vel, vel, 3); + mav_array_assign_float(packet.acc, acc, 3); + mav_array_assign_float(packet.attitude_q, attitude_q, 4); + mav_array_assign_float(packet.rates, rates, 3); + mav_array_assign_float(packet.position_cov, position_cov, 3); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FOLLOW_TARGET_LEN); #endif @@ -209,6 +270,20 @@ static inline uint16_t mavlink_msg_follow_target_encode_chan(uint8_t system_id, return mavlink_msg_follow_target_pack_chan(system_id, component_id, chan, msg, follow_target->timestamp, follow_target->est_capabilities, follow_target->lat, follow_target->lon, follow_target->alt, follow_target->vel, follow_target->acc, follow_target->attitude_q, follow_target->rates, follow_target->position_cov, follow_target->custom_state); } +/** + * @brief Encode a follow_target struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param follow_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_follow_target_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_follow_target_t* follow_target) +{ + return mavlink_msg_follow_target_pack_status(system_id, component_id, _status, msg, follow_target->timestamp, follow_target->est_capabilities, follow_target->lat, follow_target->lon, follow_target->alt, follow_target->vel, follow_target->acc, follow_target->attitude_q, follow_target->rates, follow_target->position_cov, follow_target->custom_state); +} + /** * @brief Send a follow_target message * @param chan MAVLink channel to send the message @@ -220,7 +295,7 @@ static inline uint16_t mavlink_msg_follow_target_encode_chan(uint8_t system_id, * @param alt [m] Altitude (MSL) * @param vel [m/s] target velocity (0,0,0) for unknown * @param acc [m/s/s] linear target acceleration (0,0,0) for unknown - * @param attitude_q (1 0 0 0 for unknown) + * @param attitude_q (0 0 0 0 for unknown) * @param rates (0 0 0 for unknown) * @param position_cov eph epv * @param custom_state button states or switches of a tracker device @@ -251,11 +326,11 @@ static inline void mavlink_msg_follow_target_send(mavlink_channel_t chan, uint64 packet.lon = lon; packet.alt = alt; packet.est_capabilities = est_capabilities; - mav_array_memcpy(packet.vel, vel, sizeof(float)*3); - mav_array_memcpy(packet.acc, acc, sizeof(float)*3); - mav_array_memcpy(packet.attitude_q, attitude_q, sizeof(float)*4); - mav_array_memcpy(packet.rates, rates, sizeof(float)*3); - mav_array_memcpy(packet.position_cov, position_cov, sizeof(float)*3); + mav_array_assign_float(packet.vel, vel, 3); + mav_array_assign_float(packet.acc, acc, 3); + mav_array_assign_float(packet.attitude_q, attitude_q, 4); + mav_array_assign_float(packet.rates, rates, 3); + mav_array_assign_float(packet.position_cov, position_cov, 3); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FOLLOW_TARGET, (const char *)&packet, MAVLINK_MSG_ID_FOLLOW_TARGET_MIN_LEN, MAVLINK_MSG_ID_FOLLOW_TARGET_LEN, MAVLINK_MSG_ID_FOLLOW_TARGET_CRC); #endif } @@ -276,7 +351,7 @@ static inline void mavlink_msg_follow_target_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_FOLLOW_TARGET_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -306,11 +381,11 @@ static inline void mavlink_msg_follow_target_send_buf(mavlink_message_t *msgbuf, packet->lon = lon; packet->alt = alt; packet->est_capabilities = est_capabilities; - mav_array_memcpy(packet->vel, vel, sizeof(float)*3); - mav_array_memcpy(packet->acc, acc, sizeof(float)*3); - mav_array_memcpy(packet->attitude_q, attitude_q, sizeof(float)*4); - mav_array_memcpy(packet->rates, rates, sizeof(float)*3); - mav_array_memcpy(packet->position_cov, position_cov, sizeof(float)*3); + mav_array_assign_float(packet->vel, vel, 3); + mav_array_assign_float(packet->acc, acc, 3); + mav_array_assign_float(packet->attitude_q, attitude_q, 4); + mav_array_assign_float(packet->rates, rates, 3); + mav_array_assign_float(packet->position_cov, position_cov, 3); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FOLLOW_TARGET, (const char *)packet, MAVLINK_MSG_ID_FOLLOW_TARGET_MIN_LEN, MAVLINK_MSG_ID_FOLLOW_TARGET_LEN, MAVLINK_MSG_ID_FOLLOW_TARGET_CRC); #endif } @@ -394,7 +469,7 @@ static inline uint16_t mavlink_msg_follow_target_get_acc(const mavlink_message_t /** * @brief Get field attitude_q from follow_target message * - * @return (1 0 0 0 for unknown) + * @return (0 0 0 0 for unknown) */ static inline uint16_t mavlink_msg_follow_target_get_attitude_q(const mavlink_message_t* msg, float *attitude_q) { diff --git a/lib/main/MAVLink/common/mavlink_msg_fuel_status.h b/lib/main/MAVLink/common/mavlink_msg_fuel_status.h new file mode 100644 index 00000000000..3ee8b9d798a --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_fuel_status.h @@ -0,0 +1,456 @@ +#pragma once +// MESSAGE FUEL_STATUS PACKING + +#define MAVLINK_MSG_ID_FUEL_STATUS 371 + + +typedef struct __mavlink_fuel_status_t { + float maximum_fuel; /*< Capacity when full. Must be provided.*/ + float consumed_fuel; /*< Consumed fuel (measured). This value should not be inferred: if not measured set to NaN. NaN: field not provided.*/ + float remaining_fuel; /*< Remaining fuel until empty (measured). The value should not be inferred: if not measured set to NaN. NaN: field not provided.*/ + float flow_rate; /*< Positive value when emptying/using, and negative if filling/replacing. NaN: field not provided.*/ + float temperature; /*< [K] Fuel temperature. NaN: field not provided.*/ + uint32_t fuel_type; /*< Fuel type. Defines units for fuel capacity and consumption fields above.*/ + uint8_t id; /*< Fuel ID. Must match ID of other messages for same fuel system, such as BATTERY_STATUS_V2.*/ + uint8_t percent_remaining; /*< [%] Percentage of remaining fuel, relative to full. Values: [0-100], UINT8_MAX: field not provided.*/ +} mavlink_fuel_status_t; + +#define MAVLINK_MSG_ID_FUEL_STATUS_LEN 26 +#define MAVLINK_MSG_ID_FUEL_STATUS_MIN_LEN 26 +#define MAVLINK_MSG_ID_371_LEN 26 +#define MAVLINK_MSG_ID_371_MIN_LEN 26 + +#define MAVLINK_MSG_ID_FUEL_STATUS_CRC 10 +#define MAVLINK_MSG_ID_371_CRC 10 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_FUEL_STATUS { \ + 371, \ + "FUEL_STATUS", \ + 8, \ + { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_fuel_status_t, id) }, \ + { "maximum_fuel", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_fuel_status_t, maximum_fuel) }, \ + { "consumed_fuel", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_fuel_status_t, consumed_fuel) }, \ + { "remaining_fuel", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_fuel_status_t, remaining_fuel) }, \ + { "percent_remaining", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_fuel_status_t, percent_remaining) }, \ + { "flow_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_fuel_status_t, flow_rate) }, \ + { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_fuel_status_t, temperature) }, \ + { "fuel_type", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_fuel_status_t, fuel_type) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_FUEL_STATUS { \ + "FUEL_STATUS", \ + 8, \ + { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_fuel_status_t, id) }, \ + { "maximum_fuel", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_fuel_status_t, maximum_fuel) }, \ + { "consumed_fuel", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_fuel_status_t, consumed_fuel) }, \ + { "remaining_fuel", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_fuel_status_t, remaining_fuel) }, \ + { "percent_remaining", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_fuel_status_t, percent_remaining) }, \ + { "flow_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_fuel_status_t, flow_rate) }, \ + { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_fuel_status_t, temperature) }, \ + { "fuel_type", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_fuel_status_t, fuel_type) }, \ + } \ +} +#endif + +/** + * @brief Pack a fuel_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param id Fuel ID. Must match ID of other messages for same fuel system, such as BATTERY_STATUS_V2. + * @param maximum_fuel Capacity when full. Must be provided. + * @param consumed_fuel Consumed fuel (measured). This value should not be inferred: if not measured set to NaN. NaN: field not provided. + * @param remaining_fuel Remaining fuel until empty (measured). The value should not be inferred: if not measured set to NaN. NaN: field not provided. + * @param percent_remaining [%] Percentage of remaining fuel, relative to full. Values: [0-100], UINT8_MAX: field not provided. + * @param flow_rate Positive value when emptying/using, and negative if filling/replacing. NaN: field not provided. + * @param temperature [K] Fuel temperature. NaN: field not provided. + * @param fuel_type Fuel type. Defines units for fuel capacity and consumption fields above. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_fuel_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t id, float maximum_fuel, float consumed_fuel, float remaining_fuel, uint8_t percent_remaining, float flow_rate, float temperature, uint32_t fuel_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FUEL_STATUS_LEN]; + _mav_put_float(buf, 0, maximum_fuel); + _mav_put_float(buf, 4, consumed_fuel); + _mav_put_float(buf, 8, remaining_fuel); + _mav_put_float(buf, 12, flow_rate); + _mav_put_float(buf, 16, temperature); + _mav_put_uint32_t(buf, 20, fuel_type); + _mav_put_uint8_t(buf, 24, id); + _mav_put_uint8_t(buf, 25, percent_remaining); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FUEL_STATUS_LEN); +#else + mavlink_fuel_status_t packet; + packet.maximum_fuel = maximum_fuel; + packet.consumed_fuel = consumed_fuel; + packet.remaining_fuel = remaining_fuel; + packet.flow_rate = flow_rate; + packet.temperature = temperature; + packet.fuel_type = fuel_type; + packet.id = id; + packet.percent_remaining = percent_remaining; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FUEL_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FUEL_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FUEL_STATUS_MIN_LEN, MAVLINK_MSG_ID_FUEL_STATUS_LEN, MAVLINK_MSG_ID_FUEL_STATUS_CRC); +} + +/** + * @brief Pack a fuel_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param id Fuel ID. Must match ID of other messages for same fuel system, such as BATTERY_STATUS_V2. + * @param maximum_fuel Capacity when full. Must be provided. + * @param consumed_fuel Consumed fuel (measured). This value should not be inferred: if not measured set to NaN. NaN: field not provided. + * @param remaining_fuel Remaining fuel until empty (measured). The value should not be inferred: if not measured set to NaN. NaN: field not provided. + * @param percent_remaining [%] Percentage of remaining fuel, relative to full. Values: [0-100], UINT8_MAX: field not provided. + * @param flow_rate Positive value when emptying/using, and negative if filling/replacing. NaN: field not provided. + * @param temperature [K] Fuel temperature. NaN: field not provided. + * @param fuel_type Fuel type. Defines units for fuel capacity and consumption fields above. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_fuel_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t id, float maximum_fuel, float consumed_fuel, float remaining_fuel, uint8_t percent_remaining, float flow_rate, float temperature, uint32_t fuel_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FUEL_STATUS_LEN]; + _mav_put_float(buf, 0, maximum_fuel); + _mav_put_float(buf, 4, consumed_fuel); + _mav_put_float(buf, 8, remaining_fuel); + _mav_put_float(buf, 12, flow_rate); + _mav_put_float(buf, 16, temperature); + _mav_put_uint32_t(buf, 20, fuel_type); + _mav_put_uint8_t(buf, 24, id); + _mav_put_uint8_t(buf, 25, percent_remaining); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FUEL_STATUS_LEN); +#else + mavlink_fuel_status_t packet; + packet.maximum_fuel = maximum_fuel; + packet.consumed_fuel = consumed_fuel; + packet.remaining_fuel = remaining_fuel; + packet.flow_rate = flow_rate; + packet.temperature = temperature; + packet.fuel_type = fuel_type; + packet.id = id; + packet.percent_remaining = percent_remaining; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FUEL_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FUEL_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_FUEL_STATUS_MIN_LEN, MAVLINK_MSG_ID_FUEL_STATUS_LEN, MAVLINK_MSG_ID_FUEL_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_FUEL_STATUS_MIN_LEN, MAVLINK_MSG_ID_FUEL_STATUS_LEN); +#endif +} + +/** + * @brief Pack a fuel_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param id Fuel ID. Must match ID of other messages for same fuel system, such as BATTERY_STATUS_V2. + * @param maximum_fuel Capacity when full. Must be provided. + * @param consumed_fuel Consumed fuel (measured). This value should not be inferred: if not measured set to NaN. NaN: field not provided. + * @param remaining_fuel Remaining fuel until empty (measured). The value should not be inferred: if not measured set to NaN. NaN: field not provided. + * @param percent_remaining [%] Percentage of remaining fuel, relative to full. Values: [0-100], UINT8_MAX: field not provided. + * @param flow_rate Positive value when emptying/using, and negative if filling/replacing. NaN: field not provided. + * @param temperature [K] Fuel temperature. NaN: field not provided. + * @param fuel_type Fuel type. Defines units for fuel capacity and consumption fields above. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_fuel_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t id,float maximum_fuel,float consumed_fuel,float remaining_fuel,uint8_t percent_remaining,float flow_rate,float temperature,uint32_t fuel_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FUEL_STATUS_LEN]; + _mav_put_float(buf, 0, maximum_fuel); + _mav_put_float(buf, 4, consumed_fuel); + _mav_put_float(buf, 8, remaining_fuel); + _mav_put_float(buf, 12, flow_rate); + _mav_put_float(buf, 16, temperature); + _mav_put_uint32_t(buf, 20, fuel_type); + _mav_put_uint8_t(buf, 24, id); + _mav_put_uint8_t(buf, 25, percent_remaining); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FUEL_STATUS_LEN); +#else + mavlink_fuel_status_t packet; + packet.maximum_fuel = maximum_fuel; + packet.consumed_fuel = consumed_fuel; + packet.remaining_fuel = remaining_fuel; + packet.flow_rate = flow_rate; + packet.temperature = temperature; + packet.fuel_type = fuel_type; + packet.id = id; + packet.percent_remaining = percent_remaining; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FUEL_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FUEL_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FUEL_STATUS_MIN_LEN, MAVLINK_MSG_ID_FUEL_STATUS_LEN, MAVLINK_MSG_ID_FUEL_STATUS_CRC); +} + +/** + * @brief Encode a fuel_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param fuel_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fuel_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fuel_status_t* fuel_status) +{ + return mavlink_msg_fuel_status_pack(system_id, component_id, msg, fuel_status->id, fuel_status->maximum_fuel, fuel_status->consumed_fuel, fuel_status->remaining_fuel, fuel_status->percent_remaining, fuel_status->flow_rate, fuel_status->temperature, fuel_status->fuel_type); +} + +/** + * @brief Encode a fuel_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param fuel_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fuel_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fuel_status_t* fuel_status) +{ + return mavlink_msg_fuel_status_pack_chan(system_id, component_id, chan, msg, fuel_status->id, fuel_status->maximum_fuel, fuel_status->consumed_fuel, fuel_status->remaining_fuel, fuel_status->percent_remaining, fuel_status->flow_rate, fuel_status->temperature, fuel_status->fuel_type); +} + +/** + * @brief Encode a fuel_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param fuel_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fuel_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_fuel_status_t* fuel_status) +{ + return mavlink_msg_fuel_status_pack_status(system_id, component_id, _status, msg, fuel_status->id, fuel_status->maximum_fuel, fuel_status->consumed_fuel, fuel_status->remaining_fuel, fuel_status->percent_remaining, fuel_status->flow_rate, fuel_status->temperature, fuel_status->fuel_type); +} + +/** + * @brief Send a fuel_status message + * @param chan MAVLink channel to send the message + * + * @param id Fuel ID. Must match ID of other messages for same fuel system, such as BATTERY_STATUS_V2. + * @param maximum_fuel Capacity when full. Must be provided. + * @param consumed_fuel Consumed fuel (measured). This value should not be inferred: if not measured set to NaN. NaN: field not provided. + * @param remaining_fuel Remaining fuel until empty (measured). The value should not be inferred: if not measured set to NaN. NaN: field not provided. + * @param percent_remaining [%] Percentage of remaining fuel, relative to full. Values: [0-100], UINT8_MAX: field not provided. + * @param flow_rate Positive value when emptying/using, and negative if filling/replacing. NaN: field not provided. + * @param temperature [K] Fuel temperature. NaN: field not provided. + * @param fuel_type Fuel type. Defines units for fuel capacity and consumption fields above. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_fuel_status_send(mavlink_channel_t chan, uint8_t id, float maximum_fuel, float consumed_fuel, float remaining_fuel, uint8_t percent_remaining, float flow_rate, float temperature, uint32_t fuel_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FUEL_STATUS_LEN]; + _mav_put_float(buf, 0, maximum_fuel); + _mav_put_float(buf, 4, consumed_fuel); + _mav_put_float(buf, 8, remaining_fuel); + _mav_put_float(buf, 12, flow_rate); + _mav_put_float(buf, 16, temperature); + _mav_put_uint32_t(buf, 20, fuel_type); + _mav_put_uint8_t(buf, 24, id); + _mav_put_uint8_t(buf, 25, percent_remaining); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FUEL_STATUS, buf, MAVLINK_MSG_ID_FUEL_STATUS_MIN_LEN, MAVLINK_MSG_ID_FUEL_STATUS_LEN, MAVLINK_MSG_ID_FUEL_STATUS_CRC); +#else + mavlink_fuel_status_t packet; + packet.maximum_fuel = maximum_fuel; + packet.consumed_fuel = consumed_fuel; + packet.remaining_fuel = remaining_fuel; + packet.flow_rate = flow_rate; + packet.temperature = temperature; + packet.fuel_type = fuel_type; + packet.id = id; + packet.percent_remaining = percent_remaining; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FUEL_STATUS, (const char *)&packet, MAVLINK_MSG_ID_FUEL_STATUS_MIN_LEN, MAVLINK_MSG_ID_FUEL_STATUS_LEN, MAVLINK_MSG_ID_FUEL_STATUS_CRC); +#endif +} + +/** + * @brief Send a fuel_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_fuel_status_send_struct(mavlink_channel_t chan, const mavlink_fuel_status_t* fuel_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_fuel_status_send(chan, fuel_status->id, fuel_status->maximum_fuel, fuel_status->consumed_fuel, fuel_status->remaining_fuel, fuel_status->percent_remaining, fuel_status->flow_rate, fuel_status->temperature, fuel_status->fuel_type); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FUEL_STATUS, (const char *)fuel_status, MAVLINK_MSG_ID_FUEL_STATUS_MIN_LEN, MAVLINK_MSG_ID_FUEL_STATUS_LEN, MAVLINK_MSG_ID_FUEL_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_FUEL_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_fuel_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t id, float maximum_fuel, float consumed_fuel, float remaining_fuel, uint8_t percent_remaining, float flow_rate, float temperature, uint32_t fuel_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, maximum_fuel); + _mav_put_float(buf, 4, consumed_fuel); + _mav_put_float(buf, 8, remaining_fuel); + _mav_put_float(buf, 12, flow_rate); + _mav_put_float(buf, 16, temperature); + _mav_put_uint32_t(buf, 20, fuel_type); + _mav_put_uint8_t(buf, 24, id); + _mav_put_uint8_t(buf, 25, percent_remaining); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FUEL_STATUS, buf, MAVLINK_MSG_ID_FUEL_STATUS_MIN_LEN, MAVLINK_MSG_ID_FUEL_STATUS_LEN, MAVLINK_MSG_ID_FUEL_STATUS_CRC); +#else + mavlink_fuel_status_t *packet = (mavlink_fuel_status_t *)msgbuf; + packet->maximum_fuel = maximum_fuel; + packet->consumed_fuel = consumed_fuel; + packet->remaining_fuel = remaining_fuel; + packet->flow_rate = flow_rate; + packet->temperature = temperature; + packet->fuel_type = fuel_type; + packet->id = id; + packet->percent_remaining = percent_remaining; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FUEL_STATUS, (const char *)packet, MAVLINK_MSG_ID_FUEL_STATUS_MIN_LEN, MAVLINK_MSG_ID_FUEL_STATUS_LEN, MAVLINK_MSG_ID_FUEL_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE FUEL_STATUS UNPACKING + + +/** + * @brief Get field id from fuel_status message + * + * @return Fuel ID. Must match ID of other messages for same fuel system, such as BATTERY_STATUS_V2. + */ +static inline uint8_t mavlink_msg_fuel_status_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 24); +} + +/** + * @brief Get field maximum_fuel from fuel_status message + * + * @return Capacity when full. Must be provided. + */ +static inline float mavlink_msg_fuel_status_get_maximum_fuel(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field consumed_fuel from fuel_status message + * + * @return Consumed fuel (measured). This value should not be inferred: if not measured set to NaN. NaN: field not provided. + */ +static inline float mavlink_msg_fuel_status_get_consumed_fuel(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field remaining_fuel from fuel_status message + * + * @return Remaining fuel until empty (measured). The value should not be inferred: if not measured set to NaN. NaN: field not provided. + */ +static inline float mavlink_msg_fuel_status_get_remaining_fuel(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field percent_remaining from fuel_status message + * + * @return [%] Percentage of remaining fuel, relative to full. Values: [0-100], UINT8_MAX: field not provided. + */ +static inline uint8_t mavlink_msg_fuel_status_get_percent_remaining(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 25); +} + +/** + * @brief Get field flow_rate from fuel_status message + * + * @return Positive value when emptying/using, and negative if filling/replacing. NaN: field not provided. + */ +static inline float mavlink_msg_fuel_status_get_flow_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field temperature from fuel_status message + * + * @return [K] Fuel temperature. NaN: field not provided. + */ +static inline float mavlink_msg_fuel_status_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field fuel_type from fuel_status message + * + * @return Fuel type. Defines units for fuel capacity and consumption fields above. + */ +static inline uint32_t mavlink_msg_fuel_status_get_fuel_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 20); +} + +/** + * @brief Decode a fuel_status message into a struct + * + * @param msg The message to decode + * @param fuel_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_fuel_status_decode(const mavlink_message_t* msg, mavlink_fuel_status_t* fuel_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + fuel_status->maximum_fuel = mavlink_msg_fuel_status_get_maximum_fuel(msg); + fuel_status->consumed_fuel = mavlink_msg_fuel_status_get_consumed_fuel(msg); + fuel_status->remaining_fuel = mavlink_msg_fuel_status_get_remaining_fuel(msg); + fuel_status->flow_rate = mavlink_msg_fuel_status_get_flow_rate(msg); + fuel_status->temperature = mavlink_msg_fuel_status_get_temperature(msg); + fuel_status->fuel_type = mavlink_msg_fuel_status_get_fuel_type(msg); + fuel_status->id = mavlink_msg_fuel_status_get_id(msg); + fuel_status->percent_remaining = mavlink_msg_fuel_status_get_percent_remaining(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_FUEL_STATUS_LEN? msg->len : MAVLINK_MSG_ID_FUEL_STATUS_LEN; + memset(fuel_status, 0, MAVLINK_MSG_ID_FUEL_STATUS_LEN); + memcpy(fuel_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_generator_status.h b/lib/main/MAVLink/common/mavlink_msg_generator_status.h index ef960e13cf9..050f431b2b9 100644 --- a/lib/main/MAVLink/common/mavlink_msg_generator_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_generator_status.h @@ -123,6 +123,69 @@ static inline uint16_t mavlink_msg_generator_status_pack(uint8_t system_id, uint return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_CRC); } +/** + * @brief Pack a generator_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param status Status flags. + * @param generator_speed [rpm] Speed of electrical generator or alternator. UINT16_MAX: field not provided. + * @param battery_current [A] Current into/out of battery. Positive for out. Negative for in. NaN: field not provided. + * @param load_current [A] Current going to the UAV. If battery current not available this is the DC current from the generator. Positive for out. Negative for in. NaN: field not provided + * @param power_generated [W] The power being generated. NaN: field not provided + * @param bus_voltage [V] Voltage of the bus seen at the generator, or battery bus if battery bus is controlled by generator and at a different voltage to main bus. + * @param rectifier_temperature [degC] The temperature of the rectifier or power converter. INT16_MAX: field not provided. + * @param bat_current_setpoint [A] The target battery current. Positive for out. Negative for in. NaN: field not provided + * @param generator_temperature [degC] The temperature of the mechanical motor, fuel cell core or generator. INT16_MAX: field not provided. + * @param runtime [s] Seconds this generator has run since it was rebooted. UINT32_MAX: field not provided. + * @param time_until_maintenance [s] Seconds until this generator requires maintenance. A negative value indicates maintenance is past-due. INT32_MAX: field not provided. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_generator_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t status, uint16_t generator_speed, float battery_current, float load_current, float power_generated, float bus_voltage, int16_t rectifier_temperature, float bat_current_setpoint, int16_t generator_temperature, uint32_t runtime, int32_t time_until_maintenance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GENERATOR_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, status); + _mav_put_float(buf, 8, battery_current); + _mav_put_float(buf, 12, load_current); + _mav_put_float(buf, 16, power_generated); + _mav_put_float(buf, 20, bus_voltage); + _mav_put_float(buf, 24, bat_current_setpoint); + _mav_put_uint32_t(buf, 28, runtime); + _mav_put_int32_t(buf, 32, time_until_maintenance); + _mav_put_uint16_t(buf, 36, generator_speed); + _mav_put_int16_t(buf, 38, rectifier_temperature); + _mav_put_int16_t(buf, 40, generator_temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN); +#else + mavlink_generator_status_t packet; + packet.status = status; + packet.battery_current = battery_current; + packet.load_current = load_current; + packet.power_generated = power_generated; + packet.bus_voltage = bus_voltage; + packet.bat_current_setpoint = bat_current_setpoint; + packet.runtime = runtime; + packet.time_until_maintenance = time_until_maintenance; + packet.generator_speed = generator_speed; + packet.rectifier_temperature = rectifier_temperature; + packet.generator_temperature = generator_temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GENERATOR_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN); +#endif +} + /** * @brief Pack a generator_status message on a channel * @param system_id ID of this system @@ -209,6 +272,20 @@ static inline uint16_t mavlink_msg_generator_status_encode_chan(uint8_t system_i return mavlink_msg_generator_status_pack_chan(system_id, component_id, chan, msg, generator_status->status, generator_status->generator_speed, generator_status->battery_current, generator_status->load_current, generator_status->power_generated, generator_status->bus_voltage, generator_status->rectifier_temperature, generator_status->bat_current_setpoint, generator_status->generator_temperature, generator_status->runtime, generator_status->time_until_maintenance); } +/** + * @brief Encode a generator_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param generator_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_generator_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_generator_status_t* generator_status) +{ + return mavlink_msg_generator_status_pack_status(system_id, component_id, _status, msg, generator_status->status, generator_status->generator_speed, generator_status->battery_current, generator_status->load_current, generator_status->power_generated, generator_status->bus_voltage, generator_status->rectifier_temperature, generator_status->bat_current_setpoint, generator_status->generator_temperature, generator_status->runtime, generator_status->time_until_maintenance); +} + /** * @brief Send a generator_status message * @param chan MAVLink channel to send the message @@ -278,7 +355,7 @@ static inline void mavlink_msg_generator_status_send_struct(mavlink_channel_t ch #if MAVLINK_MSG_ID_GENERATOR_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_device_attitude_status.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_device_attitude_status.h index 63275480b4e..85aa87f30cc 100644 --- a/lib/main/MAVLink/common/mavlink_msg_gimbal_device_attitude_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_device_attitude_status.h @@ -6,19 +6,22 @@ typedef struct __mavlink_gimbal_device_attitude_status_t { uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ - float q[4]; /*< Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set)*/ - float angular_velocity_x; /*< [rad/s] X component of angular velocity (NaN if unknown)*/ - float angular_velocity_y; /*< [rad/s] Y component of angular velocity (NaN if unknown)*/ - float angular_velocity_z; /*< [rad/s] Z component of angular velocity (NaN if unknown)*/ + float q[4]; /*< Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description.*/ + float angular_velocity_x; /*< [rad/s] X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN if unknown.*/ + float angular_velocity_y; /*< [rad/s] Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN if unknown.*/ + float angular_velocity_z; /*< [rad/s] Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN if unknown.*/ uint32_t failure_flags; /*< Failure flags (0 for no failure)*/ uint16_t flags; /*< Current gimbal flags set.*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ + float delta_yaw; /*< [rad] Yaw angle relating the quaternions in earth and body frames (see message description). NaN if unknown.*/ + float delta_yaw_velocity; /*< [rad/s] Yaw angular velocity relating the angular velocities in earth and body frames (see message description). NaN if unknown.*/ + uint8_t gimbal_device_id; /*< This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set a number between 1-6. If the component ID is separate, this field is not required and must be set to 0.*/ } mavlink_gimbal_device_attitude_status_t; -#define MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN 40 +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN 49 #define MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN 40 -#define MAVLINK_MSG_ID_285_LEN 40 +#define MAVLINK_MSG_ID_285_LEN 49 #define MAVLINK_MSG_ID_285_MIN_LEN 40 #define MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC 137 @@ -30,7 +33,7 @@ typedef struct __mavlink_gimbal_device_attitude_status_t { #define MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_ATTITUDE_STATUS { \ 285, \ "GIMBAL_DEVICE_ATTITUDE_STATUS", \ - 9, \ + 12, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 38, offsetof(mavlink_gimbal_device_attitude_status_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 39, offsetof(mavlink_gimbal_device_attitude_status_t, target_component) }, \ { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_device_attitude_status_t, time_boot_ms) }, \ @@ -40,12 +43,15 @@ typedef struct __mavlink_gimbal_device_attitude_status_t { { "angular_velocity_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_y) }, \ { "angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_z) }, \ { "failure_flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 32, offsetof(mavlink_gimbal_device_attitude_status_t, failure_flags) }, \ + { "delta_yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_gimbal_device_attitude_status_t, delta_yaw) }, \ + { "delta_yaw_velocity", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_gimbal_device_attitude_status_t, delta_yaw_velocity) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_gimbal_device_attitude_status_t, gimbal_device_id) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_ATTITUDE_STATUS { \ "GIMBAL_DEVICE_ATTITUDE_STATUS", \ - 9, \ + 12, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 38, offsetof(mavlink_gimbal_device_attitude_status_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 39, offsetof(mavlink_gimbal_device_attitude_status_t, target_component) }, \ { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_device_attitude_status_t, time_boot_ms) }, \ @@ -55,6 +61,9 @@ typedef struct __mavlink_gimbal_device_attitude_status_t { { "angular_velocity_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_y) }, \ { "angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_z) }, \ { "failure_flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 32, offsetof(mavlink_gimbal_device_attitude_status_t, failure_flags) }, \ + { "delta_yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_gimbal_device_attitude_status_t, delta_yaw) }, \ + { "delta_yaw_velocity", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_gimbal_device_attitude_status_t, delta_yaw_velocity) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_gimbal_device_attitude_status_t, gimbal_device_id) }, \ } \ } #endif @@ -69,15 +78,18 @@ typedef struct __mavlink_gimbal_device_attitude_status_t { * @param target_component Component ID * @param time_boot_ms [ms] Timestamp (time since system boot). * @param flags Current gimbal flags set. - * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) - * @param angular_velocity_x [rad/s] X component of angular velocity (NaN if unknown) - * @param angular_velocity_y [rad/s] Y component of angular velocity (NaN if unknown) - * @param angular_velocity_z [rad/s] Z component of angular velocity (NaN if unknown) + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description. + * @param angular_velocity_x [rad/s] X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN if unknown. + * @param angular_velocity_y [rad/s] Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN if unknown. + * @param angular_velocity_z [rad/s] Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN if unknown. * @param failure_flags Failure flags (0 for no failure) + * @param delta_yaw [rad] Yaw angle relating the quaternions in earth and body frames (see message description). NaN if unknown. + * @param delta_yaw_velocity [rad/s] Yaw angular velocity relating the angular velocities in earth and body frames (see message description). NaN if unknown. + * @param gimbal_device_id This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set a number between 1-6. If the component ID is separate, this field is not required and must be set to 0. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gimbal_device_attitude_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint32_t time_boot_ms, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z, uint32_t failure_flags) + uint8_t target_system, uint8_t target_component, uint32_t time_boot_ms, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z, uint32_t failure_flags, float delta_yaw, float delta_yaw_velocity, uint8_t gimbal_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN]; @@ -89,6 +101,9 @@ static inline uint16_t mavlink_msg_gimbal_device_attitude_status_pack(uint8_t sy _mav_put_uint16_t(buf, 36, flags); _mav_put_uint8_t(buf, 38, target_system); _mav_put_uint8_t(buf, 39, target_component); + _mav_put_float(buf, 40, delta_yaw); + _mav_put_float(buf, 44, delta_yaw_velocity); + _mav_put_uint8_t(buf, 48, gimbal_device_id); _mav_put_float_array(buf, 4, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN); #else @@ -101,7 +116,10 @@ static inline uint16_t mavlink_msg_gimbal_device_attitude_status_pack(uint8_t sy packet.flags = flags; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + packet.delta_yaw = delta_yaw; + packet.delta_yaw_velocity = delta_yaw_velocity; + packet.gimbal_device_id = gimbal_device_id; + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN); #endif @@ -109,6 +127,70 @@ static inline uint16_t mavlink_msg_gimbal_device_attitude_status_pack(uint8_t sy return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); } +/** + * @brief Pack a gimbal_device_attitude_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param flags Current gimbal flags set. + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description. + * @param angular_velocity_x [rad/s] X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN if unknown. + * @param angular_velocity_y [rad/s] Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN if unknown. + * @param angular_velocity_z [rad/s] Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN if unknown. + * @param failure_flags Failure flags (0 for no failure) + * @param delta_yaw [rad] Yaw angle relating the quaternions in earth and body frames (see message description). NaN if unknown. + * @param delta_yaw_velocity [rad/s] Yaw angular velocity relating the angular velocities in earth and body frames (see message description). NaN if unknown. + * @param gimbal_device_id This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set a number between 1-6. If the component ID is separate, this field is not required and must be set to 0. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_device_attitude_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t time_boot_ms, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z, uint32_t failure_flags, float delta_yaw, float delta_yaw_velocity, uint8_t gimbal_device_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, angular_velocity_x); + _mav_put_float(buf, 24, angular_velocity_y); + _mav_put_float(buf, 28, angular_velocity_z); + _mav_put_uint32_t(buf, 32, failure_flags); + _mav_put_uint16_t(buf, 36, flags); + _mav_put_uint8_t(buf, 38, target_system); + _mav_put_uint8_t(buf, 39, target_component); + _mav_put_float(buf, 40, delta_yaw); + _mav_put_float(buf, 44, delta_yaw_velocity); + _mav_put_uint8_t(buf, 48, gimbal_device_id); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN); +#else + mavlink_gimbal_device_attitude_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.failure_flags = failure_flags; + packet.flags = flags; + packet.target_system = target_system; + packet.target_component = target_component; + packet.delta_yaw = delta_yaw; + packet.delta_yaw_velocity = delta_yaw_velocity; + packet.gimbal_device_id = gimbal_device_id; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN); +#endif +} + /** * @brief Pack a gimbal_device_attitude_status message on a channel * @param system_id ID of this system @@ -119,16 +201,19 @@ static inline uint16_t mavlink_msg_gimbal_device_attitude_status_pack(uint8_t sy * @param target_component Component ID * @param time_boot_ms [ms] Timestamp (time since system boot). * @param flags Current gimbal flags set. - * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) - * @param angular_velocity_x [rad/s] X component of angular velocity (NaN if unknown) - * @param angular_velocity_y [rad/s] Y component of angular velocity (NaN if unknown) - * @param angular_velocity_z [rad/s] Z component of angular velocity (NaN if unknown) + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description. + * @param angular_velocity_x [rad/s] X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN if unknown. + * @param angular_velocity_y [rad/s] Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN if unknown. + * @param angular_velocity_z [rad/s] Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN if unknown. * @param failure_flags Failure flags (0 for no failure) + * @param delta_yaw [rad] Yaw angle relating the quaternions in earth and body frames (see message description). NaN if unknown. + * @param delta_yaw_velocity [rad/s] Yaw angular velocity relating the angular velocities in earth and body frames (see message description). NaN if unknown. + * @param gimbal_device_id This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set a number between 1-6. If the component ID is separate, this field is not required and must be set to 0. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gimbal_device_attitude_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint32_t time_boot_ms,uint16_t flags,const float *q,float angular_velocity_x,float angular_velocity_y,float angular_velocity_z,uint32_t failure_flags) + uint8_t target_system,uint8_t target_component,uint32_t time_boot_ms,uint16_t flags,const float *q,float angular_velocity_x,float angular_velocity_y,float angular_velocity_z,uint32_t failure_flags,float delta_yaw,float delta_yaw_velocity,uint8_t gimbal_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN]; @@ -140,6 +225,9 @@ static inline uint16_t mavlink_msg_gimbal_device_attitude_status_pack_chan(uint8 _mav_put_uint16_t(buf, 36, flags); _mav_put_uint8_t(buf, 38, target_system); _mav_put_uint8_t(buf, 39, target_component); + _mav_put_float(buf, 40, delta_yaw); + _mav_put_float(buf, 44, delta_yaw_velocity); + _mav_put_uint8_t(buf, 48, gimbal_device_id); _mav_put_float_array(buf, 4, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN); #else @@ -152,7 +240,10 @@ static inline uint16_t mavlink_msg_gimbal_device_attitude_status_pack_chan(uint8 packet.flags = flags; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + packet.delta_yaw = delta_yaw; + packet.delta_yaw_velocity = delta_yaw_velocity; + packet.gimbal_device_id = gimbal_device_id; + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN); #endif @@ -170,7 +261,7 @@ static inline uint16_t mavlink_msg_gimbal_device_attitude_status_pack_chan(uint8 */ static inline uint16_t mavlink_msg_gimbal_device_attitude_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_device_attitude_status_t* gimbal_device_attitude_status) { - return mavlink_msg_gimbal_device_attitude_status_pack(system_id, component_id, msg, gimbal_device_attitude_status->target_system, gimbal_device_attitude_status->target_component, gimbal_device_attitude_status->time_boot_ms, gimbal_device_attitude_status->flags, gimbal_device_attitude_status->q, gimbal_device_attitude_status->angular_velocity_x, gimbal_device_attitude_status->angular_velocity_y, gimbal_device_attitude_status->angular_velocity_z, gimbal_device_attitude_status->failure_flags); + return mavlink_msg_gimbal_device_attitude_status_pack(system_id, component_id, msg, gimbal_device_attitude_status->target_system, gimbal_device_attitude_status->target_component, gimbal_device_attitude_status->time_boot_ms, gimbal_device_attitude_status->flags, gimbal_device_attitude_status->q, gimbal_device_attitude_status->angular_velocity_x, gimbal_device_attitude_status->angular_velocity_y, gimbal_device_attitude_status->angular_velocity_z, gimbal_device_attitude_status->failure_flags, gimbal_device_attitude_status->delta_yaw, gimbal_device_attitude_status->delta_yaw_velocity, gimbal_device_attitude_status->gimbal_device_id); } /** @@ -184,7 +275,21 @@ static inline uint16_t mavlink_msg_gimbal_device_attitude_status_encode(uint8_t */ static inline uint16_t mavlink_msg_gimbal_device_attitude_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_device_attitude_status_t* gimbal_device_attitude_status) { - return mavlink_msg_gimbal_device_attitude_status_pack_chan(system_id, component_id, chan, msg, gimbal_device_attitude_status->target_system, gimbal_device_attitude_status->target_component, gimbal_device_attitude_status->time_boot_ms, gimbal_device_attitude_status->flags, gimbal_device_attitude_status->q, gimbal_device_attitude_status->angular_velocity_x, gimbal_device_attitude_status->angular_velocity_y, gimbal_device_attitude_status->angular_velocity_z, gimbal_device_attitude_status->failure_flags); + return mavlink_msg_gimbal_device_attitude_status_pack_chan(system_id, component_id, chan, msg, gimbal_device_attitude_status->target_system, gimbal_device_attitude_status->target_component, gimbal_device_attitude_status->time_boot_ms, gimbal_device_attitude_status->flags, gimbal_device_attitude_status->q, gimbal_device_attitude_status->angular_velocity_x, gimbal_device_attitude_status->angular_velocity_y, gimbal_device_attitude_status->angular_velocity_z, gimbal_device_attitude_status->failure_flags, gimbal_device_attitude_status->delta_yaw, gimbal_device_attitude_status->delta_yaw_velocity, gimbal_device_attitude_status->gimbal_device_id); +} + +/** + * @brief Encode a gimbal_device_attitude_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param gimbal_device_attitude_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_device_attitude_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_gimbal_device_attitude_status_t* gimbal_device_attitude_status) +{ + return mavlink_msg_gimbal_device_attitude_status_pack_status(system_id, component_id, _status, msg, gimbal_device_attitude_status->target_system, gimbal_device_attitude_status->target_component, gimbal_device_attitude_status->time_boot_ms, gimbal_device_attitude_status->flags, gimbal_device_attitude_status->q, gimbal_device_attitude_status->angular_velocity_x, gimbal_device_attitude_status->angular_velocity_y, gimbal_device_attitude_status->angular_velocity_z, gimbal_device_attitude_status->failure_flags, gimbal_device_attitude_status->delta_yaw, gimbal_device_attitude_status->delta_yaw_velocity, gimbal_device_attitude_status->gimbal_device_id); } /** @@ -195,15 +300,18 @@ static inline uint16_t mavlink_msg_gimbal_device_attitude_status_encode_chan(uin * @param target_component Component ID * @param time_boot_ms [ms] Timestamp (time since system boot). * @param flags Current gimbal flags set. - * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) - * @param angular_velocity_x [rad/s] X component of angular velocity (NaN if unknown) - * @param angular_velocity_y [rad/s] Y component of angular velocity (NaN if unknown) - * @param angular_velocity_z [rad/s] Z component of angular velocity (NaN if unknown) + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description. + * @param angular_velocity_x [rad/s] X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN if unknown. + * @param angular_velocity_y [rad/s] Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN if unknown. + * @param angular_velocity_z [rad/s] Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN if unknown. * @param failure_flags Failure flags (0 for no failure) + * @param delta_yaw [rad] Yaw angle relating the quaternions in earth and body frames (see message description). NaN if unknown. + * @param delta_yaw_velocity [rad/s] Yaw angular velocity relating the angular velocities in earth and body frames (see message description). NaN if unknown. + * @param gimbal_device_id This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set a number between 1-6. If the component ID is separate, this field is not required and must be set to 0. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gimbal_device_attitude_status_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t time_boot_ms, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z, uint32_t failure_flags) +static inline void mavlink_msg_gimbal_device_attitude_status_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t time_boot_ms, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z, uint32_t failure_flags, float delta_yaw, float delta_yaw_velocity, uint8_t gimbal_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN]; @@ -215,6 +323,9 @@ static inline void mavlink_msg_gimbal_device_attitude_status_send(mavlink_channe _mav_put_uint16_t(buf, 36, flags); _mav_put_uint8_t(buf, 38, target_system); _mav_put_uint8_t(buf, 39, target_component); + _mav_put_float(buf, 40, delta_yaw); + _mav_put_float(buf, 44, delta_yaw_velocity); + _mav_put_uint8_t(buf, 48, gimbal_device_id); _mav_put_float_array(buf, 4, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); #else @@ -227,7 +338,10 @@ static inline void mavlink_msg_gimbal_device_attitude_status_send(mavlink_channe packet.flags = flags; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + packet.delta_yaw = delta_yaw; + packet.delta_yaw_velocity = delta_yaw_velocity; + packet.gimbal_device_id = gimbal_device_id; + mav_array_assign_float(packet.q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); #endif } @@ -240,7 +354,7 @@ static inline void mavlink_msg_gimbal_device_attitude_status_send(mavlink_channe static inline void mavlink_msg_gimbal_device_attitude_status_send_struct(mavlink_channel_t chan, const mavlink_gimbal_device_attitude_status_t* gimbal_device_attitude_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_gimbal_device_attitude_status_send(chan, gimbal_device_attitude_status->target_system, gimbal_device_attitude_status->target_component, gimbal_device_attitude_status->time_boot_ms, gimbal_device_attitude_status->flags, gimbal_device_attitude_status->q, gimbal_device_attitude_status->angular_velocity_x, gimbal_device_attitude_status->angular_velocity_y, gimbal_device_attitude_status->angular_velocity_z, gimbal_device_attitude_status->failure_flags); + mavlink_msg_gimbal_device_attitude_status_send(chan, gimbal_device_attitude_status->target_system, gimbal_device_attitude_status->target_component, gimbal_device_attitude_status->time_boot_ms, gimbal_device_attitude_status->flags, gimbal_device_attitude_status->q, gimbal_device_attitude_status->angular_velocity_x, gimbal_device_attitude_status->angular_velocity_y, gimbal_device_attitude_status->angular_velocity_z, gimbal_device_attitude_status->failure_flags, gimbal_device_attitude_status->delta_yaw, gimbal_device_attitude_status->delta_yaw_velocity, gimbal_device_attitude_status->gimbal_device_id); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, (const char *)gimbal_device_attitude_status, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); #endif @@ -248,13 +362,13 @@ static inline void mavlink_msg_gimbal_device_attitude_status_send_struct(mavlink #if MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_gimbal_device_attitude_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t time_boot_ms, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z, uint32_t failure_flags) +static inline void mavlink_msg_gimbal_device_attitude_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t time_boot_ms, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z, uint32_t failure_flags, float delta_yaw, float delta_yaw_velocity, uint8_t gimbal_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -266,6 +380,9 @@ static inline void mavlink_msg_gimbal_device_attitude_status_send_buf(mavlink_me _mav_put_uint16_t(buf, 36, flags); _mav_put_uint8_t(buf, 38, target_system); _mav_put_uint8_t(buf, 39, target_component); + _mav_put_float(buf, 40, delta_yaw); + _mav_put_float(buf, 44, delta_yaw_velocity); + _mav_put_uint8_t(buf, 48, gimbal_device_id); _mav_put_float_array(buf, 4, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); #else @@ -278,7 +395,10 @@ static inline void mavlink_msg_gimbal_device_attitude_status_send_buf(mavlink_me packet->flags = flags; packet->target_system = target_system; packet->target_component = target_component; - mav_array_memcpy(packet->q, q, sizeof(float)*4); + packet->delta_yaw = delta_yaw; + packet->delta_yaw_velocity = delta_yaw_velocity; + packet->gimbal_device_id = gimbal_device_id; + mav_array_assign_float(packet->q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); #endif } @@ -332,7 +452,7 @@ static inline uint16_t mavlink_msg_gimbal_device_attitude_status_get_flags(const /** * @brief Get field q from gimbal_device_attitude_status message * - * @return Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) + * @return Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description. */ static inline uint16_t mavlink_msg_gimbal_device_attitude_status_get_q(const mavlink_message_t* msg, float *q) { @@ -342,7 +462,7 @@ static inline uint16_t mavlink_msg_gimbal_device_attitude_status_get_q(const mav /** * @brief Get field angular_velocity_x from gimbal_device_attitude_status message * - * @return [rad/s] X component of angular velocity (NaN if unknown) + * @return [rad/s] X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN if unknown. */ static inline float mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_x(const mavlink_message_t* msg) { @@ -352,7 +472,7 @@ static inline float mavlink_msg_gimbal_device_attitude_status_get_angular_veloci /** * @brief Get field angular_velocity_y from gimbal_device_attitude_status message * - * @return [rad/s] Y component of angular velocity (NaN if unknown) + * @return [rad/s] Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN if unknown. */ static inline float mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_y(const mavlink_message_t* msg) { @@ -362,7 +482,7 @@ static inline float mavlink_msg_gimbal_device_attitude_status_get_angular_veloci /** * @brief Get field angular_velocity_z from gimbal_device_attitude_status message * - * @return [rad/s] Z component of angular velocity (NaN if unknown) + * @return [rad/s] Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN if unknown. */ static inline float mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_z(const mavlink_message_t* msg) { @@ -379,6 +499,36 @@ static inline uint32_t mavlink_msg_gimbal_device_attitude_status_get_failure_fla return _MAV_RETURN_uint32_t(msg, 32); } +/** + * @brief Get field delta_yaw from gimbal_device_attitude_status message + * + * @return [rad] Yaw angle relating the quaternions in earth and body frames (see message description). NaN if unknown. + */ +static inline float mavlink_msg_gimbal_device_attitude_status_get_delta_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field delta_yaw_velocity from gimbal_device_attitude_status message + * + * @return [rad/s] Yaw angular velocity relating the angular velocities in earth and body frames (see message description). NaN if unknown. + */ +static inline float mavlink_msg_gimbal_device_attitude_status_get_delta_yaw_velocity(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Get field gimbal_device_id from gimbal_device_attitude_status message + * + * @return This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set a number between 1-6. If the component ID is separate, this field is not required and must be set to 0. + */ +static inline uint8_t mavlink_msg_gimbal_device_attitude_status_get_gimbal_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 48); +} + /** * @brief Decode a gimbal_device_attitude_status message into a struct * @@ -397,6 +547,9 @@ static inline void mavlink_msg_gimbal_device_attitude_status_decode(const mavlin gimbal_device_attitude_status->flags = mavlink_msg_gimbal_device_attitude_status_get_flags(msg); gimbal_device_attitude_status->target_system = mavlink_msg_gimbal_device_attitude_status_get_target_system(msg); gimbal_device_attitude_status->target_component = mavlink_msg_gimbal_device_attitude_status_get_target_component(msg); + gimbal_device_attitude_status->delta_yaw = mavlink_msg_gimbal_device_attitude_status_get_delta_yaw(msg); + gimbal_device_attitude_status->delta_yaw_velocity = mavlink_msg_gimbal_device_attitude_status_get_delta_yaw_velocity(msg); + gimbal_device_attitude_status->gimbal_device_id = mavlink_msg_gimbal_device_attitude_status_get_gimbal_device_id(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN; memset(gimbal_device_attitude_status, 0, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_device_information.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_device_information.h index 2f0c86bdc97..77c5e901799 100644 --- a/lib/main/MAVLink/common/mavlink_msg_gimbal_device_information.h +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_device_information.h @@ -7,24 +7,25 @@ typedef struct __mavlink_gimbal_device_information_t { uint64_t uid; /*< UID of gimbal hardware (0 if unknown).*/ uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ - uint32_t firmware_version; /*< Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff).*/ - uint32_t hardware_version; /*< Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff).*/ - float roll_min; /*< [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left)*/ - float roll_max; /*< [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left)*/ - float pitch_min; /*< [rad] Minimum hardware pitch angle (positive: up, negative: down)*/ - float pitch_max; /*< [rad] Maximum hardware pitch angle (positive: up, negative: down)*/ - float yaw_min; /*< [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left)*/ - float yaw_max; /*< [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left)*/ + uint32_t firmware_version; /*< Version of the gimbal firmware, encoded as: `(Dev & 0xff) << 24 \| (Patch & 0xff) << 16 \| (Minor & 0xff) << 8 \| (Major & 0xff)`.*/ + uint32_t hardware_version; /*< Version of the gimbal hardware, encoded as: `(Dev & 0xff) << 24 \| (Patch & 0xff) << 16 \| (Minor & 0xff) << 8 \| (Major & 0xff)`.*/ + float roll_min; /*< [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left). NAN if unknown.*/ + float roll_max; /*< [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left). NAN if unknown.*/ + float pitch_min; /*< [rad] Minimum hardware pitch angle (positive: up, negative: down). NAN if unknown.*/ + float pitch_max; /*< [rad] Maximum hardware pitch angle (positive: up, negative: down). NAN if unknown.*/ + float yaw_min; /*< [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left). NAN if unknown.*/ + float yaw_max; /*< [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left). NAN if unknown.*/ uint16_t cap_flags; /*< Bitmap of gimbal capability flags.*/ uint16_t custom_cap_flags; /*< Bitmap for use for gimbal-specific capability flags.*/ char vendor_name[32]; /*< Name of the gimbal vendor.*/ char model_name[32]; /*< Name of the gimbal model.*/ char custom_name[32]; /*< Custom name of the gimbal given to it by the user.*/ + uint8_t gimbal_device_id; /*< This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set to a number between 1-6. If the component ID is separate, this field is not required and must be set to 0.*/ } mavlink_gimbal_device_information_t; -#define MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN 144 +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN 145 #define MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN 144 -#define MAVLINK_MSG_ID_283_LEN 144 +#define MAVLINK_MSG_ID_283_LEN 145 #define MAVLINK_MSG_ID_283_MIN_LEN 144 #define MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC 74 @@ -38,7 +39,7 @@ typedef struct __mavlink_gimbal_device_information_t { #define MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_INFORMATION { \ 283, \ "GIMBAL_DEVICE_INFORMATION", \ - 15, \ + 16, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_gimbal_device_information_t, time_boot_ms) }, \ { "vendor_name", NULL, MAVLINK_TYPE_CHAR, 32, 48, offsetof(mavlink_gimbal_device_information_t, vendor_name) }, \ { "model_name", NULL, MAVLINK_TYPE_CHAR, 32, 80, offsetof(mavlink_gimbal_device_information_t, model_name) }, \ @@ -54,12 +55,13 @@ typedef struct __mavlink_gimbal_device_information_t { { "pitch_max", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_gimbal_device_information_t, pitch_max) }, \ { "yaw_min", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_gimbal_device_information_t, yaw_min) }, \ { "yaw_max", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_gimbal_device_information_t, yaw_max) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 144, offsetof(mavlink_gimbal_device_information_t, gimbal_device_id) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_INFORMATION { \ "GIMBAL_DEVICE_INFORMATION", \ - 15, \ + 16, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_gimbal_device_information_t, time_boot_ms) }, \ { "vendor_name", NULL, MAVLINK_TYPE_CHAR, 32, 48, offsetof(mavlink_gimbal_device_information_t, vendor_name) }, \ { "model_name", NULL, MAVLINK_TYPE_CHAR, 32, 80, offsetof(mavlink_gimbal_device_information_t, model_name) }, \ @@ -75,6 +77,7 @@ typedef struct __mavlink_gimbal_device_information_t { { "pitch_max", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_gimbal_device_information_t, pitch_max) }, \ { "yaw_min", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_gimbal_device_information_t, yaw_min) }, \ { "yaw_max", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_gimbal_device_information_t, yaw_max) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 144, offsetof(mavlink_gimbal_device_information_t, gimbal_device_id) }, \ } \ } #endif @@ -89,21 +92,22 @@ typedef struct __mavlink_gimbal_device_information_t { * @param vendor_name Name of the gimbal vendor. * @param model_name Name of the gimbal model. * @param custom_name Custom name of the gimbal given to it by the user. - * @param firmware_version Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). - * @param hardware_version Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + * @param firmware_version Version of the gimbal firmware, encoded as: `(Dev & 0xff) << 24 \| (Patch & 0xff) << 16 \| (Minor & 0xff) << 8 \| (Major & 0xff)`. + * @param hardware_version Version of the gimbal hardware, encoded as: `(Dev & 0xff) << 24 \| (Patch & 0xff) << 16 \| (Minor & 0xff) << 8 \| (Major & 0xff)`. * @param uid UID of gimbal hardware (0 if unknown). * @param cap_flags Bitmap of gimbal capability flags. * @param custom_cap_flags Bitmap for use for gimbal-specific capability flags. - * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) - * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) - * @param pitch_min [rad] Minimum hardware pitch angle (positive: up, negative: down) - * @param pitch_max [rad] Maximum hardware pitch angle (positive: up, negative: down) - * @param yaw_min [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left) - * @param yaw_max [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left) + * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left). NAN if unknown. + * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left). NAN if unknown. + * @param pitch_min [rad] Minimum hardware pitch angle (positive: up, negative: down). NAN if unknown. + * @param pitch_max [rad] Maximum hardware pitch angle (positive: up, negative: down). NAN if unknown. + * @param yaw_min [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left). NAN if unknown. + * @param yaw_max [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left). NAN if unknown. + * @param gimbal_device_id This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set to a number between 1-6. If the component ID is separate, this field is not required and must be set to 0. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gimbal_device_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, const char *vendor_name, const char *model_name, const char *custom_name, uint32_t firmware_version, uint32_t hardware_version, uint64_t uid, uint16_t cap_flags, uint16_t custom_cap_flags, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max) + uint32_t time_boot_ms, const char *vendor_name, const char *model_name, const char *custom_name, uint32_t firmware_version, uint32_t hardware_version, uint64_t uid, uint16_t cap_flags, uint16_t custom_cap_flags, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max, uint8_t gimbal_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN]; @@ -119,6 +123,7 @@ static inline uint16_t mavlink_msg_gimbal_device_information_pack(uint8_t system _mav_put_float(buf, 40, yaw_max); _mav_put_uint16_t(buf, 44, cap_flags); _mav_put_uint16_t(buf, 46, custom_cap_flags); + _mav_put_uint8_t(buf, 144, gimbal_device_id); _mav_put_char_array(buf, 48, vendor_name, 32); _mav_put_char_array(buf, 80, model_name, 32); _mav_put_char_array(buf, 112, custom_name, 32); @@ -137,6 +142,79 @@ static inline uint16_t mavlink_msg_gimbal_device_information_pack(uint8_t system packet.yaw_max = yaw_max; packet.cap_flags = cap_flags; packet.custom_cap_flags = custom_cap_flags; + packet.gimbal_device_id = gimbal_device_id; + mav_array_assign_char(packet.vendor_name, vendor_name, 32); + mav_array_assign_char(packet.model_name, model_name, 32); + mav_array_assign_char(packet.custom_name, custom_name, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); +} + +/** + * @brief Pack a gimbal_device_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param vendor_name Name of the gimbal vendor. + * @param model_name Name of the gimbal model. + * @param custom_name Custom name of the gimbal given to it by the user. + * @param firmware_version Version of the gimbal firmware, encoded as: `(Dev & 0xff) << 24 \| (Patch & 0xff) << 16 \| (Minor & 0xff) << 8 \| (Major & 0xff)`. + * @param hardware_version Version of the gimbal hardware, encoded as: `(Dev & 0xff) << 24 \| (Patch & 0xff) << 16 \| (Minor & 0xff) << 8 \| (Major & 0xff)`. + * @param uid UID of gimbal hardware (0 if unknown). + * @param cap_flags Bitmap of gimbal capability flags. + * @param custom_cap_flags Bitmap for use for gimbal-specific capability flags. + * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left). NAN if unknown. + * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left). NAN if unknown. + * @param pitch_min [rad] Minimum hardware pitch angle (positive: up, negative: down). NAN if unknown. + * @param pitch_max [rad] Maximum hardware pitch angle (positive: up, negative: down). NAN if unknown. + * @param yaw_min [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left). NAN if unknown. + * @param yaw_max [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left). NAN if unknown. + * @param gimbal_device_id This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set to a number between 1-6. If the component ID is separate, this field is not required and must be set to 0. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_device_information_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, const char *vendor_name, const char *model_name, const char *custom_name, uint32_t firmware_version, uint32_t hardware_version, uint64_t uid, uint16_t cap_flags, uint16_t custom_cap_flags, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max, uint8_t gimbal_device_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN]; + _mav_put_uint64_t(buf, 0, uid); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_uint32_t(buf, 12, firmware_version); + _mav_put_uint32_t(buf, 16, hardware_version); + _mav_put_float(buf, 20, roll_min); + _mav_put_float(buf, 24, roll_max); + _mav_put_float(buf, 28, pitch_min); + _mav_put_float(buf, 32, pitch_max); + _mav_put_float(buf, 36, yaw_min); + _mav_put_float(buf, 40, yaw_max); + _mav_put_uint16_t(buf, 44, cap_flags); + _mav_put_uint16_t(buf, 46, custom_cap_flags); + _mav_put_uint8_t(buf, 144, gimbal_device_id); + _mav_put_char_array(buf, 48, vendor_name, 32); + _mav_put_char_array(buf, 80, model_name, 32); + _mav_put_char_array(buf, 112, custom_name, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN); +#else + mavlink_gimbal_device_information_t packet; + packet.uid = uid; + packet.time_boot_ms = time_boot_ms; + packet.firmware_version = firmware_version; + packet.hardware_version = hardware_version; + packet.roll_min = roll_min; + packet.roll_max = roll_max; + packet.pitch_min = pitch_min; + packet.pitch_max = pitch_max; + packet.yaw_min = yaw_min; + packet.yaw_max = yaw_max; + packet.cap_flags = cap_flags; + packet.custom_cap_flags = custom_cap_flags; + packet.gimbal_device_id = gimbal_device_id; mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(char)*32); mav_array_memcpy(packet.model_name, model_name, sizeof(char)*32); mav_array_memcpy(packet.custom_name, custom_name, sizeof(char)*32); @@ -144,7 +222,11 @@ static inline uint16_t mavlink_msg_gimbal_device_information_pack(uint8_t system #endif msg->msgid = MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN); +#endif } /** @@ -157,22 +239,23 @@ static inline uint16_t mavlink_msg_gimbal_device_information_pack(uint8_t system * @param vendor_name Name of the gimbal vendor. * @param model_name Name of the gimbal model. * @param custom_name Custom name of the gimbal given to it by the user. - * @param firmware_version Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). - * @param hardware_version Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + * @param firmware_version Version of the gimbal firmware, encoded as: `(Dev & 0xff) << 24 \| (Patch & 0xff) << 16 \| (Minor & 0xff) << 8 \| (Major & 0xff)`. + * @param hardware_version Version of the gimbal hardware, encoded as: `(Dev & 0xff) << 24 \| (Patch & 0xff) << 16 \| (Minor & 0xff) << 8 \| (Major & 0xff)`. * @param uid UID of gimbal hardware (0 if unknown). * @param cap_flags Bitmap of gimbal capability flags. * @param custom_cap_flags Bitmap for use for gimbal-specific capability flags. - * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) - * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) - * @param pitch_min [rad] Minimum hardware pitch angle (positive: up, negative: down) - * @param pitch_max [rad] Maximum hardware pitch angle (positive: up, negative: down) - * @param yaw_min [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left) - * @param yaw_max [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left) + * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left). NAN if unknown. + * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left). NAN if unknown. + * @param pitch_min [rad] Minimum hardware pitch angle (positive: up, negative: down). NAN if unknown. + * @param pitch_max [rad] Maximum hardware pitch angle (positive: up, negative: down). NAN if unknown. + * @param yaw_min [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left). NAN if unknown. + * @param yaw_max [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left). NAN if unknown. + * @param gimbal_device_id This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set to a number between 1-6. If the component ID is separate, this field is not required and must be set to 0. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gimbal_device_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,const char *vendor_name,const char *model_name,const char *custom_name,uint32_t firmware_version,uint32_t hardware_version,uint64_t uid,uint16_t cap_flags,uint16_t custom_cap_flags,float roll_min,float roll_max,float pitch_min,float pitch_max,float yaw_min,float yaw_max) + uint32_t time_boot_ms,const char *vendor_name,const char *model_name,const char *custom_name,uint32_t firmware_version,uint32_t hardware_version,uint64_t uid,uint16_t cap_flags,uint16_t custom_cap_flags,float roll_min,float roll_max,float pitch_min,float pitch_max,float yaw_min,float yaw_max,uint8_t gimbal_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN]; @@ -188,6 +271,7 @@ static inline uint16_t mavlink_msg_gimbal_device_information_pack_chan(uint8_t s _mav_put_float(buf, 40, yaw_max); _mav_put_uint16_t(buf, 44, cap_flags); _mav_put_uint16_t(buf, 46, custom_cap_flags); + _mav_put_uint8_t(buf, 144, gimbal_device_id); _mav_put_char_array(buf, 48, vendor_name, 32); _mav_put_char_array(buf, 80, model_name, 32); _mav_put_char_array(buf, 112, custom_name, 32); @@ -206,9 +290,10 @@ static inline uint16_t mavlink_msg_gimbal_device_information_pack_chan(uint8_t s packet.yaw_max = yaw_max; packet.cap_flags = cap_flags; packet.custom_cap_flags = custom_cap_flags; - mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(char)*32); - mav_array_memcpy(packet.model_name, model_name, sizeof(char)*32); - mav_array_memcpy(packet.custom_name, custom_name, sizeof(char)*32); + packet.gimbal_device_id = gimbal_device_id; + mav_array_assign_char(packet.vendor_name, vendor_name, 32); + mav_array_assign_char(packet.model_name, model_name, 32); + mav_array_assign_char(packet.custom_name, custom_name, 32); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN); #endif @@ -226,7 +311,7 @@ static inline uint16_t mavlink_msg_gimbal_device_information_pack_chan(uint8_t s */ static inline uint16_t mavlink_msg_gimbal_device_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_device_information_t* gimbal_device_information) { - return mavlink_msg_gimbal_device_information_pack(system_id, component_id, msg, gimbal_device_information->time_boot_ms, gimbal_device_information->vendor_name, gimbal_device_information->model_name, gimbal_device_information->custom_name, gimbal_device_information->firmware_version, gimbal_device_information->hardware_version, gimbal_device_information->uid, gimbal_device_information->cap_flags, gimbal_device_information->custom_cap_flags, gimbal_device_information->roll_min, gimbal_device_information->roll_max, gimbal_device_information->pitch_min, gimbal_device_information->pitch_max, gimbal_device_information->yaw_min, gimbal_device_information->yaw_max); + return mavlink_msg_gimbal_device_information_pack(system_id, component_id, msg, gimbal_device_information->time_boot_ms, gimbal_device_information->vendor_name, gimbal_device_information->model_name, gimbal_device_information->custom_name, gimbal_device_information->firmware_version, gimbal_device_information->hardware_version, gimbal_device_information->uid, gimbal_device_information->cap_flags, gimbal_device_information->custom_cap_flags, gimbal_device_information->roll_min, gimbal_device_information->roll_max, gimbal_device_information->pitch_min, gimbal_device_information->pitch_max, gimbal_device_information->yaw_min, gimbal_device_information->yaw_max, gimbal_device_information->gimbal_device_id); } /** @@ -240,7 +325,21 @@ static inline uint16_t mavlink_msg_gimbal_device_information_encode(uint8_t syst */ static inline uint16_t mavlink_msg_gimbal_device_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_device_information_t* gimbal_device_information) { - return mavlink_msg_gimbal_device_information_pack_chan(system_id, component_id, chan, msg, gimbal_device_information->time_boot_ms, gimbal_device_information->vendor_name, gimbal_device_information->model_name, gimbal_device_information->custom_name, gimbal_device_information->firmware_version, gimbal_device_information->hardware_version, gimbal_device_information->uid, gimbal_device_information->cap_flags, gimbal_device_information->custom_cap_flags, gimbal_device_information->roll_min, gimbal_device_information->roll_max, gimbal_device_information->pitch_min, gimbal_device_information->pitch_max, gimbal_device_information->yaw_min, gimbal_device_information->yaw_max); + return mavlink_msg_gimbal_device_information_pack_chan(system_id, component_id, chan, msg, gimbal_device_information->time_boot_ms, gimbal_device_information->vendor_name, gimbal_device_information->model_name, gimbal_device_information->custom_name, gimbal_device_information->firmware_version, gimbal_device_information->hardware_version, gimbal_device_information->uid, gimbal_device_information->cap_flags, gimbal_device_information->custom_cap_flags, gimbal_device_information->roll_min, gimbal_device_information->roll_max, gimbal_device_information->pitch_min, gimbal_device_information->pitch_max, gimbal_device_information->yaw_min, gimbal_device_information->yaw_max, gimbal_device_information->gimbal_device_id); +} + +/** + * @brief Encode a gimbal_device_information struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param gimbal_device_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_device_information_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_gimbal_device_information_t* gimbal_device_information) +{ + return mavlink_msg_gimbal_device_information_pack_status(system_id, component_id, _status, msg, gimbal_device_information->time_boot_ms, gimbal_device_information->vendor_name, gimbal_device_information->model_name, gimbal_device_information->custom_name, gimbal_device_information->firmware_version, gimbal_device_information->hardware_version, gimbal_device_information->uid, gimbal_device_information->cap_flags, gimbal_device_information->custom_cap_flags, gimbal_device_information->roll_min, gimbal_device_information->roll_max, gimbal_device_information->pitch_min, gimbal_device_information->pitch_max, gimbal_device_information->yaw_min, gimbal_device_information->yaw_max, gimbal_device_information->gimbal_device_id); } /** @@ -251,21 +350,22 @@ static inline uint16_t mavlink_msg_gimbal_device_information_encode_chan(uint8_t * @param vendor_name Name of the gimbal vendor. * @param model_name Name of the gimbal model. * @param custom_name Custom name of the gimbal given to it by the user. - * @param firmware_version Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). - * @param hardware_version Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + * @param firmware_version Version of the gimbal firmware, encoded as: `(Dev & 0xff) << 24 \| (Patch & 0xff) << 16 \| (Minor & 0xff) << 8 \| (Major & 0xff)`. + * @param hardware_version Version of the gimbal hardware, encoded as: `(Dev & 0xff) << 24 \| (Patch & 0xff) << 16 \| (Minor & 0xff) << 8 \| (Major & 0xff)`. * @param uid UID of gimbal hardware (0 if unknown). * @param cap_flags Bitmap of gimbal capability flags. * @param custom_cap_flags Bitmap for use for gimbal-specific capability flags. - * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) - * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) - * @param pitch_min [rad] Minimum hardware pitch angle (positive: up, negative: down) - * @param pitch_max [rad] Maximum hardware pitch angle (positive: up, negative: down) - * @param yaw_min [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left) - * @param yaw_max [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left) + * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left). NAN if unknown. + * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left). NAN if unknown. + * @param pitch_min [rad] Minimum hardware pitch angle (positive: up, negative: down). NAN if unknown. + * @param pitch_max [rad] Maximum hardware pitch angle (positive: up, negative: down). NAN if unknown. + * @param yaw_min [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left). NAN if unknown. + * @param yaw_max [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left). NAN if unknown. + * @param gimbal_device_id This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set to a number between 1-6. If the component ID is separate, this field is not required and must be set to 0. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gimbal_device_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, const char *vendor_name, const char *model_name, const char *custom_name, uint32_t firmware_version, uint32_t hardware_version, uint64_t uid, uint16_t cap_flags, uint16_t custom_cap_flags, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max) +static inline void mavlink_msg_gimbal_device_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, const char *vendor_name, const char *model_name, const char *custom_name, uint32_t firmware_version, uint32_t hardware_version, uint64_t uid, uint16_t cap_flags, uint16_t custom_cap_flags, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max, uint8_t gimbal_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN]; @@ -281,6 +381,7 @@ static inline void mavlink_msg_gimbal_device_information_send(mavlink_channel_t _mav_put_float(buf, 40, yaw_max); _mav_put_uint16_t(buf, 44, cap_flags); _mav_put_uint16_t(buf, 46, custom_cap_flags); + _mav_put_uint8_t(buf, 144, gimbal_device_id); _mav_put_char_array(buf, 48, vendor_name, 32); _mav_put_char_array(buf, 80, model_name, 32); _mav_put_char_array(buf, 112, custom_name, 32); @@ -299,9 +400,10 @@ static inline void mavlink_msg_gimbal_device_information_send(mavlink_channel_t packet.yaw_max = yaw_max; packet.cap_flags = cap_flags; packet.custom_cap_flags = custom_cap_flags; - mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(char)*32); - mav_array_memcpy(packet.model_name, model_name, sizeof(char)*32); - mav_array_memcpy(packet.custom_name, custom_name, sizeof(char)*32); + packet.gimbal_device_id = gimbal_device_id; + mav_array_assign_char(packet.vendor_name, vendor_name, 32); + mav_array_assign_char(packet.model_name, model_name, 32); + mav_array_assign_char(packet.custom_name, custom_name, 32); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); #endif } @@ -314,7 +416,7 @@ static inline void mavlink_msg_gimbal_device_information_send(mavlink_channel_t static inline void mavlink_msg_gimbal_device_information_send_struct(mavlink_channel_t chan, const mavlink_gimbal_device_information_t* gimbal_device_information) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_gimbal_device_information_send(chan, gimbal_device_information->time_boot_ms, gimbal_device_information->vendor_name, gimbal_device_information->model_name, gimbal_device_information->custom_name, gimbal_device_information->firmware_version, gimbal_device_information->hardware_version, gimbal_device_information->uid, gimbal_device_information->cap_flags, gimbal_device_information->custom_cap_flags, gimbal_device_information->roll_min, gimbal_device_information->roll_max, gimbal_device_information->pitch_min, gimbal_device_information->pitch_max, gimbal_device_information->yaw_min, gimbal_device_information->yaw_max); + mavlink_msg_gimbal_device_information_send(chan, gimbal_device_information->time_boot_ms, gimbal_device_information->vendor_name, gimbal_device_information->model_name, gimbal_device_information->custom_name, gimbal_device_information->firmware_version, gimbal_device_information->hardware_version, gimbal_device_information->uid, gimbal_device_information->cap_flags, gimbal_device_information->custom_cap_flags, gimbal_device_information->roll_min, gimbal_device_information->roll_max, gimbal_device_information->pitch_min, gimbal_device_information->pitch_max, gimbal_device_information->yaw_min, gimbal_device_information->yaw_max, gimbal_device_information->gimbal_device_id); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, (const char *)gimbal_device_information, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); #endif @@ -322,13 +424,13 @@ static inline void mavlink_msg_gimbal_device_information_send_struct(mavlink_cha #if MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_gimbal_device_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const char *vendor_name, const char *model_name, const char *custom_name, uint32_t firmware_version, uint32_t hardware_version, uint64_t uid, uint16_t cap_flags, uint16_t custom_cap_flags, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max) +static inline void mavlink_msg_gimbal_device_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const char *vendor_name, const char *model_name, const char *custom_name, uint32_t firmware_version, uint32_t hardware_version, uint64_t uid, uint16_t cap_flags, uint16_t custom_cap_flags, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max, uint8_t gimbal_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -344,6 +446,7 @@ static inline void mavlink_msg_gimbal_device_information_send_buf(mavlink_messag _mav_put_float(buf, 40, yaw_max); _mav_put_uint16_t(buf, 44, cap_flags); _mav_put_uint16_t(buf, 46, custom_cap_flags); + _mav_put_uint8_t(buf, 144, gimbal_device_id); _mav_put_char_array(buf, 48, vendor_name, 32); _mav_put_char_array(buf, 80, model_name, 32); _mav_put_char_array(buf, 112, custom_name, 32); @@ -362,9 +465,10 @@ static inline void mavlink_msg_gimbal_device_information_send_buf(mavlink_messag packet->yaw_max = yaw_max; packet->cap_flags = cap_flags; packet->custom_cap_flags = custom_cap_flags; - mav_array_memcpy(packet->vendor_name, vendor_name, sizeof(char)*32); - mav_array_memcpy(packet->model_name, model_name, sizeof(char)*32); - mav_array_memcpy(packet->custom_name, custom_name, sizeof(char)*32); + packet->gimbal_device_id = gimbal_device_id; + mav_array_assign_char(packet->vendor_name, vendor_name, 32); + mav_array_assign_char(packet->model_name, model_name, 32); + mav_array_assign_char(packet->custom_name, custom_name, 32); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); #endif } @@ -418,7 +522,7 @@ static inline uint16_t mavlink_msg_gimbal_device_information_get_custom_name(con /** * @brief Get field firmware_version from gimbal_device_information message * - * @return Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + * @return Version of the gimbal firmware, encoded as: `(Dev & 0xff) << 24 \| (Patch & 0xff) << 16 \| (Minor & 0xff) << 8 \| (Major & 0xff)`. */ static inline uint32_t mavlink_msg_gimbal_device_information_get_firmware_version(const mavlink_message_t* msg) { @@ -428,7 +532,7 @@ static inline uint32_t mavlink_msg_gimbal_device_information_get_firmware_versio /** * @brief Get field hardware_version from gimbal_device_information message * - * @return Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + * @return Version of the gimbal hardware, encoded as: `(Dev & 0xff) << 24 \| (Patch & 0xff) << 16 \| (Minor & 0xff) << 8 \| (Major & 0xff)`. */ static inline uint32_t mavlink_msg_gimbal_device_information_get_hardware_version(const mavlink_message_t* msg) { @@ -468,7 +572,7 @@ static inline uint16_t mavlink_msg_gimbal_device_information_get_custom_cap_flag /** * @brief Get field roll_min from gimbal_device_information message * - * @return [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @return [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left). NAN if unknown. */ static inline float mavlink_msg_gimbal_device_information_get_roll_min(const mavlink_message_t* msg) { @@ -478,7 +582,7 @@ static inline float mavlink_msg_gimbal_device_information_get_roll_min(const mav /** * @brief Get field roll_max from gimbal_device_information message * - * @return [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @return [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left). NAN if unknown. */ static inline float mavlink_msg_gimbal_device_information_get_roll_max(const mavlink_message_t* msg) { @@ -488,7 +592,7 @@ static inline float mavlink_msg_gimbal_device_information_get_roll_max(const mav /** * @brief Get field pitch_min from gimbal_device_information message * - * @return [rad] Minimum hardware pitch angle (positive: up, negative: down) + * @return [rad] Minimum hardware pitch angle (positive: up, negative: down). NAN if unknown. */ static inline float mavlink_msg_gimbal_device_information_get_pitch_min(const mavlink_message_t* msg) { @@ -498,7 +602,7 @@ static inline float mavlink_msg_gimbal_device_information_get_pitch_min(const ma /** * @brief Get field pitch_max from gimbal_device_information message * - * @return [rad] Maximum hardware pitch angle (positive: up, negative: down) + * @return [rad] Maximum hardware pitch angle (positive: up, negative: down). NAN if unknown. */ static inline float mavlink_msg_gimbal_device_information_get_pitch_max(const mavlink_message_t* msg) { @@ -508,7 +612,7 @@ static inline float mavlink_msg_gimbal_device_information_get_pitch_max(const ma /** * @brief Get field yaw_min from gimbal_device_information message * - * @return [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left) + * @return [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left). NAN if unknown. */ static inline float mavlink_msg_gimbal_device_information_get_yaw_min(const mavlink_message_t* msg) { @@ -518,13 +622,23 @@ static inline float mavlink_msg_gimbal_device_information_get_yaw_min(const mavl /** * @brief Get field yaw_max from gimbal_device_information message * - * @return [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left) + * @return [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left). NAN if unknown. */ static inline float mavlink_msg_gimbal_device_information_get_yaw_max(const mavlink_message_t* msg) { return _MAV_RETURN_float(msg, 40); } +/** + * @brief Get field gimbal_device_id from gimbal_device_information message + * + * @return This field is to be used if the gimbal manager and the gimbal device are the same component and hence have the same component ID. This field is then set to a number between 1-6. If the component ID is separate, this field is not required and must be set to 0. + */ +static inline uint8_t mavlink_msg_gimbal_device_information_get_gimbal_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 144); +} + /** * @brief Decode a gimbal_device_information message into a struct * @@ -549,6 +663,7 @@ static inline void mavlink_msg_gimbal_device_information_decode(const mavlink_me mavlink_msg_gimbal_device_information_get_vendor_name(msg, gimbal_device_information->vendor_name); mavlink_msg_gimbal_device_information_get_model_name(msg, gimbal_device_information->model_name); mavlink_msg_gimbal_device_information_get_custom_name(msg, gimbal_device_information->custom_name); + gimbal_device_information->gimbal_device_id = mavlink_msg_gimbal_device_information_get_gimbal_device_id(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN; memset(gimbal_device_information, 0, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_device_set_attitude.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_device_set_attitude.h index 1cfda326669..f8cf2076b03 100644 --- a/lib/main/MAVLink/common/mavlink_msg_gimbal_device_set_attitude.h +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_device_set_attitude.h @@ -5,10 +5,10 @@ typedef struct __mavlink_gimbal_device_set_attitude_t { - float q[4]; /*< Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set, set all fields to NaN if only angular velocity should be used)*/ - float angular_velocity_x; /*< [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored.*/ - float angular_velocity_y; /*< [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored.*/ - float angular_velocity_z; /*< [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored.*/ + float q[4]; /*< Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description. Set fields to NaN to be ignored.*/ + float angular_velocity_x; /*< [rad/s] X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN to be ignored.*/ + float angular_velocity_y; /*< [rad/s] Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN to be ignored.*/ + float angular_velocity_z; /*< [rad/s] Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN to be ignored.*/ uint16_t flags; /*< Low level gimbal flags.*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ @@ -62,10 +62,10 @@ typedef struct __mavlink_gimbal_device_set_attitude_t { * @param target_system System ID * @param target_component Component ID * @param flags Low level gimbal flags. - * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set, set all fields to NaN if only angular velocity should be used) - * @param angular_velocity_x [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. - * @param angular_velocity_y [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. - * @param angular_velocity_z [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description. Set fields to NaN to be ignored. + * @param angular_velocity_x [rad/s] X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN to be ignored. + * @param angular_velocity_y [rad/s] Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN to be ignored. + * @param angular_velocity_z [rad/s] Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN to be ignored. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gimbal_device_set_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_gimbal_device_set_attitude_pack(uint8_t syste packet.flags = flags; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN); #endif @@ -97,6 +97,55 @@ static inline uint16_t mavlink_msg_gimbal_device_set_attitude_pack(uint8_t syste return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC); } +/** + * @brief Pack a gimbal_device_set_attitude message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param flags Low level gimbal flags. + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description. Set fields to NaN to be ignored. + * @param angular_velocity_x [rad/s] X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN to be ignored. + * @param angular_velocity_y [rad/s] Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN to be ignored. + * @param angular_velocity_z [rad/s] Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN to be ignored. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_device_set_attitude_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN]; + _mav_put_float(buf, 16, angular_velocity_x); + _mav_put_float(buf, 20, angular_velocity_y); + _mav_put_float(buf, 24, angular_velocity_z); + _mav_put_uint16_t(buf, 28, flags); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_float_array(buf, 0, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN); +#else + mavlink_gimbal_device_set_attitude_t packet; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.flags = flags; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN); +#endif +} + /** * @brief Pack a gimbal_device_set_attitude message on a channel * @param system_id ID of this system @@ -106,10 +155,10 @@ static inline uint16_t mavlink_msg_gimbal_device_set_attitude_pack(uint8_t syste * @param target_system System ID * @param target_component Component ID * @param flags Low level gimbal flags. - * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set, set all fields to NaN if only angular velocity should be used) - * @param angular_velocity_x [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. - * @param angular_velocity_y [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. - * @param angular_velocity_z [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description. Set fields to NaN to be ignored. + * @param angular_velocity_x [rad/s] X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN to be ignored. + * @param angular_velocity_y [rad/s] Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN to be ignored. + * @param angular_velocity_z [rad/s] Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN to be ignored. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gimbal_device_set_attitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -134,7 +183,7 @@ static inline uint16_t mavlink_msg_gimbal_device_set_attitude_pack_chan(uint8_t packet.flags = flags; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN); #endif @@ -169,6 +218,20 @@ static inline uint16_t mavlink_msg_gimbal_device_set_attitude_encode_chan(uint8_ return mavlink_msg_gimbal_device_set_attitude_pack_chan(system_id, component_id, chan, msg, gimbal_device_set_attitude->target_system, gimbal_device_set_attitude->target_component, gimbal_device_set_attitude->flags, gimbal_device_set_attitude->q, gimbal_device_set_attitude->angular_velocity_x, gimbal_device_set_attitude->angular_velocity_y, gimbal_device_set_attitude->angular_velocity_z); } +/** + * @brief Encode a gimbal_device_set_attitude struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param gimbal_device_set_attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_device_set_attitude_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_gimbal_device_set_attitude_t* gimbal_device_set_attitude) +{ + return mavlink_msg_gimbal_device_set_attitude_pack_status(system_id, component_id, _status, msg, gimbal_device_set_attitude->target_system, gimbal_device_set_attitude->target_component, gimbal_device_set_attitude->flags, gimbal_device_set_attitude->q, gimbal_device_set_attitude->angular_velocity_x, gimbal_device_set_attitude->angular_velocity_y, gimbal_device_set_attitude->angular_velocity_z); +} + /** * @brief Send a gimbal_device_set_attitude message * @param chan MAVLink channel to send the message @@ -176,10 +239,10 @@ static inline uint16_t mavlink_msg_gimbal_device_set_attitude_encode_chan(uint8_ * @param target_system System ID * @param target_component Component ID * @param flags Low level gimbal flags. - * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set, set all fields to NaN if only angular velocity should be used) - * @param angular_velocity_x [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. - * @param angular_velocity_y [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. - * @param angular_velocity_z [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description. Set fields to NaN to be ignored. + * @param angular_velocity_x [rad/s] X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN to be ignored. + * @param angular_velocity_y [rad/s] Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN to be ignored. + * @param angular_velocity_z [rad/s] Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN to be ignored. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -203,7 +266,7 @@ static inline void mavlink_msg_gimbal_device_set_attitude_send(mavlink_channel_t packet.flags = flags; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC); #endif } @@ -224,7 +287,7 @@ static inline void mavlink_msg_gimbal_device_set_attitude_send_struct(mavlink_ch #if MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -250,7 +313,7 @@ static inline void mavlink_msg_gimbal_device_set_attitude_send_buf(mavlink_messa packet->flags = flags; packet->target_system = target_system; packet->target_component = target_component; - mav_array_memcpy(packet->q, q, sizeof(float)*4); + mav_array_assign_float(packet->q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC); #endif } @@ -294,7 +357,7 @@ static inline uint16_t mavlink_msg_gimbal_device_set_attitude_get_flags(const ma /** * @brief Get field q from gimbal_device_set_attitude message * - * @return Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set, set all fields to NaN if only angular velocity should be used) + * @return Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation). The frame is described in the message description. Set fields to NaN to be ignored. */ static inline uint16_t mavlink_msg_gimbal_device_set_attitude_get_q(const mavlink_message_t* msg, float *q) { @@ -304,7 +367,7 @@ static inline uint16_t mavlink_msg_gimbal_device_set_attitude_get_q(const mavlin /** * @brief Get field angular_velocity_x from gimbal_device_set_attitude message * - * @return [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. + * @return [rad/s] X component of angular velocity (positive: rolling to the right). The frame is described in the message description. NaN to be ignored. */ static inline float mavlink_msg_gimbal_device_set_attitude_get_angular_velocity_x(const mavlink_message_t* msg) { @@ -314,7 +377,7 @@ static inline float mavlink_msg_gimbal_device_set_attitude_get_angular_velocity_ /** * @brief Get field angular_velocity_y from gimbal_device_set_attitude message * - * @return [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. + * @return [rad/s] Y component of angular velocity (positive: pitching up). The frame is described in the message description. NaN to be ignored. */ static inline float mavlink_msg_gimbal_device_set_attitude_get_angular_velocity_y(const mavlink_message_t* msg) { @@ -324,7 +387,7 @@ static inline float mavlink_msg_gimbal_device_set_attitude_get_angular_velocity_ /** * @brief Get field angular_velocity_z from gimbal_device_set_attitude message * - * @return [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + * @return [rad/s] Z component of angular velocity (positive: yawing to the right). The frame is described in the message description. NaN to be ignored. */ static inline float mavlink_msg_gimbal_device_set_attitude_get_angular_velocity_z(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_information.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_information.h index 58c8c59d976..f3415a1c2b7 100644 --- a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_information.h +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_information.h @@ -13,7 +13,7 @@ typedef struct __mavlink_gimbal_manager_information_t { float pitch_max; /*< [rad] Maximum pitch angle (positive: up, negative: down)*/ float yaw_min; /*< [rad] Minimum yaw angle (positive: to the right, negative: to the left)*/ float yaw_max; /*< [rad] Maximum yaw angle (positive: to the right, negative: to the left)*/ - uint8_t gimbal_device_id; /*< Gimbal device ID that this gimbal manager is responsible for.*/ + uint8_t gimbal_device_id; /*< Gimbal device ID that this gimbal manager is responsible for. Component ID of gimbal device (or 1-6 for non-MAVLink gimbal).*/ } mavlink_gimbal_manager_information_t; #define MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN 33 @@ -67,7 +67,7 @@ typedef struct __mavlink_gimbal_manager_information_t { * * @param time_boot_ms [ms] Timestamp (time since system boot). * @param cap_flags Bitmap of gimbal capability flags. - * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. Component ID of gimbal device (or 1-6 for non-MAVLink gimbal). * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) * @param pitch_min [rad] Minimum pitch angle (positive: up, negative: down) @@ -111,6 +111,63 @@ static inline uint16_t mavlink_msg_gimbal_manager_information_pack(uint8_t syste return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_CRC); } +/** + * @brief Pack a gimbal_manager_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param cap_flags Bitmap of gimbal capability flags. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. Component ID of gimbal device (or 1-6 for non-MAVLink gimbal). + * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param pitch_min [rad] Minimum pitch angle (positive: up, negative: down) + * @param pitch_max [rad] Maximum pitch angle (positive: up, negative: down) + * @param yaw_min [rad] Minimum yaw angle (positive: to the right, negative: to the left) + * @param yaw_max [rad] Maximum yaw angle (positive: to the right, negative: to the left) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_information_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint32_t cap_flags, uint8_t gimbal_device_id, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, cap_flags); + _mav_put_float(buf, 8, roll_min); + _mav_put_float(buf, 12, roll_max); + _mav_put_float(buf, 16, pitch_min); + _mav_put_float(buf, 20, pitch_max); + _mav_put_float(buf, 24, yaw_min); + _mav_put_float(buf, 28, yaw_max); + _mav_put_uint8_t(buf, 32, gimbal_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN); +#else + mavlink_gimbal_manager_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.cap_flags = cap_flags; + packet.roll_min = roll_min; + packet.roll_max = roll_max; + packet.pitch_min = pitch_min; + packet.pitch_max = pitch_max; + packet.yaw_min = yaw_min; + packet.yaw_max = yaw_max; + packet.gimbal_device_id = gimbal_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN); +#endif +} + /** * @brief Pack a gimbal_manager_information message on a channel * @param system_id ID of this system @@ -119,7 +176,7 @@ static inline uint16_t mavlink_msg_gimbal_manager_information_pack(uint8_t syste * @param msg The MAVLink message to compress the data into * @param time_boot_ms [ms] Timestamp (time since system boot). * @param cap_flags Bitmap of gimbal capability flags. - * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. Component ID of gimbal device (or 1-6 for non-MAVLink gimbal). * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) * @param pitch_min [rad] Minimum pitch angle (positive: up, negative: down) @@ -191,13 +248,27 @@ static inline uint16_t mavlink_msg_gimbal_manager_information_encode_chan(uint8_ return mavlink_msg_gimbal_manager_information_pack_chan(system_id, component_id, chan, msg, gimbal_manager_information->time_boot_ms, gimbal_manager_information->cap_flags, gimbal_manager_information->gimbal_device_id, gimbal_manager_information->roll_min, gimbal_manager_information->roll_max, gimbal_manager_information->pitch_min, gimbal_manager_information->pitch_max, gimbal_manager_information->yaw_min, gimbal_manager_information->yaw_max); } +/** + * @brief Encode a gimbal_manager_information struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_information_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_gimbal_manager_information_t* gimbal_manager_information) +{ + return mavlink_msg_gimbal_manager_information_pack_status(system_id, component_id, _status, msg, gimbal_manager_information->time_boot_ms, gimbal_manager_information->cap_flags, gimbal_manager_information->gimbal_device_id, gimbal_manager_information->roll_min, gimbal_manager_information->roll_max, gimbal_manager_information->pitch_min, gimbal_manager_information->pitch_max, gimbal_manager_information->yaw_min, gimbal_manager_information->yaw_max); +} + /** * @brief Send a gimbal_manager_information message * @param chan MAVLink channel to send the message * * @param time_boot_ms [ms] Timestamp (time since system boot). * @param cap_flags Bitmap of gimbal capability flags. - * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. Component ID of gimbal device (or 1-6 for non-MAVLink gimbal). * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) * @param pitch_min [rad] Minimum pitch angle (positive: up, negative: down) @@ -254,7 +325,7 @@ static inline void mavlink_msg_gimbal_manager_information_send_struct(mavlink_ch #if MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -320,7 +391,7 @@ static inline uint32_t mavlink_msg_gimbal_manager_information_get_cap_flags(cons /** * @brief Get field gimbal_device_id from gimbal_manager_information message * - * @return Gimbal device ID that this gimbal manager is responsible for. + * @return Gimbal device ID that this gimbal manager is responsible for. Component ID of gimbal device (or 1-6 for non-MAVLink gimbal). */ static inline uint8_t mavlink_msg_gimbal_manager_information_get_gimbal_device_id(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_attitude.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_attitude.h index 828f1e6a3fd..6388ebada81 100644 --- a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_attitude.h +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_attitude.h @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_gimbal_manager_set_attitude_pack(uint8_t syst packet.target_system = target_system; packet.target_component = target_component; packet.gimbal_device_id = gimbal_device_id; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN); #endif @@ -103,6 +103,58 @@ static inline uint16_t mavlink_msg_gimbal_manager_set_attitude_pack(uint8_t syst return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC); } +/** + * @brief Pack a gimbal_manager_set_attitude message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags to use. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_MANAGER_FLAGS_YAW_LOCK is set) + * @param angular_velocity_x [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. + * @param angular_velocity_y [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. + * @param angular_velocity_z [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_attitude_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 20, angular_velocity_x); + _mav_put_float(buf, 24, angular_velocity_y); + _mav_put_float(buf, 28, angular_velocity_z); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, gimbal_device_id); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN); +#else + mavlink_gimbal_manager_set_attitude_t packet; + packet.flags = flags; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN); +#endif +} + /** * @brief Pack a gimbal_manager_set_attitude message on a channel * @param system_id ID of this system @@ -143,7 +195,7 @@ static inline uint16_t mavlink_msg_gimbal_manager_set_attitude_pack_chan(uint8_t packet.target_system = target_system; packet.target_component = target_component; packet.gimbal_device_id = gimbal_device_id; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN); #endif @@ -178,6 +230,20 @@ static inline uint16_t mavlink_msg_gimbal_manager_set_attitude_encode_chan(uint8 return mavlink_msg_gimbal_manager_set_attitude_pack_chan(system_id, component_id, chan, msg, gimbal_manager_set_attitude->target_system, gimbal_manager_set_attitude->target_component, gimbal_manager_set_attitude->flags, gimbal_manager_set_attitude->gimbal_device_id, gimbal_manager_set_attitude->q, gimbal_manager_set_attitude->angular_velocity_x, gimbal_manager_set_attitude->angular_velocity_y, gimbal_manager_set_attitude->angular_velocity_z); } +/** + * @brief Encode a gimbal_manager_set_attitude struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_set_attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_attitude_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_gimbal_manager_set_attitude_t* gimbal_manager_set_attitude) +{ + return mavlink_msg_gimbal_manager_set_attitude_pack_status(system_id, component_id, _status, msg, gimbal_manager_set_attitude->target_system, gimbal_manager_set_attitude->target_component, gimbal_manager_set_attitude->flags, gimbal_manager_set_attitude->gimbal_device_id, gimbal_manager_set_attitude->q, gimbal_manager_set_attitude->angular_velocity_x, gimbal_manager_set_attitude->angular_velocity_y, gimbal_manager_set_attitude->angular_velocity_z); +} + /** * @brief Send a gimbal_manager_set_attitude message * @param chan MAVLink channel to send the message @@ -215,7 +281,7 @@ static inline void mavlink_msg_gimbal_manager_set_attitude_send(mavlink_channel_ packet.target_system = target_system; packet.target_component = target_component; packet.gimbal_device_id = gimbal_device_id; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC); #endif } @@ -236,7 +302,7 @@ static inline void mavlink_msg_gimbal_manager_set_attitude_send_struct(mavlink_c #if MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -264,7 +330,7 @@ static inline void mavlink_msg_gimbal_manager_set_attitude_send_buf(mavlink_mess packet->target_system = target_system; packet->target_component = target_component; packet->gimbal_device_id = gimbal_device_id; - mav_array_memcpy(packet->q, q, sizeof(float)*4); + mav_array_assign_float(packet->q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_manual_control.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_manual_control.h index d3096c49bb9..2705ce924ac 100644 --- a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_manual_control.h +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_manual_control.h @@ -105,6 +105,60 @@ static inline uint16_t mavlink_msg_gimbal_manager_set_manual_control_pack(uint8_ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_CRC); } +/** + * @brief Pack a gimbal_manager_set_manual_control message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param pitch Pitch angle unitless (-1..1, positive: up, negative: down, NaN to be ignored). + * @param yaw Yaw angle unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + * @param pitch_rate Pitch angular rate unitless (-1..1, positive: up, negative: down, NaN to be ignored). + * @param yaw_rate Yaw angular rate unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_manual_control_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, float pitch, float yaw, float pitch_rate, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, pitch_rate); + _mav_put_float(buf, 16, yaw_rate); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 22, gimbal_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN); +#else + mavlink_gimbal_manager_set_manual_control_t packet; + packet.flags = flags; + packet.pitch = pitch; + packet.yaw = yaw; + packet.pitch_rate = pitch_rate; + packet.yaw_rate = yaw_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN); +#endif +} + /** * @brief Pack a gimbal_manager_set_manual_control message on a channel * @param system_id ID of this system @@ -182,6 +236,20 @@ static inline uint16_t mavlink_msg_gimbal_manager_set_manual_control_encode_chan return mavlink_msg_gimbal_manager_set_manual_control_pack_chan(system_id, component_id, chan, msg, gimbal_manager_set_manual_control->target_system, gimbal_manager_set_manual_control->target_component, gimbal_manager_set_manual_control->flags, gimbal_manager_set_manual_control->gimbal_device_id, gimbal_manager_set_manual_control->pitch, gimbal_manager_set_manual_control->yaw, gimbal_manager_set_manual_control->pitch_rate, gimbal_manager_set_manual_control->yaw_rate); } +/** + * @brief Encode a gimbal_manager_set_manual_control struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_set_manual_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_manual_control_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_gimbal_manager_set_manual_control_t* gimbal_manager_set_manual_control) +{ + return mavlink_msg_gimbal_manager_set_manual_control_pack_status(system_id, component_id, _status, msg, gimbal_manager_set_manual_control->target_system, gimbal_manager_set_manual_control->target_component, gimbal_manager_set_manual_control->flags, gimbal_manager_set_manual_control->gimbal_device_id, gimbal_manager_set_manual_control->pitch, gimbal_manager_set_manual_control->yaw, gimbal_manager_set_manual_control->pitch_rate, gimbal_manager_set_manual_control->yaw_rate); +} + /** * @brief Send a gimbal_manager_set_manual_control message * @param chan MAVLink channel to send the message @@ -242,7 +310,7 @@ static inline void mavlink_msg_gimbal_manager_set_manual_control_send_struct(mav #if MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_pitchyaw.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_pitchyaw.h index de6a43b1d3c..01ef99fbe38 100644 --- a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_pitchyaw.h +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_pitchyaw.h @@ -105,6 +105,60 @@ static inline uint16_t mavlink_msg_gimbal_manager_set_pitchyaw_pack(uint8_t syst return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_CRC); } +/** + * @brief Pack a gimbal_manager_set_pitchyaw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags to use. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param pitch [rad] Pitch angle (positive: up, negative: down, NaN to be ignored). + * @param yaw [rad] Yaw angle (positive: to the right, negative: to the left, NaN to be ignored). + * @param pitch_rate [rad/s] Pitch angular rate (positive: up, negative: down, NaN to be ignored). + * @param yaw_rate [rad/s] Yaw angular rate (positive: to the right, negative: to the left, NaN to be ignored). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_pitchyaw_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, float pitch, float yaw, float pitch_rate, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, pitch_rate); + _mav_put_float(buf, 16, yaw_rate); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 22, gimbal_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN); +#else + mavlink_gimbal_manager_set_pitchyaw_t packet; + packet.flags = flags; + packet.pitch = pitch; + packet.yaw = yaw; + packet.pitch_rate = pitch_rate; + packet.yaw_rate = yaw_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN); +#endif +} + /** * @brief Pack a gimbal_manager_set_pitchyaw message on a channel * @param system_id ID of this system @@ -182,6 +236,20 @@ static inline uint16_t mavlink_msg_gimbal_manager_set_pitchyaw_encode_chan(uint8 return mavlink_msg_gimbal_manager_set_pitchyaw_pack_chan(system_id, component_id, chan, msg, gimbal_manager_set_pitchyaw->target_system, gimbal_manager_set_pitchyaw->target_component, gimbal_manager_set_pitchyaw->flags, gimbal_manager_set_pitchyaw->gimbal_device_id, gimbal_manager_set_pitchyaw->pitch, gimbal_manager_set_pitchyaw->yaw, gimbal_manager_set_pitchyaw->pitch_rate, gimbal_manager_set_pitchyaw->yaw_rate); } +/** + * @brief Encode a gimbal_manager_set_pitchyaw struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_set_pitchyaw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_pitchyaw_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_gimbal_manager_set_pitchyaw_t* gimbal_manager_set_pitchyaw) +{ + return mavlink_msg_gimbal_manager_set_pitchyaw_pack_status(system_id, component_id, _status, msg, gimbal_manager_set_pitchyaw->target_system, gimbal_manager_set_pitchyaw->target_component, gimbal_manager_set_pitchyaw->flags, gimbal_manager_set_pitchyaw->gimbal_device_id, gimbal_manager_set_pitchyaw->pitch, gimbal_manager_set_pitchyaw->yaw, gimbal_manager_set_pitchyaw->pitch_rate, gimbal_manager_set_pitchyaw->yaw_rate); +} + /** * @brief Send a gimbal_manager_set_pitchyaw message * @param chan MAVLink channel to send the message @@ -242,7 +310,7 @@ static inline void mavlink_msg_gimbal_manager_set_pitchyaw_send_struct(mavlink_c #if MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_status.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_status.h index 4eace068bb1..e659c49b860 100644 --- a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_status.h @@ -7,7 +7,7 @@ typedef struct __mavlink_gimbal_manager_status_t { uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ uint32_t flags; /*< High level gimbal manager flags currently applied.*/ - uint8_t gimbal_device_id; /*< Gimbal device ID that this gimbal manager is responsible for.*/ + uint8_t gimbal_device_id; /*< Gimbal device ID that this gimbal manager is responsible for. Component ID of gimbal device (or 1-6 for non-MAVLink gimbal).*/ uint8_t primary_control_sysid; /*< System ID of MAVLink component with primary control, 0 for none.*/ uint8_t primary_control_compid; /*< Component ID of MAVLink component with primary control, 0 for none.*/ uint8_t secondary_control_sysid; /*< System ID of MAVLink component with secondary control, 0 for none.*/ @@ -61,7 +61,7 @@ typedef struct __mavlink_gimbal_manager_status_t { * * @param time_boot_ms [ms] Timestamp (time since system boot). * @param flags High level gimbal manager flags currently applied. - * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. Component ID of gimbal device (or 1-6 for non-MAVLink gimbal). * @param primary_control_sysid System ID of MAVLink component with primary control, 0 for none. * @param primary_control_compid Component ID of MAVLink component with primary control, 0 for none. * @param secondary_control_sysid System ID of MAVLink component with secondary control, 0 for none. @@ -99,6 +99,57 @@ static inline uint16_t mavlink_msg_gimbal_manager_status_pack(uint8_t system_id, return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_CRC); } +/** + * @brief Pack a gimbal_manager_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param flags High level gimbal manager flags currently applied. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. Component ID of gimbal device (or 1-6 for non-MAVLink gimbal). + * @param primary_control_sysid System ID of MAVLink component with primary control, 0 for none. + * @param primary_control_compid Component ID of MAVLink component with primary control, 0 for none. + * @param secondary_control_sysid System ID of MAVLink component with secondary control, 0 for none. + * @param secondary_control_compid Component ID of MAVLink component with secondary control, 0 for none. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint32_t flags, uint8_t gimbal_device_id, uint8_t primary_control_sysid, uint8_t primary_control_compid, uint8_t secondary_control_sysid, uint8_t secondary_control_compid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, flags); + _mav_put_uint8_t(buf, 8, gimbal_device_id); + _mav_put_uint8_t(buf, 9, primary_control_sysid); + _mav_put_uint8_t(buf, 10, primary_control_compid); + _mav_put_uint8_t(buf, 11, secondary_control_sysid); + _mav_put_uint8_t(buf, 12, secondary_control_compid); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN); +#else + mavlink_gimbal_manager_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.flags = flags; + packet.gimbal_device_id = gimbal_device_id; + packet.primary_control_sysid = primary_control_sysid; + packet.primary_control_compid = primary_control_compid; + packet.secondary_control_sysid = secondary_control_sysid; + packet.secondary_control_compid = secondary_control_compid; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN); +#endif +} + /** * @brief Pack a gimbal_manager_status message on a channel * @param system_id ID of this system @@ -107,7 +158,7 @@ static inline uint16_t mavlink_msg_gimbal_manager_status_pack(uint8_t system_id, * @param msg The MAVLink message to compress the data into * @param time_boot_ms [ms] Timestamp (time since system boot). * @param flags High level gimbal manager flags currently applied. - * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. Component ID of gimbal device (or 1-6 for non-MAVLink gimbal). * @param primary_control_sysid System ID of MAVLink component with primary control, 0 for none. * @param primary_control_compid Component ID of MAVLink component with primary control, 0 for none. * @param secondary_control_sysid System ID of MAVLink component with secondary control, 0 for none. @@ -173,13 +224,27 @@ static inline uint16_t mavlink_msg_gimbal_manager_status_encode_chan(uint8_t sys return mavlink_msg_gimbal_manager_status_pack_chan(system_id, component_id, chan, msg, gimbal_manager_status->time_boot_ms, gimbal_manager_status->flags, gimbal_manager_status->gimbal_device_id, gimbal_manager_status->primary_control_sysid, gimbal_manager_status->primary_control_compid, gimbal_manager_status->secondary_control_sysid, gimbal_manager_status->secondary_control_compid); } +/** + * @brief Encode a gimbal_manager_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_gimbal_manager_status_t* gimbal_manager_status) +{ + return mavlink_msg_gimbal_manager_status_pack_status(system_id, component_id, _status, msg, gimbal_manager_status->time_boot_ms, gimbal_manager_status->flags, gimbal_manager_status->gimbal_device_id, gimbal_manager_status->primary_control_sysid, gimbal_manager_status->primary_control_compid, gimbal_manager_status->secondary_control_sysid, gimbal_manager_status->secondary_control_compid); +} + /** * @brief Send a gimbal_manager_status message * @param chan MAVLink channel to send the message * * @param time_boot_ms [ms] Timestamp (time since system boot). * @param flags High level gimbal manager flags currently applied. - * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. Component ID of gimbal device (or 1-6 for non-MAVLink gimbal). * @param primary_control_sysid System ID of MAVLink component with primary control, 0 for none. * @param primary_control_compid Component ID of MAVLink component with primary control, 0 for none. * @param secondary_control_sysid System ID of MAVLink component with secondary control, 0 for none. @@ -230,7 +295,7 @@ static inline void mavlink_msg_gimbal_manager_status_send_struct(mavlink_channel #if MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -292,7 +357,7 @@ static inline uint32_t mavlink_msg_gimbal_manager_status_get_flags(const mavlink /** * @brief Get field gimbal_device_id from gimbal_manager_status message * - * @return Gimbal device ID that this gimbal manager is responsible for. + * @return Gimbal device ID that this gimbal manager is responsible for. Component ID of gimbal device (or 1-6 for non-MAVLink gimbal). */ static inline uint8_t mavlink_msg_gimbal_manager_status_get_gimbal_device_id(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_global_position_int.h b/lib/main/MAVLink/common/mavlink_msg_global_position_int.h index 8b601f23477..84482700408 100755 --- a/lib/main/MAVLink/common/mavlink_msg_global_position_int.h +++ b/lib/main/MAVLink/common/mavlink_msg_global_position_int.h @@ -9,7 +9,7 @@ typedef struct __mavlink_global_position_int_t { int32_t lat; /*< [degE7] Latitude, expressed*/ int32_t lon; /*< [degE7] Longitude, expressed*/ int32_t alt; /*< [mm] Altitude (MSL). Note that virtually all GPS modules provide both WGS84 and MSL.*/ - int32_t relative_alt; /*< [mm] Altitude above ground*/ + int32_t relative_alt; /*< [mm] Altitude above home*/ int16_t vx; /*< [cm/s] Ground X Speed (Latitude, positive north)*/ int16_t vy; /*< [cm/s] Ground Y Speed (Longitude, positive east)*/ int16_t vz; /*< [cm/s] Ground Z Speed (Altitude, positive down)*/ @@ -69,7 +69,7 @@ typedef struct __mavlink_global_position_int_t { * @param lat [degE7] Latitude, expressed * @param lon [degE7] Longitude, expressed * @param alt [mm] Altitude (MSL). Note that virtually all GPS modules provide both WGS84 and MSL. - * @param relative_alt [mm] Altitude above ground + * @param relative_alt [mm] Altitude above home * @param vx [cm/s] Ground X Speed (Latitude, positive north) * @param vy [cm/s] Ground Y Speed (Longitude, positive east) * @param vz [cm/s] Ground Z Speed (Altitude, positive down) @@ -111,6 +111,63 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); } +/** + * @brief Pack a global_position_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param lat [degE7] Latitude, expressed + * @param lon [degE7] Longitude, expressed + * @param alt [mm] Altitude (MSL). Note that virtually all GPS modules provide both WGS84 and MSL. + * @param relative_alt [mm] Altitude above home + * @param vx [cm/s] Ground X Speed (Latitude, positive north) + * @param vy [cm/s] Ground Y Speed (Longitude, positive east) + * @param vz [cm/s] Ground Z Speed (Altitude, positive down) + * @param hdg [cdeg] Vehicle heading (yaw angle), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_global_position_int_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_int32_t(buf, 12, alt); + _mav_put_int32_t(buf, 16, relative_alt); + _mav_put_int16_t(buf, 20, vx); + _mav_put_int16_t(buf, 22, vy); + _mav_put_int16_t(buf, 24, vz); + _mav_put_uint16_t(buf, 26, hdg); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#else + mavlink_global_position_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.hdg = hdg; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#endif +} + /** * @brief Pack a global_position_int message on a channel * @param system_id ID of this system @@ -121,7 +178,7 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u * @param lat [degE7] Latitude, expressed * @param lon [degE7] Longitude, expressed * @param alt [mm] Altitude (MSL). Note that virtually all GPS modules provide both WGS84 and MSL. - * @param relative_alt [mm] Altitude above ground + * @param relative_alt [mm] Altitude above home * @param vx [cm/s] Ground X Speed (Latitude, positive north) * @param vy [cm/s] Ground Y Speed (Longitude, positive east) * @param vz [cm/s] Ground Z Speed (Altitude, positive down) @@ -191,6 +248,20 @@ static inline uint16_t mavlink_msg_global_position_int_encode_chan(uint8_t syste return mavlink_msg_global_position_int_pack_chan(system_id, component_id, chan, msg, global_position_int->time_boot_ms, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->relative_alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg); } +/** + * @brief Encode a global_position_int struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param global_position_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_position_int_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_global_position_int_t* global_position_int) +{ + return mavlink_msg_global_position_int_pack_status(system_id, component_id, _status, msg, global_position_int->time_boot_ms, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->relative_alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg); +} + /** * @brief Send a global_position_int message * @param chan MAVLink channel to send the message @@ -199,7 +270,7 @@ static inline uint16_t mavlink_msg_global_position_int_encode_chan(uint8_t syste * @param lat [degE7] Latitude, expressed * @param lon [degE7] Longitude, expressed * @param alt [mm] Altitude (MSL). Note that virtually all GPS modules provide both WGS84 and MSL. - * @param relative_alt [mm] Altitude above ground + * @param relative_alt [mm] Altitude above home * @param vx [cm/s] Ground X Speed (Latitude, positive north) * @param vy [cm/s] Ground Y Speed (Longitude, positive east) * @param vz [cm/s] Ground Z Speed (Altitude, positive down) @@ -254,7 +325,7 @@ static inline void mavlink_msg_global_position_int_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -340,7 +411,7 @@ static inline int32_t mavlink_msg_global_position_int_get_alt(const mavlink_mess /** * @brief Get field relative_alt from global_position_int message * - * @return [mm] Altitude above ground + * @return [mm] Altitude above home */ static inline int32_t mavlink_msg_global_position_int_get_relative_alt(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_global_position_int_cov.h b/lib/main/MAVLink/common/mavlink_msg_global_position_int_cov.h index 3cb44be787c..4e1ab8a987b 100755 --- a/lib/main/MAVLink/common/mavlink_msg_global_position_int_cov.h +++ b/lib/main/MAVLink/common/mavlink_msg_global_position_int_cov.h @@ -107,7 +107,7 @@ static inline uint16_t mavlink_msg_global_position_int_cov_pack(uint8_t system_i packet.vy = vy; packet.vz = vz; packet.estimator_type = estimator_type; - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*36); + mav_array_assign_float(packet.covariance, covariance, 36); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); #endif @@ -115,6 +115,64 @@ static inline uint16_t mavlink_msg_global_position_int_cov_pack(uint8_t system_i return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_CRC); } +/** + * @brief Pack a global_position_int_cov message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param estimator_type Class id of the estimator this estimate originated from. + * @param lat [degE7] Latitude + * @param lon [degE7] Longitude + * @param alt [mm] Altitude in meters above MSL + * @param relative_alt [mm] Altitude above ground + * @param vx [m/s] Ground X Speed (Latitude) + * @param vy [m/s] Ground Y Speed (Longitude) + * @param vz [m/s] Ground Z Speed (Altitude) + * @param covariance Row-major representation of a 6x6 position and velocity 6x6 cross-covariance matrix (states: lat, lon, alt, vx, vy, vz; first six entries are the first ROW, next six entries are the second row, etc.). If unknown, assign NaN value to first element in the array. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_global_position_int_cov_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint8_t estimator_type, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, float vx, float vy, float vz, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_int32_t(buf, 20, relative_alt); + _mav_put_float(buf, 24, vx); + _mav_put_float(buf, 28, vy); + _mav_put_float(buf, 32, vz); + _mav_put_uint8_t(buf, 180, estimator_type); + _mav_put_float_array(buf, 36, covariance, 36); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#else + mavlink_global_position_int_cov_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*36); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); +#endif +} + /** * @brief Pack a global_position_int_cov message on a channel * @param system_id ID of this system @@ -161,7 +219,7 @@ static inline uint16_t mavlink_msg_global_position_int_cov_pack_chan(uint8_t sys packet.vy = vy; packet.vz = vz; packet.estimator_type = estimator_type; - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*36); + mav_array_assign_float(packet.covariance, covariance, 36); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN); #endif @@ -196,6 +254,20 @@ static inline uint16_t mavlink_msg_global_position_int_cov_encode_chan(uint8_t s return mavlink_msg_global_position_int_cov_pack_chan(system_id, component_id, chan, msg, global_position_int_cov->time_usec, global_position_int_cov->estimator_type, global_position_int_cov->lat, global_position_int_cov->lon, global_position_int_cov->alt, global_position_int_cov->relative_alt, global_position_int_cov->vx, global_position_int_cov->vy, global_position_int_cov->vz, global_position_int_cov->covariance); } +/** + * @brief Encode a global_position_int_cov struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param global_position_int_cov C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_position_int_cov_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_global_position_int_cov_t* global_position_int_cov) +{ + return mavlink_msg_global_position_int_cov_pack_status(system_id, component_id, _status, msg, global_position_int_cov->time_usec, global_position_int_cov->estimator_type, global_position_int_cov->lat, global_position_int_cov->lon, global_position_int_cov->alt, global_position_int_cov->relative_alt, global_position_int_cov->vx, global_position_int_cov->vy, global_position_int_cov->vz, global_position_int_cov->covariance); +} + /** * @brief Send a global_position_int_cov message * @param chan MAVLink channel to send the message @@ -239,7 +311,7 @@ static inline void mavlink_msg_global_position_int_cov_send(mavlink_channel_t ch packet.vy = vy; packet.vz = vz; packet.estimator_type = estimator_type; - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*36); + mav_array_assign_float(packet.covariance, covariance, 36); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_CRC); #endif } @@ -260,7 +332,7 @@ static inline void mavlink_msg_global_position_int_cov_send_struct(mavlink_chann #if MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -292,7 +364,7 @@ static inline void mavlink_msg_global_position_int_cov_send_buf(mavlink_message_ packet->vy = vy; packet->vz = vz; packet->estimator_type = estimator_type; - mav_array_memcpy(packet->covariance, covariance, sizeof(float)*36); + mav_array_assign_float(packet->covariance, covariance, 36); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_COV_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_global_vision_position_estimate.h b/lib/main/MAVLink/common/mavlink_msg_global_vision_position_estimate.h index 94a3d05dd4c..30ab385a612 100755 --- a/lib/main/MAVLink/common/mavlink_msg_global_vision_position_estimate.h +++ b/lib/main/MAVLink/common/mavlink_msg_global_vision_position_estimate.h @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t packet.pitch = pitch; packet.yaw = yaw; packet.reset_counter = reset_counter; - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); + mav_array_assign_float(packet.covariance, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); #endif @@ -109,6 +109,61 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); } +/** + * @brief Pack a global_vision_position_estimate message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param usec [us] Timestamp (UNIX time or since system boot) + * @param x [m] Global X position + * @param y [m] Global Y position + * @param z [m] Global Z position + * @param roll [rad] Roll angle + * @param pitch [rad] Pitch angle + * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x_global, y_global, z_global, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance, uint8_t reset_counter) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + _mav_put_uint8_t(buf, 116, reset_counter); + _mav_put_float_array(buf, 32, covariance, 21); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#else + mavlink_global_vision_position_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#endif +} + /** * @brief Pack a global_vision_position_estimate message on a channel * @param system_id ID of this system @@ -152,7 +207,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_chan(uin packet.pitch = pitch; packet.yaw = yaw; packet.reset_counter = reset_counter; - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); + mav_array_assign_float(packet.covariance, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); #endif @@ -187,6 +242,20 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_encode_chan(u return mavlink_msg_global_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw, global_vision_position_estimate->covariance, global_vision_position_estimate->reset_counter); } +/** + * @brief Encode a global_vision_position_estimate struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param global_vision_position_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_vision_position_estimate_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_global_vision_position_estimate_t* global_vision_position_estimate) +{ + return mavlink_msg_global_vision_position_estimate_pack_status(system_id, component_id, _status, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw, global_vision_position_estimate->covariance, global_vision_position_estimate->reset_counter); +} + /** * @brief Send a global_vision_position_estimate message * @param chan MAVLink channel to send the message @@ -227,7 +296,7 @@ static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_chan packet.pitch = pitch; packet.yaw = yaw; packet.reset_counter = reset_counter; - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); + mav_array_assign_float(packet.covariance, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); #endif } @@ -248,7 +317,7 @@ static inline void mavlink_msg_global_vision_position_estimate_send_struct(mavli #if MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -278,7 +347,7 @@ static inline void mavlink_msg_global_vision_position_estimate_send_buf(mavlink_ packet->pitch = pitch; packet->yaw = yaw; packet->reset_counter = reset_counter; - mav_array_memcpy(packet->covariance, covariance, sizeof(float)*21); + mav_array_assign_float(packet->covariance, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_gps2_raw.h b/lib/main/MAVLink/common/mavlink_msg_gps2_raw.h index 5e741ef4930..7c74b9804ba 100755 --- a/lib/main/MAVLink/common/mavlink_msg_gps2_raw.h +++ b/lib/main/MAVLink/common/mavlink_msg_gps2_raw.h @@ -10,19 +10,24 @@ typedef struct __mavlink_gps2_raw_t { int32_t lon; /*< [degE7] Longitude (WGS84)*/ int32_t alt; /*< [mm] Altitude (MSL). Positive for up.*/ uint32_t dgps_age; /*< [ms] Age of DGPS info*/ - uint16_t eph; /*< GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX*/ - uint16_t epv; /*< GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX*/ + uint16_t eph; /*< GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX*/ + uint16_t epv; /*< GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX*/ uint16_t vel; /*< [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX*/ uint16_t cog; /*< [cdeg] Course over ground (NOT heading, but direction of movement): 0.0..359.99 degrees. If unknown, set to: UINT16_MAX*/ uint8_t fix_type; /*< GPS fix type.*/ - uint8_t satellites_visible; /*< Number of satellites visible. If unknown, set to 255*/ + uint8_t satellites_visible; /*< Number of satellites visible. If unknown, set to UINT8_MAX*/ uint8_t dgps_numch; /*< Number of DGPS satellites*/ - uint16_t yaw; /*< [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north.*/ + uint16_t yaw; /*< [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use UINT16_MAX if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north.*/ + int32_t alt_ellipsoid; /*< [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up.*/ + uint32_t h_acc; /*< [mm] Position uncertainty.*/ + uint32_t v_acc; /*< [mm] Altitude uncertainty.*/ + uint32_t vel_acc; /*< [mm/s] Speed uncertainty.*/ + uint32_t hdg_acc; /*< [degE5] Heading / track uncertainty*/ }) mavlink_gps2_raw_t; -#define MAVLINK_MSG_ID_GPS2_RAW_LEN 37 +#define MAVLINK_MSG_ID_GPS2_RAW_LEN 57 #define MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN 35 -#define MAVLINK_MSG_ID_124_LEN 37 +#define MAVLINK_MSG_ID_124_LEN 57 #define MAVLINK_MSG_ID_124_MIN_LEN 35 #define MAVLINK_MSG_ID_GPS2_RAW_CRC 87 @@ -34,7 +39,7 @@ typedef struct __mavlink_gps2_raw_t { #define MAVLINK_MESSAGE_INFO_GPS2_RAW { \ 124, \ "GPS2_RAW", \ - 13, \ + 18, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps2_raw_t, time_usec) }, \ { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps2_raw_t, fix_type) }, \ { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps2_raw_t, lat) }, \ @@ -48,12 +53,17 @@ typedef struct __mavlink_gps2_raw_t { { "dgps_numch", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps2_raw_t, dgps_numch) }, \ { "dgps_age", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps2_raw_t, dgps_age) }, \ { "yaw", NULL, MAVLINK_TYPE_UINT16_T, 0, 35, offsetof(mavlink_gps2_raw_t, yaw) }, \ + { "alt_ellipsoid", NULL, MAVLINK_TYPE_INT32_T, 0, 37, offsetof(mavlink_gps2_raw_t, alt_ellipsoid) }, \ + { "h_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 41, offsetof(mavlink_gps2_raw_t, h_acc) }, \ + { "v_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 45, offsetof(mavlink_gps2_raw_t, v_acc) }, \ + { "vel_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 49, offsetof(mavlink_gps2_raw_t, vel_acc) }, \ + { "hdg_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 53, offsetof(mavlink_gps2_raw_t, hdg_acc) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_GPS2_RAW { \ "GPS2_RAW", \ - 13, \ + 18, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps2_raw_t, time_usec) }, \ { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps2_raw_t, fix_type) }, \ { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps2_raw_t, lat) }, \ @@ -67,6 +77,11 @@ typedef struct __mavlink_gps2_raw_t { { "dgps_numch", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps2_raw_t, dgps_numch) }, \ { "dgps_age", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps2_raw_t, dgps_age) }, \ { "yaw", NULL, MAVLINK_TYPE_UINT16_T, 0, 35, offsetof(mavlink_gps2_raw_t, yaw) }, \ + { "alt_ellipsoid", NULL, MAVLINK_TYPE_INT32_T, 0, 37, offsetof(mavlink_gps2_raw_t, alt_ellipsoid) }, \ + { "h_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 41, offsetof(mavlink_gps2_raw_t, h_acc) }, \ + { "v_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 45, offsetof(mavlink_gps2_raw_t, v_acc) }, \ + { "vel_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 49, offsetof(mavlink_gps2_raw_t, vel_acc) }, \ + { "hdg_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 53, offsetof(mavlink_gps2_raw_t, hdg_acc) }, \ } \ } #endif @@ -82,18 +97,23 @@ typedef struct __mavlink_gps2_raw_t { * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [mm] Altitude (MSL). Positive for up. - * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX - * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param eph GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param cog [cdeg] Course over ground (NOT heading, but direction of movement): 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param satellites_visible Number of satellites visible. If unknown, set to UINT8_MAX * @param dgps_numch Number of DGPS satellites * @param dgps_age [ms] Age of DGPS info - * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use UINT16_MAX if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + * @param alt_ellipsoid [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. + * @param h_acc [mm] Position uncertainty. + * @param v_acc [mm] Altitude uncertainty. + * @param vel_acc [mm/s] Speed uncertainty. + * @param hdg_acc [degE5] Heading / track uncertainty * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age, uint16_t yaw) + uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age, uint16_t yaw, int32_t alt_ellipsoid, uint32_t h_acc, uint32_t v_acc, uint32_t vel_acc, uint32_t hdg_acc) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; @@ -110,6 +130,11 @@ static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t comp _mav_put_uint8_t(buf, 33, satellites_visible); _mav_put_uint8_t(buf, 34, dgps_numch); _mav_put_uint16_t(buf, 35, yaw); + _mav_put_int32_t(buf, 37, alt_ellipsoid); + _mav_put_uint32_t(buf, 41, h_acc); + _mav_put_uint32_t(buf, 45, v_acc); + _mav_put_uint32_t(buf, 49, vel_acc); + _mav_put_uint32_t(buf, 53, hdg_acc); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); #else @@ -127,6 +152,11 @@ static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t comp packet.satellites_visible = satellites_visible; packet.dgps_numch = dgps_numch; packet.yaw = yaw; + packet.alt_ellipsoid = alt_ellipsoid; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_acc = vel_acc; + packet.hdg_acc = hdg_acc; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); #endif @@ -135,6 +165,90 @@ static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t comp return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); } +/** + * @brief Pack a gps2_raw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param fix_type GPS fix type. + * @param lat [degE7] Latitude (WGS84) + * @param lon [degE7] Longitude (WGS84) + * @param alt [mm] Altitude (MSL). Positive for up. + * @param eph GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX + * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX + * @param cog [cdeg] Course over ground (NOT heading, but direction of movement): 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @param satellites_visible Number of satellites visible. If unknown, set to UINT8_MAX + * @param dgps_numch Number of DGPS satellites + * @param dgps_age [ms] Age of DGPS info + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use UINT16_MAX if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + * @param alt_ellipsoid [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. + * @param h_acc [mm] Position uncertainty. + * @param v_acc [mm] Altitude uncertainty. + * @param vel_acc [mm/s] Speed uncertainty. + * @param hdg_acc [degE5] Heading / track uncertainty + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps2_raw_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age, uint16_t yaw, int32_t alt_ellipsoid, uint32_t h_acc, uint32_t v_acc, uint32_t vel_acc, uint32_t hdg_acc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint32_t(buf, 20, dgps_age); + _mav_put_uint16_t(buf, 24, eph); + _mav_put_uint16_t(buf, 26, epv); + _mav_put_uint16_t(buf, 28, vel); + _mav_put_uint16_t(buf, 30, cog); + _mav_put_uint8_t(buf, 32, fix_type); + _mav_put_uint8_t(buf, 33, satellites_visible); + _mav_put_uint8_t(buf, 34, dgps_numch); + _mav_put_uint16_t(buf, 35, yaw); + _mav_put_int32_t(buf, 37, alt_ellipsoid); + _mav_put_uint32_t(buf, 41, h_acc); + _mav_put_uint32_t(buf, 45, v_acc); + _mav_put_uint32_t(buf, 49, vel_acc); + _mav_put_uint32_t(buf, 53, hdg_acc); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#else + mavlink_gps2_raw_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.dgps_age = dgps_age; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + packet.dgps_numch = dgps_numch; + packet.yaw = yaw; + packet.alt_ellipsoid = alt_ellipsoid; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_acc = vel_acc; + packet.hdg_acc = hdg_acc; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS2_RAW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +} + /** * @brief Pack a gps2_raw message on a channel * @param system_id ID of this system @@ -146,19 +260,24 @@ static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t comp * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [mm] Altitude (MSL). Positive for up. - * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX - * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param eph GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param cog [cdeg] Course over ground (NOT heading, but direction of movement): 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param satellites_visible Number of satellites visible. If unknown, set to UINT8_MAX * @param dgps_numch Number of DGPS satellites * @param dgps_age [ms] Age of DGPS info - * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use UINT16_MAX if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + * @param alt_ellipsoid [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. + * @param h_acc [mm] Position uncertainty. + * @param v_acc [mm] Altitude uncertainty. + * @param vel_acc [mm/s] Speed uncertainty. + * @param hdg_acc [degE5] Heading / track uncertainty * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible,uint8_t dgps_numch,uint32_t dgps_age,uint16_t yaw) + uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible,uint8_t dgps_numch,uint32_t dgps_age,uint16_t yaw,int32_t alt_ellipsoid,uint32_t h_acc,uint32_t v_acc,uint32_t vel_acc,uint32_t hdg_acc) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; @@ -175,6 +294,11 @@ static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t _mav_put_uint8_t(buf, 33, satellites_visible); _mav_put_uint8_t(buf, 34, dgps_numch); _mav_put_uint16_t(buf, 35, yaw); + _mav_put_int32_t(buf, 37, alt_ellipsoid); + _mav_put_uint32_t(buf, 41, h_acc); + _mav_put_uint32_t(buf, 45, v_acc); + _mav_put_uint32_t(buf, 49, vel_acc); + _mav_put_uint32_t(buf, 53, hdg_acc); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); #else @@ -192,6 +316,11 @@ static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t packet.satellites_visible = satellites_visible; packet.dgps_numch = dgps_numch; packet.yaw = yaw; + packet.alt_ellipsoid = alt_ellipsoid; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_acc = vel_acc; + packet.hdg_acc = hdg_acc; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); #endif @@ -210,7 +339,7 @@ static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_gps2_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw) { - return mavlink_msg_gps2_raw_pack(system_id, component_id, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age, gps2_raw->yaw); + return mavlink_msg_gps2_raw_pack(system_id, component_id, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age, gps2_raw->yaw, gps2_raw->alt_ellipsoid, gps2_raw->h_acc, gps2_raw->v_acc, gps2_raw->vel_acc, gps2_raw->hdg_acc); } /** @@ -224,7 +353,21 @@ static inline uint16_t mavlink_msg_gps2_raw_encode(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_gps2_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw) { - return mavlink_msg_gps2_raw_pack_chan(system_id, component_id, chan, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age, gps2_raw->yaw); + return mavlink_msg_gps2_raw_pack_chan(system_id, component_id, chan, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age, gps2_raw->yaw, gps2_raw->alt_ellipsoid, gps2_raw->h_acc, gps2_raw->v_acc, gps2_raw->vel_acc, gps2_raw->hdg_acc); +} + +/** + * @brief Encode a gps2_raw struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param gps2_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps2_raw_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw) +{ + return mavlink_msg_gps2_raw_pack_status(system_id, component_id, _status, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age, gps2_raw->yaw, gps2_raw->alt_ellipsoid, gps2_raw->h_acc, gps2_raw->v_acc, gps2_raw->vel_acc, gps2_raw->hdg_acc); } /** @@ -236,18 +379,23 @@ static inline uint16_t mavlink_msg_gps2_raw_encode_chan(uint8_t system_id, uint8 * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [mm] Altitude (MSL). Positive for up. - * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX - * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param eph GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param cog [cdeg] Course over ground (NOT heading, but direction of movement): 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param satellites_visible Number of satellites visible. If unknown, set to UINT8_MAX * @param dgps_numch Number of DGPS satellites * @param dgps_age [ms] Age of DGPS info - * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use UINT16_MAX if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + * @param alt_ellipsoid [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. + * @param h_acc [mm] Position uncertainty. + * @param v_acc [mm] Altitude uncertainty. + * @param vel_acc [mm/s] Speed uncertainty. + * @param hdg_acc [degE5] Heading / track uncertainty */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age, uint16_t yaw) +static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age, uint16_t yaw, int32_t alt_ellipsoid, uint32_t h_acc, uint32_t v_acc, uint32_t vel_acc, uint32_t hdg_acc) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; @@ -264,6 +412,11 @@ static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t ti _mav_put_uint8_t(buf, 33, satellites_visible); _mav_put_uint8_t(buf, 34, dgps_numch); _mav_put_uint16_t(buf, 35, yaw); + _mav_put_int32_t(buf, 37, alt_ellipsoid); + _mav_put_uint32_t(buf, 41, h_acc); + _mav_put_uint32_t(buf, 45, v_acc); + _mav_put_uint32_t(buf, 49, vel_acc); + _mav_put_uint32_t(buf, 53, hdg_acc); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); #else @@ -281,6 +434,11 @@ static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t ti packet.satellites_visible = satellites_visible; packet.dgps_numch = dgps_numch; packet.yaw = yaw; + packet.alt_ellipsoid = alt_ellipsoid; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_acc = vel_acc; + packet.hdg_acc = hdg_acc; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); #endif @@ -294,7 +452,7 @@ static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t ti static inline void mavlink_msg_gps2_raw_send_struct(mavlink_channel_t chan, const mavlink_gps2_raw_t* gps2_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_gps2_raw_send(chan, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age, gps2_raw->yaw); + mavlink_msg_gps2_raw_send(chan, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age, gps2_raw->yaw, gps2_raw->alt_ellipsoid, gps2_raw->h_acc, gps2_raw->v_acc, gps2_raw->vel_acc, gps2_raw->hdg_acc); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)gps2_raw, MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); #endif @@ -302,13 +460,13 @@ static inline void mavlink_msg_gps2_raw_send_struct(mavlink_channel_t chan, cons #if MAVLINK_MSG_ID_GPS2_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_gps2_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age, uint16_t yaw) +static inline void mavlink_msg_gps2_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age, uint16_t yaw, int32_t alt_ellipsoid, uint32_t h_acc, uint32_t v_acc, uint32_t vel_acc, uint32_t hdg_acc) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -325,6 +483,11 @@ static inline void mavlink_msg_gps2_raw_send_buf(mavlink_message_t *msgbuf, mavl _mav_put_uint8_t(buf, 33, satellites_visible); _mav_put_uint8_t(buf, 34, dgps_numch); _mav_put_uint16_t(buf, 35, yaw); + _mav_put_int32_t(buf, 37, alt_ellipsoid); + _mav_put_uint32_t(buf, 41, h_acc); + _mav_put_uint32_t(buf, 45, v_acc); + _mav_put_uint32_t(buf, 49, vel_acc); + _mav_put_uint32_t(buf, 53, hdg_acc); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); #else @@ -342,6 +505,11 @@ static inline void mavlink_msg_gps2_raw_send_buf(mavlink_message_t *msgbuf, mavl packet->satellites_visible = satellites_visible; packet->dgps_numch = dgps_numch; packet->yaw = yaw; + packet->alt_ellipsoid = alt_ellipsoid; + packet->h_acc = h_acc; + packet->v_acc = v_acc; + packet->vel_acc = vel_acc; + packet->hdg_acc = hdg_acc; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)packet, MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); #endif @@ -406,7 +574,7 @@ static inline int32_t mavlink_msg_gps2_raw_get_alt(const mavlink_message_t* msg) /** * @brief Get field eph from gps2_raw message * - * @return GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @return GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps2_raw_get_eph(const mavlink_message_t* msg) { @@ -416,7 +584,7 @@ static inline uint16_t mavlink_msg_gps2_raw_get_eph(const mavlink_message_t* msg /** * @brief Get field epv from gps2_raw message * - * @return GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX + * @return GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps2_raw_get_epv(const mavlink_message_t* msg) { @@ -446,7 +614,7 @@ static inline uint16_t mavlink_msg_gps2_raw_get_cog(const mavlink_message_t* msg /** * @brief Get field satellites_visible from gps2_raw message * - * @return Number of satellites visible. If unknown, set to 255 + * @return Number of satellites visible. If unknown, set to UINT8_MAX */ static inline uint8_t mavlink_msg_gps2_raw_get_satellites_visible(const mavlink_message_t* msg) { @@ -476,13 +644,63 @@ static inline uint32_t mavlink_msg_gps2_raw_get_dgps_age(const mavlink_message_t /** * @brief Get field yaw from gps2_raw message * - * @return [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + * @return [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use UINT16_MAX if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. */ static inline uint16_t mavlink_msg_gps2_raw_get_yaw(const mavlink_message_t* msg) { return _MAV_RETURN_uint16_t(msg, 35); } +/** + * @brief Get field alt_ellipsoid from gps2_raw message + * + * @return [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. + */ +static inline int32_t mavlink_msg_gps2_raw_get_alt_ellipsoid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 37); +} + +/** + * @brief Get field h_acc from gps2_raw message + * + * @return [mm] Position uncertainty. + */ +static inline uint32_t mavlink_msg_gps2_raw_get_h_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 41); +} + +/** + * @brief Get field v_acc from gps2_raw message + * + * @return [mm] Altitude uncertainty. + */ +static inline uint32_t mavlink_msg_gps2_raw_get_v_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 45); +} + +/** + * @brief Get field vel_acc from gps2_raw message + * + * @return [mm/s] Speed uncertainty. + */ +static inline uint32_t mavlink_msg_gps2_raw_get_vel_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 49); +} + +/** + * @brief Get field hdg_acc from gps2_raw message + * + * @return [degE5] Heading / track uncertainty + */ +static inline uint32_t mavlink_msg_gps2_raw_get_hdg_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 53); +} + /** * @brief Decode a gps2_raw message into a struct * @@ -505,6 +723,11 @@ static inline void mavlink_msg_gps2_raw_decode(const mavlink_message_t* msg, mav gps2_raw->satellites_visible = mavlink_msg_gps2_raw_get_satellites_visible(msg); gps2_raw->dgps_numch = mavlink_msg_gps2_raw_get_dgps_numch(msg); gps2_raw->yaw = mavlink_msg_gps2_raw_get_yaw(msg); + gps2_raw->alt_ellipsoid = mavlink_msg_gps2_raw_get_alt_ellipsoid(msg); + gps2_raw->h_acc = mavlink_msg_gps2_raw_get_h_acc(msg); + gps2_raw->v_acc = mavlink_msg_gps2_raw_get_v_acc(msg); + gps2_raw->vel_acc = mavlink_msg_gps2_raw_get_vel_acc(msg); + gps2_raw->hdg_acc = mavlink_msg_gps2_raw_get_hdg_acc(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_GPS2_RAW_LEN? msg->len : MAVLINK_MSG_ID_GPS2_RAW_LEN; memset(gps2_raw, 0, MAVLINK_MSG_ID_GPS2_RAW_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_gps2_rtk.h b/lib/main/MAVLink/common/mavlink_msg_gps2_rtk.h index 2af9b53ca72..a26f3c46918 100755 --- a/lib/main/MAVLink/common/mavlink_msg_gps2_rtk.h +++ b/lib/main/MAVLink/common/mavlink_msg_gps2_rtk.h @@ -135,6 +135,75 @@ static inline uint16_t mavlink_msg_gps2_rtk_pack(uint8_t system_id, uint8_t comp return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); } +/** + * @brief Pack a gps2_rtk message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_last_baseline_ms [ms] Time since boot of last baseline message received. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow [ms] GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate [Hz] Rate of baseline messages being received by GPS + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline + * @param baseline_a_mm [mm] Current baseline in ECEF x or NED north component. + * @param baseline_b_mm [mm] Current baseline in ECEF y or NED east component. + * @param baseline_c_mm [mm] Current baseline in ECEF z or NED down component. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps2_rtk_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS2_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#else + mavlink_gps2_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS2_RTK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS2_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS2_RTK_LEN, MAVLINK_MSG_ID_GPS2_RTK_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS2_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS2_RTK_LEN); +#endif +} + /** * @brief Pack a gps2_rtk message on a channel * @param system_id ID of this system @@ -227,6 +296,20 @@ static inline uint16_t mavlink_msg_gps2_rtk_encode_chan(uint8_t system_id, uint8 return mavlink_msg_gps2_rtk_pack_chan(system_id, component_id, chan, msg, gps2_rtk->time_last_baseline_ms, gps2_rtk->rtk_receiver_id, gps2_rtk->wn, gps2_rtk->tow, gps2_rtk->rtk_health, gps2_rtk->rtk_rate, gps2_rtk->nsats, gps2_rtk->baseline_coords_type, gps2_rtk->baseline_a_mm, gps2_rtk->baseline_b_mm, gps2_rtk->baseline_c_mm, gps2_rtk->accuracy, gps2_rtk->iar_num_hypotheses); } +/** + * @brief Encode a gps2_rtk struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param gps2_rtk C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps2_rtk_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_gps2_rtk_t* gps2_rtk) +{ + return mavlink_msg_gps2_rtk_pack_status(system_id, component_id, _status, msg, gps2_rtk->time_last_baseline_ms, gps2_rtk->rtk_receiver_id, gps2_rtk->wn, gps2_rtk->tow, gps2_rtk->rtk_health, gps2_rtk->rtk_rate, gps2_rtk->nsats, gps2_rtk->baseline_coords_type, gps2_rtk->baseline_a_mm, gps2_rtk->baseline_b_mm, gps2_rtk->baseline_c_mm, gps2_rtk->accuracy, gps2_rtk->iar_num_hypotheses); +} + /** * @brief Send a gps2_rtk message * @param chan MAVLink channel to send the message @@ -302,7 +385,7 @@ static inline void mavlink_msg_gps2_rtk_send_struct(mavlink_channel_t chan, cons #if MAVLINK_MSG_ID_GPS2_RTK_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_gps_global_origin.h b/lib/main/MAVLink/common/mavlink_msg_gps_global_origin.h index ccbde8fd14c..4a1be69e6dc 100755 --- a/lib/main/MAVLink/common/mavlink_msg_gps_global_origin.h +++ b/lib/main/MAVLink/common/mavlink_msg_gps_global_origin.h @@ -81,6 +81,48 @@ static inline uint16_t mavlink_msg_gps_global_origin_pack(uint8_t system_id, uin return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); } +/** + * @brief Pack a gps_global_origin message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param latitude [degE7] Latitude (WGS84) + * @param longitude [degE7] Longitude (WGS84) + * @param altitude [mm] Altitude (MSL). Positive for up. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_global_origin_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + int32_t latitude, int32_t longitude, int32_t altitude, uint64_t time_usec) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint64_t(buf, 12, time_usec); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#else + mavlink_gps_global_origin_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.time_usec = time_usec; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#endif +} + /** * @brief Pack a gps_global_origin message on a channel * @param system_id ID of this system @@ -146,6 +188,20 @@ static inline uint16_t mavlink_msg_gps_global_origin_encode_chan(uint8_t system_ return mavlink_msg_gps_global_origin_pack_chan(system_id, component_id, chan, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude, gps_global_origin->time_usec); } +/** + * @brief Encode a gps_global_origin struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param gps_global_origin C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_global_origin_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_gps_global_origin_t* gps_global_origin) +{ + return mavlink_msg_gps_global_origin_pack_status(system_id, component_id, _status, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude, gps_global_origin->time_usec); +} + /** * @brief Send a gps_global_origin message * @param chan MAVLink channel to send the message @@ -194,7 +250,7 @@ static inline void mavlink_msg_gps_global_origin_send_struct(mavlink_channel_t c #if MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_gps_inject_data.h b/lib/main/MAVLink/common/mavlink_msg_gps_inject_data.h index ff6dc0ee914..53d5fd6de29 100755 --- a/lib/main/MAVLink/common/mavlink_msg_gps_inject_data.h +++ b/lib/main/MAVLink/common/mavlink_msg_gps_inject_data.h @@ -71,7 +71,7 @@ static inline uint16_t mavlink_msg_gps_inject_data_pack(uint8_t system_id, uint8 packet.target_system = target_system; packet.target_component = target_component; packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110); + mav_array_assign_uint8_t(packet.data, data, 110); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); #endif @@ -79,6 +79,46 @@ static inline uint16_t mavlink_msg_gps_inject_data_pack(uint8_t system_id, uint8 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_INJECT_DATA_MIN_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); } +/** + * @brief Pack a gps_inject_data message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param len [bytes] Data length + * @param data Raw data (110 is enough for 12 satellites of RTCMv2) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_inject_data_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, len); + _mav_put_uint8_t_array(buf, 3, data, 110); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#else + mavlink_gps_inject_data_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_INJECT_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS_INJECT_DATA_MIN_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS_INJECT_DATA_MIN_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +} + /** * @brief Pack a gps_inject_data message on a channel * @param system_id ID of this system @@ -107,7 +147,7 @@ static inline uint16_t mavlink_msg_gps_inject_data_pack_chan(uint8_t system_id, packet.target_system = target_system; packet.target_component = target_component; packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110); + mav_array_assign_uint8_t(packet.data, data, 110); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); #endif @@ -142,6 +182,20 @@ static inline uint16_t mavlink_msg_gps_inject_data_encode_chan(uint8_t system_id return mavlink_msg_gps_inject_data_pack_chan(system_id, component_id, chan, msg, gps_inject_data->target_system, gps_inject_data->target_component, gps_inject_data->len, gps_inject_data->data); } +/** + * @brief Encode a gps_inject_data struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param gps_inject_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_inject_data_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_gps_inject_data_t* gps_inject_data) +{ + return mavlink_msg_gps_inject_data_pack_status(system_id, component_id, _status, msg, gps_inject_data->target_system, gps_inject_data->target_component, gps_inject_data->len, gps_inject_data->data); +} + /** * @brief Send a gps_inject_data message * @param chan MAVLink channel to send the message @@ -167,7 +221,7 @@ static inline void mavlink_msg_gps_inject_data_send(mavlink_channel_t chan, uint packet.target_system = target_system; packet.target_component = target_component; packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110); + mav_array_assign_uint8_t(packet.data, data, 110); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)&packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_MIN_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); #endif } @@ -188,7 +242,7 @@ static inline void mavlink_msg_gps_inject_data_send_struct(mavlink_channel_t cha #if MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -208,7 +262,7 @@ static inline void mavlink_msg_gps_inject_data_send_buf(mavlink_message_t *msgbu packet->target_system = target_system; packet->target_component = target_component; packet->len = len; - mav_array_memcpy(packet->data, data, sizeof(uint8_t)*110); + mav_array_assign_uint8_t(packet->data, data, 110); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_MIN_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_gps_input.h b/lib/main/MAVLink/common/mavlink_msg_gps_input.h index de76ddbf1d5..2c509834326 100755 --- a/lib/main/MAVLink/common/mavlink_msg_gps_input.h +++ b/lib/main/MAVLink/common/mavlink_msg_gps_input.h @@ -171,6 +171,93 @@ static inline uint16_t mavlink_msg_gps_input_pack(uint8_t system_id, uint8_t com return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_INPUT_MIN_LEN, MAVLINK_MSG_ID_GPS_INPUT_LEN, MAVLINK_MSG_ID_GPS_INPUT_CRC); } +/** + * @brief Pack a gps_input message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param gps_id ID of the GPS for multiple GPS inputs + * @param ignore_flags Bitmap indicating which GPS input flags fields to ignore. All other fields must be provided. + * @param time_week_ms [ms] GPS time (from start of GPS week) + * @param time_week GPS week number + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. 4: 3D with DGPS. 5: 3D with RTK + * @param lat [degE7] Latitude (WGS84) + * @param lon [degE7] Longitude (WGS84) + * @param alt [m] Altitude (MSL). Positive for up. + * @param hdop GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param vdop GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param vn [m/s] GPS velocity in north direction in earth-fixed NED frame + * @param ve [m/s] GPS velocity in east direction in earth-fixed NED frame + * @param vd [m/s] GPS velocity in down direction in earth-fixed NED frame + * @param speed_accuracy [m/s] GPS speed accuracy + * @param horiz_accuracy [m] GPS horizontal accuracy + * @param vert_accuracy [m] GPS vertical accuracy + * @param satellites_visible Number of satellites visible. + * @param yaw [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_input_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint8_t gps_id, uint16_t ignore_flags, uint32_t time_week_ms, uint16_t time_week, uint8_t fix_type, int32_t lat, int32_t lon, float alt, float hdop, float vdop, float vn, float ve, float vd, float speed_accuracy, float horiz_accuracy, float vert_accuracy, uint8_t satellites_visible, uint16_t yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_INPUT_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, time_week_ms); + _mav_put_int32_t(buf, 12, lat); + _mav_put_int32_t(buf, 16, lon); + _mav_put_float(buf, 20, alt); + _mav_put_float(buf, 24, hdop); + _mav_put_float(buf, 28, vdop); + _mav_put_float(buf, 32, vn); + _mav_put_float(buf, 36, ve); + _mav_put_float(buf, 40, vd); + _mav_put_float(buf, 44, speed_accuracy); + _mav_put_float(buf, 48, horiz_accuracy); + _mav_put_float(buf, 52, vert_accuracy); + _mav_put_uint16_t(buf, 56, ignore_flags); + _mav_put_uint16_t(buf, 58, time_week); + _mav_put_uint8_t(buf, 60, gps_id); + _mav_put_uint8_t(buf, 61, fix_type); + _mav_put_uint8_t(buf, 62, satellites_visible); + _mav_put_uint16_t(buf, 63, yaw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INPUT_LEN); +#else + mavlink_gps_input_t packet; + packet.time_usec = time_usec; + packet.time_week_ms = time_week_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.hdop = hdop; + packet.vdop = vdop; + packet.vn = vn; + packet.ve = ve; + packet.vd = vd; + packet.speed_accuracy = speed_accuracy; + packet.horiz_accuracy = horiz_accuracy; + packet.vert_accuracy = vert_accuracy; + packet.ignore_flags = ignore_flags; + packet.time_week = time_week; + packet.gps_id = gps_id; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INPUT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_INPUT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS_INPUT_MIN_LEN, MAVLINK_MSG_ID_GPS_INPUT_LEN, MAVLINK_MSG_ID_GPS_INPUT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS_INPUT_MIN_LEN, MAVLINK_MSG_ID_GPS_INPUT_LEN); +#endif +} + /** * @brief Pack a gps_input message on a channel * @param system_id ID of this system @@ -281,6 +368,20 @@ static inline uint16_t mavlink_msg_gps_input_encode_chan(uint8_t system_id, uint return mavlink_msg_gps_input_pack_chan(system_id, component_id, chan, msg, gps_input->time_usec, gps_input->gps_id, gps_input->ignore_flags, gps_input->time_week_ms, gps_input->time_week, gps_input->fix_type, gps_input->lat, gps_input->lon, gps_input->alt, gps_input->hdop, gps_input->vdop, gps_input->vn, gps_input->ve, gps_input->vd, gps_input->speed_accuracy, gps_input->horiz_accuracy, gps_input->vert_accuracy, gps_input->satellites_visible, gps_input->yaw); } +/** + * @brief Encode a gps_input struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param gps_input C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_input_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_gps_input_t* gps_input) +{ + return mavlink_msg_gps_input_pack_status(system_id, component_id, _status, msg, gps_input->time_usec, gps_input->gps_id, gps_input->ignore_flags, gps_input->time_week_ms, gps_input->time_week, gps_input->fix_type, gps_input->lat, gps_input->lon, gps_input->alt, gps_input->hdop, gps_input->vdop, gps_input->vn, gps_input->ve, gps_input->vd, gps_input->speed_accuracy, gps_input->horiz_accuracy, gps_input->vert_accuracy, gps_input->satellites_visible, gps_input->yaw); +} + /** * @brief Send a gps_input message * @param chan MAVLink channel to send the message @@ -374,7 +475,7 @@ static inline void mavlink_msg_gps_input_send_struct(mavlink_channel_t chan, con #if MAVLINK_MSG_ID_GPS_INPUT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_gps_raw_int.h b/lib/main/MAVLink/common/mavlink_msg_gps_raw_int.h index b87d82e4897..473900882d0 100755 --- a/lib/main/MAVLink/common/mavlink_msg_gps_raw_int.h +++ b/lib/main/MAVLink/common/mavlink_msg_gps_raw_int.h @@ -9,18 +9,18 @@ typedef struct __mavlink_gps_raw_int_t { int32_t lat; /*< [degE7] Latitude (WGS84, EGM96 ellipsoid)*/ int32_t lon; /*< [degE7] Longitude (WGS84, EGM96 ellipsoid)*/ int32_t alt; /*< [mm] Altitude (MSL). Positive for up. Note that virtually all GPS modules provide the MSL altitude in addition to the WGS84 altitude.*/ - uint16_t eph; /*< GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX*/ - uint16_t epv; /*< GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX*/ + uint16_t eph; /*< GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX*/ + uint16_t epv; /*< GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX*/ uint16_t vel; /*< [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX*/ uint16_t cog; /*< [cdeg] Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX*/ uint8_t fix_type; /*< GPS fix type.*/ - uint8_t satellites_visible; /*< Number of satellites visible. If unknown, set to 255*/ + uint8_t satellites_visible; /*< Number of satellites visible. If unknown, set to UINT8_MAX*/ int32_t alt_ellipsoid; /*< [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up.*/ uint32_t h_acc; /*< [mm] Position uncertainty.*/ uint32_t v_acc; /*< [mm] Altitude uncertainty.*/ - uint32_t vel_acc; /*< [mm] Speed uncertainty.*/ + uint32_t vel_acc; /*< [mm/s] Speed uncertainty.*/ uint32_t hdg_acc; /*< [degE5] Heading / track uncertainty*/ - uint16_t yaw; /*< [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north.*/ + uint16_t yaw; /*< [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use UINT16_MAX if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north.*/ }) mavlink_gps_raw_int_t; #define MAVLINK_MSG_ID_GPS_RAW_INT_LEN 52 @@ -91,17 +91,17 @@ typedef struct __mavlink_gps_raw_int_t { * @param lat [degE7] Latitude (WGS84, EGM96 ellipsoid) * @param lon [degE7] Longitude (WGS84, EGM96 ellipsoid) * @param alt [mm] Altitude (MSL). Positive for up. Note that virtually all GPS modules provide the MSL altitude in addition to the WGS84 altitude. - * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX - * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param eph GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param cog [cdeg] Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param satellites_visible Number of satellites visible. If unknown, set to UINT8_MAX * @param alt_ellipsoid [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. * @param h_acc [mm] Position uncertainty. * @param v_acc [mm] Altitude uncertainty. - * @param vel_acc [mm] Speed uncertainty. + * @param vel_acc [mm/s] Speed uncertainty. * @param hdg_acc [degE5] Heading / track uncertainty - * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use UINT16_MAX if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -153,6 +153,84 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RAW_INT_MIN_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); } +/** + * @brief Pack a gps_raw_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param fix_type GPS fix type. + * @param lat [degE7] Latitude (WGS84, EGM96 ellipsoid) + * @param lon [degE7] Longitude (WGS84, EGM96 ellipsoid) + * @param alt [mm] Altitude (MSL). Positive for up. Note that virtually all GPS modules provide the MSL altitude in addition to the WGS84 altitude. + * @param eph GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX + * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX + * @param cog [cdeg] Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @param satellites_visible Number of satellites visible. If unknown, set to UINT8_MAX + * @param alt_ellipsoid [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. + * @param h_acc [mm] Position uncertainty. + * @param v_acc [mm] Altitude uncertainty. + * @param vel_acc [mm/s] Speed uncertainty. + * @param hdg_acc [degE5] Heading / track uncertainty + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use UINT16_MAX if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_raw_int_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, int32_t alt_ellipsoid, uint32_t h_acc, uint32_t v_acc, uint32_t vel_acc, uint32_t hdg_acc, uint16_t yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_RAW_INT_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_uint16_t(buf, 26, cog); + _mav_put_uint8_t(buf, 28, fix_type); + _mav_put_uint8_t(buf, 29, satellites_visible); + _mav_put_int32_t(buf, 30, alt_ellipsoid); + _mav_put_uint32_t(buf, 34, h_acc); + _mav_put_uint32_t(buf, 38, v_acc); + _mav_put_uint32_t(buf, 42, vel_acc); + _mav_put_uint32_t(buf, 46, hdg_acc); + _mav_put_uint16_t(buf, 50, yaw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#else + mavlink_gps_raw_int_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + packet.alt_ellipsoid = alt_ellipsoid; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_acc = vel_acc; + packet.hdg_acc = hdg_acc; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS_RAW_INT_MIN_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS_RAW_INT_MIN_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#endif +} + /** * @brief Pack a gps_raw_int message on a channel * @param system_id ID of this system @@ -164,17 +242,17 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c * @param lat [degE7] Latitude (WGS84, EGM96 ellipsoid) * @param lon [degE7] Longitude (WGS84, EGM96 ellipsoid) * @param alt [mm] Altitude (MSL). Positive for up. Note that virtually all GPS modules provide the MSL altitude in addition to the WGS84 altitude. - * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX - * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param eph GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param cog [cdeg] Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param satellites_visible Number of satellites visible. If unknown, set to UINT8_MAX * @param alt_ellipsoid [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. * @param h_acc [mm] Position uncertainty. * @param v_acc [mm] Altitude uncertainty. - * @param vel_acc [mm] Speed uncertainty. + * @param vel_acc [mm/s] Speed uncertainty. * @param hdg_acc [degE5] Heading / track uncertainty - * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use UINT16_MAX if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -254,6 +332,20 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode_chan(uint8_t system_id, ui return mavlink_msg_gps_raw_int_pack_chan(system_id, component_id, chan, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible, gps_raw_int->alt_ellipsoid, gps_raw_int->h_acc, gps_raw_int->v_acc, gps_raw_int->vel_acc, gps_raw_int->hdg_acc, gps_raw_int->yaw); } +/** + * @brief Encode a gps_raw_int struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param gps_raw_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_raw_int_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_gps_raw_int_t* gps_raw_int) +{ + return mavlink_msg_gps_raw_int_pack_status(system_id, component_id, _status, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible, gps_raw_int->alt_ellipsoid, gps_raw_int->h_acc, gps_raw_int->v_acc, gps_raw_int->vel_acc, gps_raw_int->hdg_acc, gps_raw_int->yaw); +} + /** * @brief Send a gps_raw_int message * @param chan MAVLink channel to send the message @@ -263,17 +355,17 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode_chan(uint8_t system_id, ui * @param lat [degE7] Latitude (WGS84, EGM96 ellipsoid) * @param lon [degE7] Longitude (WGS84, EGM96 ellipsoid) * @param alt [mm] Altitude (MSL). Positive for up. Note that virtually all GPS modules provide the MSL altitude in addition to the WGS84 altitude. - * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX - * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param eph GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param cog [cdeg] Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX - * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param satellites_visible Number of satellites visible. If unknown, set to UINT8_MAX * @param alt_ellipsoid [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. * @param h_acc [mm] Position uncertainty. * @param v_acc [mm] Altitude uncertainty. - * @param vel_acc [mm] Speed uncertainty. + * @param vel_acc [mm/s] Speed uncertainty. * @param hdg_acc [degE5] Heading / track uncertainty - * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use UINT16_MAX if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -338,7 +430,7 @@ static inline void mavlink_msg_gps_raw_int_send_struct(mavlink_channel_t chan, c #if MAVLINK_MSG_ID_GPS_RAW_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -448,7 +540,7 @@ static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* m /** * @brief Get field eph from gps_raw_int message * - * @return GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @return GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* msg) { @@ -458,7 +550,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* /** * @brief Get field epv from gps_raw_int message * - * @return GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX + * @return GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg) { @@ -488,7 +580,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_cog(const mavlink_message_t* /** * @brief Get field satellites_visible from gps_raw_int message * - * @return Number of satellites visible. If unknown, set to 255 + * @return Number of satellites visible. If unknown, set to UINT8_MAX */ static inline uint8_t mavlink_msg_gps_raw_int_get_satellites_visible(const mavlink_message_t* msg) { @@ -528,7 +620,7 @@ static inline uint32_t mavlink_msg_gps_raw_int_get_v_acc(const mavlink_message_t /** * @brief Get field vel_acc from gps_raw_int message * - * @return [mm] Speed uncertainty. + * @return [mm/s] Speed uncertainty. */ static inline uint32_t mavlink_msg_gps_raw_int_get_vel_acc(const mavlink_message_t* msg) { @@ -548,7 +640,7 @@ static inline uint32_t mavlink_msg_gps_raw_int_get_hdg_acc(const mavlink_message /** * @brief Get field yaw from gps_raw_int message * - * @return [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + * @return [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use UINT16_MAX if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. */ static inline uint16_t mavlink_msg_gps_raw_int_get_yaw(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_gps_rtcm_data.h b/lib/main/MAVLink/common/mavlink_msg_gps_rtcm_data.h index 393d6adb31c..72791b24f7a 100755 --- a/lib/main/MAVLink/common/mavlink_msg_gps_rtcm_data.h +++ b/lib/main/MAVLink/common/mavlink_msg_gps_rtcm_data.h @@ -65,7 +65,7 @@ static inline uint16_t mavlink_msg_gps_rtcm_data_pack(uint8_t system_id, uint8_t mavlink_gps_rtcm_data_t packet; packet.flags = flags; packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*180); + mav_array_assign_uint8_t(packet.data, data, 180); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RTCM_DATA_LEN); #endif @@ -73,6 +73,43 @@ static inline uint16_t mavlink_msg_gps_rtcm_data_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RTCM_DATA_MIN_LEN, MAVLINK_MSG_ID_GPS_RTCM_DATA_LEN, MAVLINK_MSG_ID_GPS_RTCM_DATA_CRC); } +/** + * @brief Pack a gps_rtcm_data message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param flags LSB: 1 means message is fragmented, next 2 bits are the fragment ID, the remaining 5 bits are used for the sequence ID. Messages are only to be flushed to the GPS when the entire message has been reconstructed on the autopilot. The fragment ID specifies which order the fragments should be assembled into a buffer, while the sequence ID is used to detect a mismatch between different buffers. The buffer is considered fully reconstructed when either all 4 fragments are present, or all the fragments before the first fragment with a non full payload is received. This management is used to ensure that normal GPS operation doesn't corrupt RTCM data, and to recover from a unreliable transport delivery order. + * @param len [bytes] data length + * @param data RTCM message (may be fragmented) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_rtcm_data_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t flags, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_RTCM_DATA_LEN]; + _mav_put_uint8_t(buf, 0, flags); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 180); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RTCM_DATA_LEN); +#else + mavlink_gps_rtcm_data_t packet; + packet.flags = flags; + packet.len = len; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*180); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RTCM_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_RTCM_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS_RTCM_DATA_MIN_LEN, MAVLINK_MSG_ID_GPS_RTCM_DATA_LEN, MAVLINK_MSG_ID_GPS_RTCM_DATA_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS_RTCM_DATA_MIN_LEN, MAVLINK_MSG_ID_GPS_RTCM_DATA_LEN); +#endif +} + /** * @brief Pack a gps_rtcm_data message on a channel * @param system_id ID of this system @@ -98,7 +135,7 @@ static inline uint16_t mavlink_msg_gps_rtcm_data_pack_chan(uint8_t system_id, ui mavlink_gps_rtcm_data_t packet; packet.flags = flags; packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*180); + mav_array_assign_uint8_t(packet.data, data, 180); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RTCM_DATA_LEN); #endif @@ -133,6 +170,20 @@ static inline uint16_t mavlink_msg_gps_rtcm_data_encode_chan(uint8_t system_id, return mavlink_msg_gps_rtcm_data_pack_chan(system_id, component_id, chan, msg, gps_rtcm_data->flags, gps_rtcm_data->len, gps_rtcm_data->data); } +/** + * @brief Encode a gps_rtcm_data struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param gps_rtcm_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_rtcm_data_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_gps_rtcm_data_t* gps_rtcm_data) +{ + return mavlink_msg_gps_rtcm_data_pack_status(system_id, component_id, _status, msg, gps_rtcm_data->flags, gps_rtcm_data->len, gps_rtcm_data->data); +} + /** * @brief Send a gps_rtcm_data message * @param chan MAVLink channel to send the message @@ -155,7 +206,7 @@ static inline void mavlink_msg_gps_rtcm_data_send(mavlink_channel_t chan, uint8_ mavlink_gps_rtcm_data_t packet; packet.flags = flags; packet.len = len; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*180); + mav_array_assign_uint8_t(packet.data, data, 180); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTCM_DATA, (const char *)&packet, MAVLINK_MSG_ID_GPS_RTCM_DATA_MIN_LEN, MAVLINK_MSG_ID_GPS_RTCM_DATA_LEN, MAVLINK_MSG_ID_GPS_RTCM_DATA_CRC); #endif } @@ -176,7 +227,7 @@ static inline void mavlink_msg_gps_rtcm_data_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_GPS_RTCM_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -194,7 +245,7 @@ static inline void mavlink_msg_gps_rtcm_data_send_buf(mavlink_message_t *msgbuf, mavlink_gps_rtcm_data_t *packet = (mavlink_gps_rtcm_data_t *)msgbuf; packet->flags = flags; packet->len = len; - mav_array_memcpy(packet->data, data, sizeof(uint8_t)*180); + mav_array_assign_uint8_t(packet->data, data, 180); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RTCM_DATA, (const char *)packet, MAVLINK_MSG_ID_GPS_RTCM_DATA_MIN_LEN, MAVLINK_MSG_ID_GPS_RTCM_DATA_LEN, MAVLINK_MSG_ID_GPS_RTCM_DATA_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_gps_rtk.h b/lib/main/MAVLink/common/mavlink_msg_gps_rtk.h index abf898f2d4a..478a4d88ee0 100755 --- a/lib/main/MAVLink/common/mavlink_msg_gps_rtk.h +++ b/lib/main/MAVLink/common/mavlink_msg_gps_rtk.h @@ -135,6 +135,75 @@ static inline uint16_t mavlink_msg_gps_rtk_pack(uint8_t system_id, uint8_t compo return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); } +/** + * @brief Pack a gps_rtk message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_last_baseline_ms [ms] Time since boot of last baseline message received. + * @param rtk_receiver_id Identification of connected RTK receiver. + * @param wn GPS Week Number of last baseline + * @param tow [ms] GPS Time of Week of last baseline + * @param rtk_health GPS-specific health report for RTK data. + * @param rtk_rate [Hz] Rate of baseline messages being received by GPS + * @param nsats Current number of sats used for RTK calculation. + * @param baseline_coords_type Coordinate system of baseline + * @param baseline_a_mm [mm] Current baseline in ECEF x or NED north component. + * @param baseline_b_mm [mm] Current baseline in ECEF y or NED east component. + * @param baseline_c_mm [mm] Current baseline in ECEF z or NED down component. + * @param accuracy Current estimate of baseline accuracy. + * @param iar_num_hypotheses Current number of integer ambiguity hypotheses. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_rtk_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_last_baseline_ms, uint8_t rtk_receiver_id, uint16_t wn, uint32_t tow, uint8_t rtk_health, uint8_t rtk_rate, uint8_t nsats, uint8_t baseline_coords_type, int32_t baseline_a_mm, int32_t baseline_b_mm, int32_t baseline_c_mm, uint32_t accuracy, int32_t iar_num_hypotheses) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_RTK_LEN]; + _mav_put_uint32_t(buf, 0, time_last_baseline_ms); + _mav_put_uint32_t(buf, 4, tow); + _mav_put_int32_t(buf, 8, baseline_a_mm); + _mav_put_int32_t(buf, 12, baseline_b_mm); + _mav_put_int32_t(buf, 16, baseline_c_mm); + _mav_put_uint32_t(buf, 20, accuracy); + _mav_put_int32_t(buf, 24, iar_num_hypotheses); + _mav_put_uint16_t(buf, 28, wn); + _mav_put_uint8_t(buf, 30, rtk_receiver_id); + _mav_put_uint8_t(buf, 31, rtk_health); + _mav_put_uint8_t(buf, 32, rtk_rate); + _mav_put_uint8_t(buf, 33, nsats); + _mav_put_uint8_t(buf, 34, baseline_coords_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RTK_LEN); +#else + mavlink_gps_rtk_t packet; + packet.time_last_baseline_ms = time_last_baseline_ms; + packet.tow = tow; + packet.baseline_a_mm = baseline_a_mm; + packet.baseline_b_mm = baseline_b_mm; + packet.baseline_c_mm = baseline_c_mm; + packet.accuracy = accuracy; + packet.iar_num_hypotheses = iar_num_hypotheses; + packet.wn = wn; + packet.rtk_receiver_id = rtk_receiver_id; + packet.rtk_health = rtk_health; + packet.rtk_rate = rtk_rate; + packet.nsats = nsats; + packet.baseline_coords_type = baseline_coords_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_RTK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS_RTK_LEN, MAVLINK_MSG_ID_GPS_RTK_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS_RTK_MIN_LEN, MAVLINK_MSG_ID_GPS_RTK_LEN); +#endif +} + /** * @brief Pack a gps_rtk message on a channel * @param system_id ID of this system @@ -227,6 +296,20 @@ static inline uint16_t mavlink_msg_gps_rtk_encode_chan(uint8_t system_id, uint8_ return mavlink_msg_gps_rtk_pack_chan(system_id, component_id, chan, msg, gps_rtk->time_last_baseline_ms, gps_rtk->rtk_receiver_id, gps_rtk->wn, gps_rtk->tow, gps_rtk->rtk_health, gps_rtk->rtk_rate, gps_rtk->nsats, gps_rtk->baseline_coords_type, gps_rtk->baseline_a_mm, gps_rtk->baseline_b_mm, gps_rtk->baseline_c_mm, gps_rtk->accuracy, gps_rtk->iar_num_hypotheses); } +/** + * @brief Encode a gps_rtk struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param gps_rtk C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_rtk_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_gps_rtk_t* gps_rtk) +{ + return mavlink_msg_gps_rtk_pack_status(system_id, component_id, _status, msg, gps_rtk->time_last_baseline_ms, gps_rtk->rtk_receiver_id, gps_rtk->wn, gps_rtk->tow, gps_rtk->rtk_health, gps_rtk->rtk_rate, gps_rtk->nsats, gps_rtk->baseline_coords_type, gps_rtk->baseline_a_mm, gps_rtk->baseline_b_mm, gps_rtk->baseline_c_mm, gps_rtk->accuracy, gps_rtk->iar_num_hypotheses); +} + /** * @brief Send a gps_rtk message * @param chan MAVLink channel to send the message @@ -302,7 +385,7 @@ static inline void mavlink_msg_gps_rtk_send_struct(mavlink_channel_t chan, const #if MAVLINK_MSG_ID_GPS_RTK_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_gps_status.h b/lib/main/MAVLink/common/mavlink_msg_gps_status.h index cedd545845c..8d541980d92 100755 --- a/lib/main/MAVLink/common/mavlink_msg_gps_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_gps_status.h @@ -71,6 +71,48 @@ typedef struct __mavlink_gps_status_t { static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t satellites_visible, const uint8_t *satellite_prn, const uint8_t *satellite_used, const uint8_t *satellite_elevation, const uint8_t *satellite_azimuth, const uint8_t *satellite_snr) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GPS_STATUS_LEN]; + _mav_put_uint8_t(buf, 0, satellites_visible); + _mav_put_uint8_t_array(buf, 1, satellite_prn, 20); + _mav_put_uint8_t_array(buf, 21, satellite_used, 20); + _mav_put_uint8_t_array(buf, 41, satellite_elevation, 20); + _mav_put_uint8_t_array(buf, 61, satellite_azimuth, 20); + _mav_put_uint8_t_array(buf, 81, satellite_snr, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#else + mavlink_gps_status_t packet; + packet.satellites_visible = satellites_visible; + mav_array_assign_uint8_t(packet.satellite_prn, satellite_prn, 20); + mav_array_assign_uint8_t(packet.satellite_used, satellite_used, 20); + mav_array_assign_uint8_t(packet.satellite_elevation, satellite_elevation, 20); + mav_array_assign_uint8_t(packet.satellite_azimuth, satellite_azimuth, 20); + mav_array_assign_uint8_t(packet.satellite_snr, satellite_snr, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_STATUS_MIN_LEN, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); +} + +/** + * @brief Pack a gps_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param satellites_visible Number of satellites visible + * @param satellite_prn Global satellite ID + * @param satellite_used 0: Satellite not used, 1: used for localization + * @param satellite_elevation [deg] Elevation (0: right on top of receiver, 90: on the horizon) of satellite + * @param satellite_azimuth [deg] Direction of satellite, 0: 0 deg, 255: 360 deg. + * @param satellite_snr [dB] Signal to noise ratio of satellite + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t satellites_visible, const uint8_t *satellite_prn, const uint8_t *satellite_used, const uint8_t *satellite_elevation, const uint8_t *satellite_azimuth, const uint8_t *satellite_snr) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_STATUS_LEN]; _mav_put_uint8_t(buf, 0, satellites_visible); @@ -92,7 +134,11 @@ static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t co #endif msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_STATUS_MIN_LEN, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS_STATUS_MIN_LEN, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_GPS_STATUS_MIN_LEN, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#endif } /** @@ -125,11 +171,11 @@ static inline uint16_t mavlink_msg_gps_status_pack_chan(uint8_t system_id, uint8 #else mavlink_gps_status_t packet; packet.satellites_visible = satellites_visible; - mav_array_memcpy(packet.satellite_prn, satellite_prn, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_used, satellite_used, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_elevation, satellite_elevation, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_azimuth, satellite_azimuth, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_snr, satellite_snr, sizeof(uint8_t)*20); + mav_array_assign_uint8_t(packet.satellite_prn, satellite_prn, 20); + mav_array_assign_uint8_t(packet.satellite_used, satellite_used, 20); + mav_array_assign_uint8_t(packet.satellite_elevation, satellite_elevation, 20); + mav_array_assign_uint8_t(packet.satellite_azimuth, satellite_azimuth, 20); + mav_array_assign_uint8_t(packet.satellite_snr, satellite_snr, 20); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_STATUS_LEN); #endif @@ -164,6 +210,20 @@ static inline uint16_t mavlink_msg_gps_status_encode_chan(uint8_t system_id, uin return mavlink_msg_gps_status_pack_chan(system_id, component_id, chan, msg, gps_status->satellites_visible, gps_status->satellite_prn, gps_status->satellite_used, gps_status->satellite_elevation, gps_status->satellite_azimuth, gps_status->satellite_snr); } +/** + * @brief Encode a gps_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param gps_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_gps_status_t* gps_status) +{ + return mavlink_msg_gps_status_pack_status(system_id, component_id, _status, msg, gps_status->satellites_visible, gps_status->satellite_prn, gps_status->satellite_used, gps_status->satellite_elevation, gps_status->satellite_azimuth, gps_status->satellite_snr); +} + /** * @brief Send a gps_status message * @param chan MAVLink channel to send the message @@ -191,11 +251,11 @@ static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t s #else mavlink_gps_status_t packet; packet.satellites_visible = satellites_visible; - mav_array_memcpy(packet.satellite_prn, satellite_prn, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_used, satellite_used, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_elevation, satellite_elevation, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_azimuth, satellite_azimuth, sizeof(uint8_t)*20); - mav_array_memcpy(packet.satellite_snr, satellite_snr, sizeof(uint8_t)*20); + mav_array_assign_uint8_t(packet.satellite_prn, satellite_prn, 20); + mav_array_assign_uint8_t(packet.satellite_used, satellite_used, 20); + mav_array_assign_uint8_t(packet.satellite_elevation, satellite_elevation, 20); + mav_array_assign_uint8_t(packet.satellite_azimuth, satellite_azimuth, 20); + mav_array_assign_uint8_t(packet.satellite_snr, satellite_snr, 20); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_GPS_STATUS_MIN_LEN, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); #endif } @@ -216,7 +276,7 @@ static inline void mavlink_msg_gps_status_send_struct(mavlink_channel_t chan, co #if MAVLINK_MSG_ID_GPS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -236,11 +296,11 @@ static inline void mavlink_msg_gps_status_send_buf(mavlink_message_t *msgbuf, ma #else mavlink_gps_status_t *packet = (mavlink_gps_status_t *)msgbuf; packet->satellites_visible = satellites_visible; - mav_array_memcpy(packet->satellite_prn, satellite_prn, sizeof(uint8_t)*20); - mav_array_memcpy(packet->satellite_used, satellite_used, sizeof(uint8_t)*20); - mav_array_memcpy(packet->satellite_elevation, satellite_elevation, sizeof(uint8_t)*20); - mav_array_memcpy(packet->satellite_azimuth, satellite_azimuth, sizeof(uint8_t)*20); - mav_array_memcpy(packet->satellite_snr, satellite_snr, sizeof(uint8_t)*20); + mav_array_assign_uint8_t(packet->satellite_prn, satellite_prn, 20); + mav_array_assign_uint8_t(packet->satellite_used, satellite_used, 20); + mav_array_assign_uint8_t(packet->satellite_elevation, satellite_elevation, 20); + mav_array_assign_uint8_t(packet->satellite_azimuth, satellite_azimuth, 20); + mav_array_assign_uint8_t(packet->satellite_snr, satellite_snr, 20); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)packet, MAVLINK_MSG_ID_GPS_STATUS_MIN_LEN, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_high_latency.h b/lib/main/MAVLink/common/mavlink_msg_high_latency.h index a0f4e84963d..49d6a520c00 100755 --- a/lib/main/MAVLink/common/mavlink_msg_high_latency.h +++ b/lib/main/MAVLink/common/mavlink_msg_high_latency.h @@ -22,7 +22,7 @@ typedef struct __mavlink_high_latency_t { uint8_t airspeed_sp; /*< [m/s] airspeed setpoint*/ uint8_t groundspeed; /*< [m/s] groundspeed*/ int8_t climb_rate; /*< [m/s] climb rate*/ - uint8_t gps_nsat; /*< Number of satellites visible. If unknown, set to 255*/ + uint8_t gps_nsat; /*< Number of satellites visible. If unknown, set to UINT8_MAX*/ uint8_t gps_fix_type; /*< GPS Fix type.*/ uint8_t battery_remaining; /*< [%] Remaining battery (percentage)*/ int8_t temperature; /*< [degC] Autopilot temperature (degrees C)*/ @@ -126,7 +126,7 @@ typedef struct __mavlink_high_latency_t { * @param airspeed_sp [m/s] airspeed setpoint * @param groundspeed [m/s] groundspeed * @param climb_rate [m/s] climb rate - * @param gps_nsat Number of satellites visible. If unknown, set to 255 + * @param gps_nsat Number of satellites visible. If unknown, set to UINT8_MAX * @param gps_fix_type GPS Fix type. * @param battery_remaining [%] Remaining battery (percentage) * @param temperature [degC] Autopilot temperature (degrees C) @@ -201,6 +201,108 @@ static inline uint16_t mavlink_msg_high_latency_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIGH_LATENCY_MIN_LEN, MAVLINK_MSG_ID_HIGH_LATENCY_LEN, MAVLINK_MSG_ID_HIGH_LATENCY_CRC); } +/** + * @brief Pack a high_latency message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param base_mode Bitmap of enabled system modes. + * @param custom_mode A bitfield for use for autopilot-specific flags. + * @param landed_state The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + * @param roll [cdeg] roll + * @param pitch [cdeg] pitch + * @param heading [cdeg] heading + * @param throttle [%] throttle (percentage) + * @param heading_sp [cdeg] heading setpoint + * @param latitude [degE7] Latitude + * @param longitude [degE7] Longitude + * @param altitude_amsl [m] Altitude above mean sea level + * @param altitude_sp [m] Altitude setpoint relative to the home position + * @param airspeed [m/s] airspeed + * @param airspeed_sp [m/s] airspeed setpoint + * @param groundspeed [m/s] groundspeed + * @param climb_rate [m/s] climb rate + * @param gps_nsat Number of satellites visible. If unknown, set to UINT8_MAX + * @param gps_fix_type GPS Fix type. + * @param battery_remaining [%] Remaining battery (percentage) + * @param temperature [degC] Autopilot temperature (degrees C) + * @param temperature_air [degC] Air temperature (degrees C) from airspeed sensor + * @param failsafe failsafe (each bit represents a failsafe where 0=ok, 1=failsafe active (bit0:RC, bit1:batt, bit2:GPS, bit3:GCS, bit4:fence) + * @param wp_num current waypoint number + * @param wp_distance [m] distance to target + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_high_latency_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t base_mode, uint32_t custom_mode, uint8_t landed_state, int16_t roll, int16_t pitch, uint16_t heading, int8_t throttle, int16_t heading_sp, int32_t latitude, int32_t longitude, int16_t altitude_amsl, int16_t altitude_sp, uint8_t airspeed, uint8_t airspeed_sp, uint8_t groundspeed, int8_t climb_rate, uint8_t gps_nsat, uint8_t gps_fix_type, uint8_t battery_remaining, int8_t temperature, int8_t temperature_air, uint8_t failsafe, uint8_t wp_num, uint16_t wp_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIGH_LATENCY_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_int32_t(buf, 4, latitude); + _mav_put_int32_t(buf, 8, longitude); + _mav_put_int16_t(buf, 12, roll); + _mav_put_int16_t(buf, 14, pitch); + _mav_put_uint16_t(buf, 16, heading); + _mav_put_int16_t(buf, 18, heading_sp); + _mav_put_int16_t(buf, 20, altitude_amsl); + _mav_put_int16_t(buf, 22, altitude_sp); + _mav_put_uint16_t(buf, 24, wp_distance); + _mav_put_uint8_t(buf, 26, base_mode); + _mav_put_uint8_t(buf, 27, landed_state); + _mav_put_int8_t(buf, 28, throttle); + _mav_put_uint8_t(buf, 29, airspeed); + _mav_put_uint8_t(buf, 30, airspeed_sp); + _mav_put_uint8_t(buf, 31, groundspeed); + _mav_put_int8_t(buf, 32, climb_rate); + _mav_put_uint8_t(buf, 33, gps_nsat); + _mav_put_uint8_t(buf, 34, gps_fix_type); + _mav_put_uint8_t(buf, 35, battery_remaining); + _mav_put_int8_t(buf, 36, temperature); + _mav_put_int8_t(buf, 37, temperature_air); + _mav_put_uint8_t(buf, 38, failsafe); + _mav_put_uint8_t(buf, 39, wp_num); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIGH_LATENCY_LEN); +#else + mavlink_high_latency_t packet; + packet.custom_mode = custom_mode; + packet.latitude = latitude; + packet.longitude = longitude; + packet.roll = roll; + packet.pitch = pitch; + packet.heading = heading; + packet.heading_sp = heading_sp; + packet.altitude_amsl = altitude_amsl; + packet.altitude_sp = altitude_sp; + packet.wp_distance = wp_distance; + packet.base_mode = base_mode; + packet.landed_state = landed_state; + packet.throttle = throttle; + packet.airspeed = airspeed; + packet.airspeed_sp = airspeed_sp; + packet.groundspeed = groundspeed; + packet.climb_rate = climb_rate; + packet.gps_nsat = gps_nsat; + packet.gps_fix_type = gps_fix_type; + packet.battery_remaining = battery_remaining; + packet.temperature = temperature; + packet.temperature_air = temperature_air; + packet.failsafe = failsafe; + packet.wp_num = wp_num; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIGH_LATENCY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIGH_LATENCY; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIGH_LATENCY_MIN_LEN, MAVLINK_MSG_ID_HIGH_LATENCY_LEN, MAVLINK_MSG_ID_HIGH_LATENCY_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIGH_LATENCY_MIN_LEN, MAVLINK_MSG_ID_HIGH_LATENCY_LEN); +#endif +} + /** * @brief Pack a high_latency message on a channel * @param system_id ID of this system @@ -223,7 +325,7 @@ static inline uint16_t mavlink_msg_high_latency_pack(uint8_t system_id, uint8_t * @param airspeed_sp [m/s] airspeed setpoint * @param groundspeed [m/s] groundspeed * @param climb_rate [m/s] climb rate - * @param gps_nsat Number of satellites visible. If unknown, set to 255 + * @param gps_nsat Number of satellites visible. If unknown, set to UINT8_MAX * @param gps_fix_type GPS Fix type. * @param battery_remaining [%] Remaining battery (percentage) * @param temperature [degC] Autopilot temperature (degrees C) @@ -326,6 +428,20 @@ static inline uint16_t mavlink_msg_high_latency_encode_chan(uint8_t system_id, u return mavlink_msg_high_latency_pack_chan(system_id, component_id, chan, msg, high_latency->base_mode, high_latency->custom_mode, high_latency->landed_state, high_latency->roll, high_latency->pitch, high_latency->heading, high_latency->throttle, high_latency->heading_sp, high_latency->latitude, high_latency->longitude, high_latency->altitude_amsl, high_latency->altitude_sp, high_latency->airspeed, high_latency->airspeed_sp, high_latency->groundspeed, high_latency->climb_rate, high_latency->gps_nsat, high_latency->gps_fix_type, high_latency->battery_remaining, high_latency->temperature, high_latency->temperature_air, high_latency->failsafe, high_latency->wp_num, high_latency->wp_distance); } +/** + * @brief Encode a high_latency struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param high_latency C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_high_latency_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_high_latency_t* high_latency) +{ + return mavlink_msg_high_latency_pack_status(system_id, component_id, _status, msg, high_latency->base_mode, high_latency->custom_mode, high_latency->landed_state, high_latency->roll, high_latency->pitch, high_latency->heading, high_latency->throttle, high_latency->heading_sp, high_latency->latitude, high_latency->longitude, high_latency->altitude_amsl, high_latency->altitude_sp, high_latency->airspeed, high_latency->airspeed_sp, high_latency->groundspeed, high_latency->climb_rate, high_latency->gps_nsat, high_latency->gps_fix_type, high_latency->battery_remaining, high_latency->temperature, high_latency->temperature_air, high_latency->failsafe, high_latency->wp_num, high_latency->wp_distance); +} + /** * @brief Send a high_latency message * @param chan MAVLink channel to send the message @@ -346,7 +462,7 @@ static inline uint16_t mavlink_msg_high_latency_encode_chan(uint8_t system_id, u * @param airspeed_sp [m/s] airspeed setpoint * @param groundspeed [m/s] groundspeed * @param climb_rate [m/s] climb rate - * @param gps_nsat Number of satellites visible. If unknown, set to 255 + * @param gps_nsat Number of satellites visible. If unknown, set to UINT8_MAX * @param gps_fix_type GPS Fix type. * @param battery_remaining [%] Remaining battery (percentage) * @param temperature [degC] Autopilot temperature (degrees C) @@ -434,7 +550,7 @@ static inline void mavlink_msg_high_latency_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_HIGH_LATENCY_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -670,7 +786,7 @@ static inline int8_t mavlink_msg_high_latency_get_climb_rate(const mavlink_messa /** * @brief Get field gps_nsat from high_latency message * - * @return Number of satellites visible. If unknown, set to 255 + * @return Number of satellites visible. If unknown, set to UINT8_MAX */ static inline uint8_t mavlink_msg_high_latency_get_gps_nsat(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_high_latency2.h b/lib/main/MAVLink/common/mavlink_msg_high_latency2.h index 3de15a71016..1a812b78d2b 100644 --- a/lib/main/MAVLink/common/mavlink_msg_high_latency2.h +++ b/lib/main/MAVLink/common/mavlink_msg_high_latency2.h @@ -26,7 +26,7 @@ typedef struct __mavlink_high_latency2_t { uint8_t wind_heading; /*< [deg/2] Wind heading*/ uint8_t eph; /*< [dm] Maximum error horizontal position since last message*/ uint8_t epv; /*< [dm] Maximum error vertical position since last message*/ - int8_t temperature_air; /*< [degC] Air temperature from airspeed sensor*/ + int8_t temperature_air; /*< [degC] Air temperature*/ int8_t climb_rate; /*< [dm/s] Maximum climb rate magnitude since last message*/ int8_t battery; /*< [%] Battery level (-1 if field not provided).*/ int8_t custom0; /*< Field for custom payload.*/ @@ -138,7 +138,7 @@ typedef struct __mavlink_high_latency2_t { * @param wind_heading [deg/2] Wind heading * @param eph [dm] Maximum error horizontal position since last message * @param epv [dm] Maximum error vertical position since last message - * @param temperature_air [degC] Air temperature from airspeed sensor + * @param temperature_air [degC] Air temperature * @param climb_rate [dm/s] Maximum climb rate magnitude since last message * @param battery [%] Battery level (-1 if field not provided). * @param wp_num Current waypoint number @@ -219,6 +219,117 @@ static inline uint16_t mavlink_msg_high_latency2_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIGH_LATENCY2_MIN_LEN, MAVLINK_MSG_ID_HIGH_LATENCY2_LEN, MAVLINK_MSG_ID_HIGH_LATENCY2_CRC); } +/** + * @brief Pack a high_latency2 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param timestamp [ms] Timestamp (milliseconds since boot or Unix epoch) + * @param type Type of the MAV (quadrotor, helicopter, etc.) + * @param autopilot Autopilot type / class. Use MAV_AUTOPILOT_INVALID for components that are not flight controllers. + * @param custom_mode A bitfield for use for autopilot-specific flags (2 byte version). + * @param latitude [degE7] Latitude + * @param longitude [degE7] Longitude + * @param altitude [m] Altitude above mean sea level + * @param target_altitude [m] Altitude setpoint + * @param heading [deg/2] Heading + * @param target_heading [deg/2] Heading setpoint + * @param target_distance [dam] Distance to target waypoint or position + * @param throttle [%] Throttle + * @param airspeed [m/s*5] Airspeed + * @param airspeed_sp [m/s*5] Airspeed setpoint + * @param groundspeed [m/s*5] Groundspeed + * @param windspeed [m/s*5] Windspeed + * @param wind_heading [deg/2] Wind heading + * @param eph [dm] Maximum error horizontal position since last message + * @param epv [dm] Maximum error vertical position since last message + * @param temperature_air [degC] Air temperature + * @param climb_rate [dm/s] Maximum climb rate magnitude since last message + * @param battery [%] Battery level (-1 if field not provided). + * @param wp_num Current waypoint number + * @param failure_flags Bitmap of failure flags. + * @param custom0 Field for custom payload. + * @param custom1 Field for custom payload. + * @param custom2 Field for custom payload. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_high_latency2_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t timestamp, uint8_t type, uint8_t autopilot, uint16_t custom_mode, int32_t latitude, int32_t longitude, int16_t altitude, int16_t target_altitude, uint8_t heading, uint8_t target_heading, uint16_t target_distance, uint8_t throttle, uint8_t airspeed, uint8_t airspeed_sp, uint8_t groundspeed, uint8_t windspeed, uint8_t wind_heading, uint8_t eph, uint8_t epv, int8_t temperature_air, int8_t climb_rate, int8_t battery, uint16_t wp_num, uint16_t failure_flags, int8_t custom0, int8_t custom1, int8_t custom2) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIGH_LATENCY2_LEN]; + _mav_put_uint32_t(buf, 0, timestamp); + _mav_put_int32_t(buf, 4, latitude); + _mav_put_int32_t(buf, 8, longitude); + _mav_put_uint16_t(buf, 12, custom_mode); + _mav_put_int16_t(buf, 14, altitude); + _mav_put_int16_t(buf, 16, target_altitude); + _mav_put_uint16_t(buf, 18, target_distance); + _mav_put_uint16_t(buf, 20, wp_num); + _mav_put_uint16_t(buf, 22, failure_flags); + _mav_put_uint8_t(buf, 24, type); + _mav_put_uint8_t(buf, 25, autopilot); + _mav_put_uint8_t(buf, 26, heading); + _mav_put_uint8_t(buf, 27, target_heading); + _mav_put_uint8_t(buf, 28, throttle); + _mav_put_uint8_t(buf, 29, airspeed); + _mav_put_uint8_t(buf, 30, airspeed_sp); + _mav_put_uint8_t(buf, 31, groundspeed); + _mav_put_uint8_t(buf, 32, windspeed); + _mav_put_uint8_t(buf, 33, wind_heading); + _mav_put_uint8_t(buf, 34, eph); + _mav_put_uint8_t(buf, 35, epv); + _mav_put_int8_t(buf, 36, temperature_air); + _mav_put_int8_t(buf, 37, climb_rate); + _mav_put_int8_t(buf, 38, battery); + _mav_put_int8_t(buf, 39, custom0); + _mav_put_int8_t(buf, 40, custom1); + _mav_put_int8_t(buf, 41, custom2); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIGH_LATENCY2_LEN); +#else + mavlink_high_latency2_t packet; + packet.timestamp = timestamp; + packet.latitude = latitude; + packet.longitude = longitude; + packet.custom_mode = custom_mode; + packet.altitude = altitude; + packet.target_altitude = target_altitude; + packet.target_distance = target_distance; + packet.wp_num = wp_num; + packet.failure_flags = failure_flags; + packet.type = type; + packet.autopilot = autopilot; + packet.heading = heading; + packet.target_heading = target_heading; + packet.throttle = throttle; + packet.airspeed = airspeed; + packet.airspeed_sp = airspeed_sp; + packet.groundspeed = groundspeed; + packet.windspeed = windspeed; + packet.wind_heading = wind_heading; + packet.eph = eph; + packet.epv = epv; + packet.temperature_air = temperature_air; + packet.climb_rate = climb_rate; + packet.battery = battery; + packet.custom0 = custom0; + packet.custom1 = custom1; + packet.custom2 = custom2; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIGH_LATENCY2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIGH_LATENCY2; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIGH_LATENCY2_MIN_LEN, MAVLINK_MSG_ID_HIGH_LATENCY2_LEN, MAVLINK_MSG_ID_HIGH_LATENCY2_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIGH_LATENCY2_MIN_LEN, MAVLINK_MSG_ID_HIGH_LATENCY2_LEN); +#endif +} + /** * @brief Pack a high_latency2 message on a channel * @param system_id ID of this system @@ -244,7 +355,7 @@ static inline uint16_t mavlink_msg_high_latency2_pack(uint8_t system_id, uint8_t * @param wind_heading [deg/2] Wind heading * @param eph [dm] Maximum error horizontal position since last message * @param epv [dm] Maximum error vertical position since last message - * @param temperature_air [degC] Air temperature from airspeed sensor + * @param temperature_air [degC] Air temperature * @param climb_rate [dm/s] Maximum climb rate magnitude since last message * @param battery [%] Battery level (-1 if field not provided). * @param wp_num Current waypoint number @@ -353,6 +464,20 @@ static inline uint16_t mavlink_msg_high_latency2_encode_chan(uint8_t system_id, return mavlink_msg_high_latency2_pack_chan(system_id, component_id, chan, msg, high_latency2->timestamp, high_latency2->type, high_latency2->autopilot, high_latency2->custom_mode, high_latency2->latitude, high_latency2->longitude, high_latency2->altitude, high_latency2->target_altitude, high_latency2->heading, high_latency2->target_heading, high_latency2->target_distance, high_latency2->throttle, high_latency2->airspeed, high_latency2->airspeed_sp, high_latency2->groundspeed, high_latency2->windspeed, high_latency2->wind_heading, high_latency2->eph, high_latency2->epv, high_latency2->temperature_air, high_latency2->climb_rate, high_latency2->battery, high_latency2->wp_num, high_latency2->failure_flags, high_latency2->custom0, high_latency2->custom1, high_latency2->custom2); } +/** + * @brief Encode a high_latency2 struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param high_latency2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_high_latency2_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_high_latency2_t* high_latency2) +{ + return mavlink_msg_high_latency2_pack_status(system_id, component_id, _status, msg, high_latency2->timestamp, high_latency2->type, high_latency2->autopilot, high_latency2->custom_mode, high_latency2->latitude, high_latency2->longitude, high_latency2->altitude, high_latency2->target_altitude, high_latency2->heading, high_latency2->target_heading, high_latency2->target_distance, high_latency2->throttle, high_latency2->airspeed, high_latency2->airspeed_sp, high_latency2->groundspeed, high_latency2->windspeed, high_latency2->wind_heading, high_latency2->eph, high_latency2->epv, high_latency2->temperature_air, high_latency2->climb_rate, high_latency2->battery, high_latency2->wp_num, high_latency2->failure_flags, high_latency2->custom0, high_latency2->custom1, high_latency2->custom2); +} + /** * @brief Send a high_latency2 message * @param chan MAVLink channel to send the message @@ -376,7 +501,7 @@ static inline uint16_t mavlink_msg_high_latency2_encode_chan(uint8_t system_id, * @param wind_heading [deg/2] Wind heading * @param eph [dm] Maximum error horizontal position since last message * @param epv [dm] Maximum error vertical position since last message - * @param temperature_air [degC] Air temperature from airspeed sensor + * @param temperature_air [degC] Air temperature * @param climb_rate [dm/s] Maximum climb rate magnitude since last message * @param battery [%] Battery level (-1 if field not provided). * @param wp_num Current waypoint number @@ -470,7 +595,7 @@ static inline void mavlink_msg_high_latency2_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_HIGH_LATENCY2_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -742,7 +867,7 @@ static inline uint8_t mavlink_msg_high_latency2_get_epv(const mavlink_message_t* /** * @brief Get field temperature_air from high_latency2 message * - * @return [degC] Air temperature from airspeed sensor + * @return [degC] Air temperature */ static inline int8_t mavlink_msg_high_latency2_get_temperature_air(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_highres_imu.h b/lib/main/MAVLink/common/mavlink_msg_highres_imu.h index a8e0899923b..ea8b9246ad1 100755 --- a/lib/main/MAVLink/common/mavlink_msg_highres_imu.h +++ b/lib/main/MAVLink/common/mavlink_msg_highres_imu.h @@ -19,7 +19,7 @@ typedef struct __mavlink_highres_imu_t { float diff_pressure; /*< [hPa] Differential pressure*/ float pressure_alt; /*< Altitude calculated from pressure*/ float temperature; /*< [degC] Temperature*/ - uint16_t fields_updated; /*< Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature*/ + uint16_t fields_updated; /*< Bitmap for fields that have updated since last message*/ uint8_t id; /*< Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0)*/ } mavlink_highres_imu_t; @@ -100,7 +100,7 @@ typedef struct __mavlink_highres_imu_t { * @param diff_pressure [hPa] Differential pressure * @param pressure_alt Altitude calculated from pressure * @param temperature [degC] Temperature - * @param fields_updated Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + * @param fields_updated Bitmap for fields that have updated since last message * @param id Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) * @return length of the message in bytes (excluding serial stream start sign) */ @@ -153,6 +153,84 @@ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t c return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIGHRES_IMU_MIN_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); } +/** + * @brief Pack a highres_imu message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param xacc [m/s/s] X acceleration + * @param yacc [m/s/s] Y acceleration + * @param zacc [m/s/s] Z acceleration + * @param xgyro [rad/s] Angular speed around X axis + * @param ygyro [rad/s] Angular speed around Y axis + * @param zgyro [rad/s] Angular speed around Z axis + * @param xmag [gauss] X Magnetic field + * @param ymag [gauss] Y Magnetic field + * @param zmag [gauss] Z Magnetic field + * @param abs_pressure [hPa] Absolute pressure + * @param diff_pressure [hPa] Differential pressure + * @param pressure_alt Altitude calculated from pressure + * @param temperature [degC] Temperature + * @param fields_updated Bitmap for fields that have updated since last message + * @param id Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_highres_imu_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated, uint8_t id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIGHRES_IMU_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint16_t(buf, 60, fields_updated); + _mav_put_uint8_t(buf, 62, id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#else + mavlink_highres_imu_t packet; + packet.time_usec = time_usec; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + packet.abs_pressure = abs_pressure; + packet.diff_pressure = diff_pressure; + packet.pressure_alt = pressure_alt; + packet.temperature = temperature; + packet.fields_updated = fields_updated; + packet.id = id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIGHRES_IMU; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIGHRES_IMU_MIN_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIGHRES_IMU_MIN_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#endif +} + /** * @brief Pack a highres_imu message on a channel * @param system_id ID of this system @@ -173,7 +251,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t c * @param diff_pressure [hPa] Differential pressure * @param pressure_alt Altitude calculated from pressure * @param temperature [degC] Temperature - * @param fields_updated Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + * @param fields_updated Bitmap for fields that have updated since last message * @param id Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) * @return length of the message in bytes (excluding serial stream start sign) */ @@ -254,6 +332,20 @@ static inline uint16_t mavlink_msg_highres_imu_encode_chan(uint8_t system_id, ui return mavlink_msg_highres_imu_pack_chan(system_id, component_id, chan, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated, highres_imu->id); } +/** + * @brief Encode a highres_imu struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param highres_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_highres_imu_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_highres_imu_t* highres_imu) +{ + return mavlink_msg_highres_imu_pack_status(system_id, component_id, _status, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated, highres_imu->id); +} + /** * @brief Send a highres_imu message * @param chan MAVLink channel to send the message @@ -272,7 +364,7 @@ static inline uint16_t mavlink_msg_highres_imu_encode_chan(uint8_t system_id, ui * @param diff_pressure [hPa] Differential pressure * @param pressure_alt Altitude calculated from pressure * @param temperature [degC] Temperature - * @param fields_updated Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + * @param fields_updated Bitmap for fields that have updated since last message * @param id Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -338,7 +430,7 @@ static inline void mavlink_msg_highres_imu_send_struct(mavlink_channel_t chan, c #if MAVLINK_MSG_ID_HIGHRES_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -538,7 +630,7 @@ static inline float mavlink_msg_highres_imu_get_temperature(const mavlink_messag /** * @brief Get field fields_updated from highres_imu message * - * @return Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + * @return Bitmap for fields that have updated since last message */ static inline uint16_t mavlink_msg_highres_imu_get_fields_updated(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_hil_actuator_controls.h b/lib/main/MAVLink/common/mavlink_msg_hil_actuator_controls.h index 31c28db427a..5c4c275d4c9 100755 --- a/lib/main/MAVLink/common/mavlink_msg_hil_actuator_controls.h +++ b/lib/main/MAVLink/common/mavlink_msg_hil_actuator_controls.h @@ -6,7 +6,7 @@ typedef struct __mavlink_hil_actuator_controls_t { uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ - uint64_t flags; /*< Flags as bitfield, 1: indicate simulation using lockstep.*/ + uint64_t flags; /*< Flags bitmask.*/ float controls[16]; /*< Control outputs -1 .. 1. Channel assignment depends on the simulated hardware.*/ uint8_t mode; /*< System mode. Includes arming state.*/ } mavlink_hil_actuator_controls_t; @@ -53,7 +53,7 @@ typedef struct __mavlink_hil_actuator_controls_t { * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @param controls Control outputs -1 .. 1. Channel assignment depends on the simulated hardware. * @param mode System mode. Includes arming state. - * @param flags Flags as bitfield, 1: indicate simulation using lockstep. + * @param flags Flags bitmask. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_hil_actuator_controls_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -71,7 +71,7 @@ static inline uint16_t mavlink_msg_hil_actuator_controls_pack(uint8_t system_id, packet.time_usec = time_usec; packet.flags = flags; packet.mode = mode; - mav_array_memcpy(packet.controls, controls, sizeof(float)*16); + mav_array_assign_float(packet.controls, controls, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_LEN); #endif @@ -79,6 +79,46 @@ static inline uint16_t mavlink_msg_hil_actuator_controls_pack(uint8_t system_id, return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_MIN_LEN, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_CRC); } +/** + * @brief Pack a hil_actuator_controls message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param controls Control outputs -1 .. 1. Channel assignment depends on the simulated hardware. + * @param mode System mode. Includes arming state. + * @param flags Flags bitmask. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_actuator_controls_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, const float *controls, uint8_t mode, uint64_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint64_t(buf, 8, flags); + _mav_put_uint8_t(buf, 80, mode); + _mav_put_float_array(buf, 16, controls, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_LEN); +#else + mavlink_hil_actuator_controls_t packet; + packet.time_usec = time_usec; + packet.flags = flags; + packet.mode = mode; + mav_array_memcpy(packet.controls, controls, sizeof(float)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_MIN_LEN, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_MIN_LEN, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_LEN); +#endif +} + /** * @brief Pack a hil_actuator_controls message on a channel * @param system_id ID of this system @@ -88,7 +128,7 @@ static inline uint16_t mavlink_msg_hil_actuator_controls_pack(uint8_t system_id, * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @param controls Control outputs -1 .. 1. Channel assignment depends on the simulated hardware. * @param mode System mode. Includes arming state. - * @param flags Flags as bitfield, 1: indicate simulation using lockstep. + * @param flags Flags bitmask. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_hil_actuator_controls_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -107,7 +147,7 @@ static inline uint16_t mavlink_msg_hil_actuator_controls_pack_chan(uint8_t syste packet.time_usec = time_usec; packet.flags = flags; packet.mode = mode; - mav_array_memcpy(packet.controls, controls, sizeof(float)*16); + mav_array_assign_float(packet.controls, controls, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_LEN); #endif @@ -142,6 +182,20 @@ static inline uint16_t mavlink_msg_hil_actuator_controls_encode_chan(uint8_t sys return mavlink_msg_hil_actuator_controls_pack_chan(system_id, component_id, chan, msg, hil_actuator_controls->time_usec, hil_actuator_controls->controls, hil_actuator_controls->mode, hil_actuator_controls->flags); } +/** + * @brief Encode a hil_actuator_controls struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param hil_actuator_controls C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_actuator_controls_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_hil_actuator_controls_t* hil_actuator_controls) +{ + return mavlink_msg_hil_actuator_controls_pack_status(system_id, component_id, _status, msg, hil_actuator_controls->time_usec, hil_actuator_controls->controls, hil_actuator_controls->mode, hil_actuator_controls->flags); +} + /** * @brief Send a hil_actuator_controls message * @param chan MAVLink channel to send the message @@ -149,7 +203,7 @@ static inline uint16_t mavlink_msg_hil_actuator_controls_encode_chan(uint8_t sys * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @param controls Control outputs -1 .. 1. Channel assignment depends on the simulated hardware. * @param mode System mode. Includes arming state. - * @param flags Flags as bitfield, 1: indicate simulation using lockstep. + * @param flags Flags bitmask. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -167,7 +221,7 @@ static inline void mavlink_msg_hil_actuator_controls_send(mavlink_channel_t chan packet.time_usec = time_usec; packet.flags = flags; packet.mode = mode; - mav_array_memcpy(packet.controls, controls, sizeof(float)*16); + mav_array_assign_float(packet.controls, controls, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS, (const char *)&packet, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_MIN_LEN, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_CRC); #endif } @@ -188,7 +242,7 @@ static inline void mavlink_msg_hil_actuator_controls_send_struct(mavlink_channel #if MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -208,7 +262,7 @@ static inline void mavlink_msg_hil_actuator_controls_send_buf(mavlink_message_t packet->time_usec = time_usec; packet->flags = flags; packet->mode = mode; - mav_array_memcpy(packet->controls, controls, sizeof(float)*16); + mav_array_assign_float(packet->controls, controls, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS, (const char *)packet, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_MIN_LEN, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS_CRC); #endif } @@ -252,7 +306,7 @@ static inline uint8_t mavlink_msg_hil_actuator_controls_get_mode(const mavlink_m /** * @brief Get field flags from hil_actuator_controls message * - * @return Flags as bitfield, 1: indicate simulation using lockstep. + * @return Flags bitmask. */ static inline uint64_t mavlink_msg_hil_actuator_controls_get_flags(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_hil_controls.h b/lib/main/MAVLink/common/mavlink_msg_hil_controls.h index a3bd993bfb2..20482821c7e 100755 --- a/lib/main/MAVLink/common/mavlink_msg_hil_controls.h +++ b/lib/main/MAVLink/common/mavlink_msg_hil_controls.h @@ -123,6 +123,69 @@ static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_CONTROLS_MIN_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); } +/** + * @brief Pack a hil_controls message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param roll_ailerons Control output -1 .. 1 + * @param pitch_elevator Control output -1 .. 1 + * @param yaw_rudder Control output -1 .. 1 + * @param throttle Throttle 0 .. 1 + * @param aux1 Aux 1, -1 .. 1 + * @param aux2 Aux 2, -1 .. 1 + * @param aux3 Aux 3, -1 .. 1 + * @param aux4 Aux 4, -1 .. 1 + * @param mode System mode. + * @param nav_mode Navigation mode (MAV_NAV_MODE) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_controls_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, float aux1, float aux2, float aux3, float aux4, uint8_t mode, uint8_t nav_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_CONTROLS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll_ailerons); + _mav_put_float(buf, 12, pitch_elevator); + _mav_put_float(buf, 16, yaw_rudder); + _mav_put_float(buf, 20, throttle); + _mav_put_float(buf, 24, aux1); + _mav_put_float(buf, 28, aux2); + _mav_put_float(buf, 32, aux3); + _mav_put_float(buf, 36, aux4); + _mav_put_uint8_t(buf, 40, mode); + _mav_put_uint8_t(buf, 41, nav_mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#else + mavlink_hil_controls_t packet; + packet.time_usec = time_usec; + packet.roll_ailerons = roll_ailerons; + packet.pitch_elevator = pitch_elevator; + packet.yaw_rudder = yaw_rudder; + packet.throttle = throttle; + packet.aux1 = aux1; + packet.aux2 = aux2; + packet.aux3 = aux3; + packet.aux4 = aux4; + packet.mode = mode; + packet.nav_mode = nav_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIL_CONTROLS_MIN_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIL_CONTROLS_MIN_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#endif +} + /** * @brief Pack a hil_controls message on a channel * @param system_id ID of this system @@ -209,6 +272,20 @@ static inline uint16_t mavlink_msg_hil_controls_encode_chan(uint8_t system_id, u return mavlink_msg_hil_controls_pack_chan(system_id, component_id, chan, msg, hil_controls->time_usec, hil_controls->roll_ailerons, hil_controls->pitch_elevator, hil_controls->yaw_rudder, hil_controls->throttle, hil_controls->aux1, hil_controls->aux2, hil_controls->aux3, hil_controls->aux4, hil_controls->mode, hil_controls->nav_mode); } +/** + * @brief Encode a hil_controls struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param hil_controls C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_controls_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_hil_controls_t* hil_controls) +{ + return mavlink_msg_hil_controls_pack_status(system_id, component_id, _status, msg, hil_controls->time_usec, hil_controls->roll_ailerons, hil_controls->pitch_elevator, hil_controls->yaw_rudder, hil_controls->throttle, hil_controls->aux1, hil_controls->aux2, hil_controls->aux3, hil_controls->aux4, hil_controls->mode, hil_controls->nav_mode); +} + /** * @brief Send a hil_controls message * @param chan MAVLink channel to send the message @@ -278,7 +355,7 @@ static inline void mavlink_msg_hil_controls_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_HIL_CONTROLS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_hil_gps.h b/lib/main/MAVLink/common/mavlink_msg_hil_gps.h index 4b8ff2b85e3..b8690f4c87f 100755 --- a/lib/main/MAVLink/common/mavlink_msg_hil_gps.h +++ b/lib/main/MAVLink/common/mavlink_msg_hil_gps.h @@ -9,15 +9,15 @@ typedef struct __mavlink_hil_gps_t { int32_t lat; /*< [degE7] Latitude (WGS84)*/ int32_t lon; /*< [degE7] Longitude (WGS84)*/ int32_t alt; /*< [mm] Altitude (MSL). Positive for up.*/ - uint16_t eph; /*< GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX*/ - uint16_t epv; /*< GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX*/ - uint16_t vel; /*< [cm/s] GPS ground speed. If unknown, set to: 65535*/ + uint16_t eph; /*< GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX*/ + uint16_t epv; /*< GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX*/ + uint16_t vel; /*< [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX*/ int16_t vn; /*< [cm/s] GPS velocity in north direction in earth-fixed NED frame*/ int16_t ve; /*< [cm/s] GPS velocity in east direction in earth-fixed NED frame*/ int16_t vd; /*< [cm/s] GPS velocity in down direction in earth-fixed NED frame*/ - uint16_t cog; /*< [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: 65535*/ + uint16_t cog; /*< [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX*/ uint8_t fix_type; /*< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.*/ - uint8_t satellites_visible; /*< Number of satellites visible. If unknown, set to 255*/ + uint8_t satellites_visible; /*< Number of satellites visible. If unknown, set to UINT8_MAX*/ uint8_t id; /*< GPS ID (zero indexed). Used for multiple GPS inputs*/ uint16_t yaw; /*< [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north*/ }) mavlink_hil_gps_t; @@ -88,14 +88,14 @@ typedef struct __mavlink_hil_gps_t { * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [mm] Altitude (MSL). Positive for up. - * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX - * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX - * @param vel [cm/s] GPS ground speed. If unknown, set to: 65535 + * @param eph GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX + * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param vn [cm/s] GPS velocity in north direction in earth-fixed NED frame * @param ve [cm/s] GPS velocity in east direction in earth-fixed NED frame * @param vd [cm/s] GPS velocity in down direction in earth-fixed NED frame - * @param cog [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: 65535 - * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param cog [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @param satellites_visible Number of satellites visible. If unknown, set to UINT8_MAX * @param id GPS ID (zero indexed). Used for multiple GPS inputs * @param yaw [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north * @return length of the message in bytes (excluding serial stream start sign) @@ -147,6 +147,81 @@ static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t compo return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_GPS_MIN_LEN, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); } +/** + * @brief Pack a hil_gps message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat [degE7] Latitude (WGS84) + * @param lon [degE7] Longitude (WGS84) + * @param alt [mm] Altitude (MSL). Positive for up. + * @param eph GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX + * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX + * @param vn [cm/s] GPS velocity in north direction in earth-fixed NED frame + * @param ve [cm/s] GPS velocity in east direction in earth-fixed NED frame + * @param vd [cm/s] GPS velocity in down direction in earth-fixed NED frame + * @param cog [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @param satellites_visible Number of satellites visible. If unknown, set to UINT8_MAX + * @param id GPS ID (zero indexed). Used for multiple GPS inputs + * @param yaw [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_gps_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible, uint8_t id, uint16_t yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_GPS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_int16_t(buf, 26, vn); + _mav_put_int16_t(buf, 28, ve); + _mav_put_int16_t(buf, 30, vd); + _mav_put_uint16_t(buf, 32, cog); + _mav_put_uint8_t(buf, 34, fix_type); + _mav_put_uint8_t(buf, 35, satellites_visible); + _mav_put_uint8_t(buf, 36, id); + _mav_put_uint16_t(buf, 37, yaw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_GPS_LEN); +#else + mavlink_hil_gps_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.vn = vn; + packet.ve = ve; + packet.vd = vd; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + packet.id = id; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_GPS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIL_GPS_MIN_LEN, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIL_GPS_MIN_LEN, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +} + /** * @brief Pack a hil_gps message on a channel * @param system_id ID of this system @@ -158,14 +233,14 @@ static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t compo * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [mm] Altitude (MSL). Positive for up. - * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX - * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX - * @param vel [cm/s] GPS ground speed. If unknown, set to: 65535 + * @param eph GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX + * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param vn [cm/s] GPS velocity in north direction in earth-fixed NED frame * @param ve [cm/s] GPS velocity in east direction in earth-fixed NED frame * @param vd [cm/s] GPS velocity in down direction in earth-fixed NED frame - * @param cog [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: 65535 - * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param cog [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @param satellites_visible Number of satellites visible. If unknown, set to UINT8_MAX * @param id GPS ID (zero indexed). Used for multiple GPS inputs * @param yaw [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north * @return length of the message in bytes (excluding serial stream start sign) @@ -245,6 +320,20 @@ static inline uint16_t mavlink_msg_hil_gps_encode_chan(uint8_t system_id, uint8_ return mavlink_msg_hil_gps_pack_chan(system_id, component_id, chan, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible, hil_gps->id, hil_gps->yaw); } +/** + * @brief Encode a hil_gps struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param hil_gps C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_gps_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_hil_gps_t* hil_gps) +{ + return mavlink_msg_hil_gps_pack_status(system_id, component_id, _status, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible, hil_gps->id, hil_gps->yaw); +} + /** * @brief Send a hil_gps message * @param chan MAVLink channel to send the message @@ -254,14 +343,14 @@ static inline uint16_t mavlink_msg_hil_gps_encode_chan(uint8_t system_id, uint8_ * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [mm] Altitude (MSL). Positive for up. - * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX - * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX - * @param vel [cm/s] GPS ground speed. If unknown, set to: 65535 + * @param eph GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX + * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param vn [cm/s] GPS velocity in north direction in earth-fixed NED frame * @param ve [cm/s] GPS velocity in east direction in earth-fixed NED frame * @param vd [cm/s] GPS velocity in down direction in earth-fixed NED frame - * @param cog [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: 65535 - * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param cog [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX + * @param satellites_visible Number of satellites visible. If unknown, set to UINT8_MAX * @param id GPS ID (zero indexed). Used for multiple GPS inputs * @param yaw [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north */ @@ -326,7 +415,7 @@ static inline void mavlink_msg_hil_gps_send_struct(mavlink_channel_t chan, const #if MAVLINK_MSG_ID_HIL_GPS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -434,7 +523,7 @@ static inline int32_t mavlink_msg_hil_gps_get_alt(const mavlink_message_t* msg) /** * @brief Get field eph from hil_gps message * - * @return GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @return GPS HDOP horizontal dilution of position (unitless * 100). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_hil_gps_get_eph(const mavlink_message_t* msg) { @@ -444,7 +533,7 @@ static inline uint16_t mavlink_msg_hil_gps_get_eph(const mavlink_message_t* msg) /** * @brief Get field epv from hil_gps message * - * @return GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX + * @return GPS VDOP vertical dilution of position (unitless * 100). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_hil_gps_get_epv(const mavlink_message_t* msg) { @@ -454,7 +543,7 @@ static inline uint16_t mavlink_msg_hil_gps_get_epv(const mavlink_message_t* msg) /** * @brief Get field vel from hil_gps message * - * @return [cm/s] GPS ground speed. If unknown, set to: 65535 + * @return [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_hil_gps_get_vel(const mavlink_message_t* msg) { @@ -494,7 +583,7 @@ static inline int16_t mavlink_msg_hil_gps_get_vd(const mavlink_message_t* msg) /** * @brief Get field cog from hil_gps message * - * @return [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: 65535 + * @return [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_hil_gps_get_cog(const mavlink_message_t* msg) { @@ -504,7 +593,7 @@ static inline uint16_t mavlink_msg_hil_gps_get_cog(const mavlink_message_t* msg) /** * @brief Get field satellites_visible from hil_gps message * - * @return Number of satellites visible. If unknown, set to 255 + * @return Number of satellites visible. If unknown, set to UINT8_MAX */ static inline uint8_t mavlink_msg_hil_gps_get_satellites_visible(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_hil_optical_flow.h b/lib/main/MAVLink/common/mavlink_msg_hil_optical_flow.h index 98c61fd5e59..f592b1b793d 100755 --- a/lib/main/MAVLink/common/mavlink_msg_hil_optical_flow.h +++ b/lib/main/MAVLink/common/mavlink_msg_hil_optical_flow.h @@ -129,6 +129,72 @@ static inline uint16_t mavlink_msg_hil_optical_flow_pack(uint8_t system_id, uint return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_MIN_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); } +/** + * @brief Pack a hil_optical_flow message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param sensor_id Sensor ID + * @param integration_time_us [us] Integration time. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + * @param integrated_x [rad] Flow in radians around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + * @param integrated_y [rad] Flow in radians around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + * @param integrated_xgyro [rad] RH rotation around X axis + * @param integrated_ygyro [rad] RH rotation around Y axis + * @param integrated_zgyro [rad] RH rotation around Z axis + * @param temperature [cdegC] Temperature + * @param quality Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + * @param time_delta_distance_us [us] Time since the distance was sampled. + * @param distance [m] Distance to the center of the flow field. Positive value (including zero): distance known. Negative value: Unknown distance. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_optical_flow_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, float integrated_xgyro, float integrated_ygyro, float integrated_zgyro, int16_t temperature, uint8_t quality, uint32_t time_delta_distance_us, float distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, integration_time_us); + _mav_put_float(buf, 12, integrated_x); + _mav_put_float(buf, 16, integrated_y); + _mav_put_float(buf, 20, integrated_xgyro); + _mav_put_float(buf, 24, integrated_ygyro); + _mav_put_float(buf, 28, integrated_zgyro); + _mav_put_uint32_t(buf, 32, time_delta_distance_us); + _mav_put_float(buf, 36, distance); + _mav_put_int16_t(buf, 40, temperature); + _mav_put_uint8_t(buf, 42, sensor_id); + _mav_put_uint8_t(buf, 43, quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#else + mavlink_hil_optical_flow_t packet; + packet.time_usec = time_usec; + packet.integration_time_us = integration_time_us; + packet.integrated_x = integrated_x; + packet.integrated_y = integrated_y; + packet.integrated_xgyro = integrated_xgyro; + packet.integrated_ygyro = integrated_ygyro; + packet.integrated_zgyro = integrated_zgyro; + packet.time_delta_distance_us = time_delta_distance_us; + packet.distance = distance; + packet.temperature = temperature; + packet.sensor_id = sensor_id; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_OPTICAL_FLOW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_MIN_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_MIN_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +} + /** * @brief Pack a hil_optical_flow message on a channel * @param system_id ID of this system @@ -218,6 +284,20 @@ static inline uint16_t mavlink_msg_hil_optical_flow_encode_chan(uint8_t system_i return mavlink_msg_hil_optical_flow_pack_chan(system_id, component_id, chan, msg, hil_optical_flow->time_usec, hil_optical_flow->sensor_id, hil_optical_flow->integration_time_us, hil_optical_flow->integrated_x, hil_optical_flow->integrated_y, hil_optical_flow->integrated_xgyro, hil_optical_flow->integrated_ygyro, hil_optical_flow->integrated_zgyro, hil_optical_flow->temperature, hil_optical_flow->quality, hil_optical_flow->time_delta_distance_us, hil_optical_flow->distance); } +/** + * @brief Encode a hil_optical_flow struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param hil_optical_flow C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_optical_flow_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_hil_optical_flow_t* hil_optical_flow) +{ + return mavlink_msg_hil_optical_flow_pack_status(system_id, component_id, _status, msg, hil_optical_flow->time_usec, hil_optical_flow->sensor_id, hil_optical_flow->integration_time_us, hil_optical_flow->integrated_x, hil_optical_flow->integrated_y, hil_optical_flow->integrated_xgyro, hil_optical_flow->integrated_ygyro, hil_optical_flow->integrated_zgyro, hil_optical_flow->temperature, hil_optical_flow->quality, hil_optical_flow->time_delta_distance_us, hil_optical_flow->distance); +} + /** * @brief Send a hil_optical_flow message * @param chan MAVLink channel to send the message @@ -290,7 +370,7 @@ static inline void mavlink_msg_hil_optical_flow_send_struct(mavlink_channel_t ch #if MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_hil_rc_inputs_raw.h b/lib/main/MAVLink/common/mavlink_msg_hil_rc_inputs_raw.h index 28ce091ab83..da581132d85 100755 --- a/lib/main/MAVLink/common/mavlink_msg_hil_rc_inputs_raw.h +++ b/lib/main/MAVLink/common/mavlink_msg_hil_rc_inputs_raw.h @@ -18,7 +18,7 @@ typedef struct __mavlink_hil_rc_inputs_raw_t { uint16_t chan10_raw; /*< [us] RC channel 10 value*/ uint16_t chan11_raw; /*< [us] RC channel 11 value*/ uint16_t chan12_raw; /*< [us] RC channel 12 value*/ - uint8_t rssi; /*< Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown.*/ + uint8_t rssi; /*< Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown.*/ } mavlink_hil_rc_inputs_raw_t; #define MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN 33 @@ -93,7 +93,7 @@ typedef struct __mavlink_hil_rc_inputs_raw_t { * @param chan10_raw [us] RC channel 10 value * @param chan11_raw [us] RC channel 11 value * @param chan12_raw [us] RC channel 12 value - * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -141,6 +141,78 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack(uint8_t system_id, uin return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_MIN_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); } +/** + * @brief Pack a hil_rc_inputs_raw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param chan1_raw [us] RC channel 1 value + * @param chan2_raw [us] RC channel 2 value + * @param chan3_raw [us] RC channel 3 value + * @param chan4_raw [us] RC channel 4 value + * @param chan5_raw [us] RC channel 5 value + * @param chan6_raw [us] RC channel 6 value + * @param chan7_raw [us] RC channel 7 value + * @param chan8_raw [us] RC channel 8 value + * @param chan9_raw [us] RC channel 9 value + * @param chan10_raw [us] RC channel 10 value + * @param chan11_raw [us] RC channel 11 value + * @param chan12_raw [us] RC channel 12 value + * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, chan1_raw); + _mav_put_uint16_t(buf, 10, chan2_raw); + _mav_put_uint16_t(buf, 12, chan3_raw); + _mav_put_uint16_t(buf, 14, chan4_raw); + _mav_put_uint16_t(buf, 16, chan5_raw); + _mav_put_uint16_t(buf, 18, chan6_raw); + _mav_put_uint16_t(buf, 20, chan7_raw); + _mav_put_uint16_t(buf, 22, chan8_raw); + _mav_put_uint16_t(buf, 24, chan9_raw); + _mav_put_uint16_t(buf, 26, chan10_raw); + _mav_put_uint16_t(buf, 28, chan11_raw); + _mav_put_uint16_t(buf, 30, chan12_raw); + _mav_put_uint8_t(buf, 32, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#else + mavlink_hil_rc_inputs_raw_t packet; + packet.time_usec = time_usec; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_MIN_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_MIN_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#endif +} + /** * @brief Pack a hil_rc_inputs_raw message on a channel * @param system_id ID of this system @@ -160,7 +232,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack(uint8_t system_id, uin * @param chan10_raw [us] RC channel 10 value * @param chan11_raw [us] RC channel 11 value * @param chan12_raw [us] RC channel 12 value - * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -236,6 +308,20 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode_chan(uint8_t system_ return mavlink_msg_hil_rc_inputs_raw_pack_chan(system_id, component_id, chan, msg, hil_rc_inputs_raw->time_usec, hil_rc_inputs_raw->chan1_raw, hil_rc_inputs_raw->chan2_raw, hil_rc_inputs_raw->chan3_raw, hil_rc_inputs_raw->chan4_raw, hil_rc_inputs_raw->chan5_raw, hil_rc_inputs_raw->chan6_raw, hil_rc_inputs_raw->chan7_raw, hil_rc_inputs_raw->chan8_raw, hil_rc_inputs_raw->chan9_raw, hil_rc_inputs_raw->chan10_raw, hil_rc_inputs_raw->chan11_raw, hil_rc_inputs_raw->chan12_raw, hil_rc_inputs_raw->rssi); } +/** + * @brief Encode a hil_rc_inputs_raw struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param hil_rc_inputs_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_hil_rc_inputs_raw_t* hil_rc_inputs_raw) +{ + return mavlink_msg_hil_rc_inputs_raw_pack_status(system_id, component_id, _status, msg, hil_rc_inputs_raw->time_usec, hil_rc_inputs_raw->chan1_raw, hil_rc_inputs_raw->chan2_raw, hil_rc_inputs_raw->chan3_raw, hil_rc_inputs_raw->chan4_raw, hil_rc_inputs_raw->chan5_raw, hil_rc_inputs_raw->chan6_raw, hil_rc_inputs_raw->chan7_raw, hil_rc_inputs_raw->chan8_raw, hil_rc_inputs_raw->chan9_raw, hil_rc_inputs_raw->chan10_raw, hil_rc_inputs_raw->chan11_raw, hil_rc_inputs_raw->chan12_raw, hil_rc_inputs_raw->rssi); +} + /** * @brief Send a hil_rc_inputs_raw message * @param chan MAVLink channel to send the message @@ -253,7 +339,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode_chan(uint8_t system_ * @param chan10_raw [us] RC channel 10 value * @param chan11_raw [us] RC channel 11 value * @param chan12_raw [us] RC channel 12 value - * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -314,7 +400,7 @@ static inline void mavlink_msg_hil_rc_inputs_raw_send_struct(mavlink_channel_t c #if MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -500,7 +586,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan12_raw(const mavlin /** * @brief Get field rssi from hil_rc_inputs_raw message * - * @return Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @return Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. */ static inline uint8_t mavlink_msg_hil_rc_inputs_raw_get_rssi(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_hil_sensor.h b/lib/main/MAVLink/common/mavlink_msg_hil_sensor.h index 007c5a6989a..290465d83e3 100755 --- a/lib/main/MAVLink/common/mavlink_msg_hil_sensor.h +++ b/lib/main/MAVLink/common/mavlink_msg_hil_sensor.h @@ -19,7 +19,7 @@ typedef struct __mavlink_hil_sensor_t { float diff_pressure; /*< [hPa] Differential pressure (airspeed)*/ float pressure_alt; /*< Altitude calculated from pressure*/ float temperature; /*< [degC] Temperature*/ - uint32_t fields_updated; /*< Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature, bit 31: full reset of attitude/position/velocities/etc was performed in sim.*/ + uint32_t fields_updated; /*< Bitmap for fields that have updated since last message*/ uint8_t id; /*< Sensor ID (zero indexed). Used for multiple sensor inputs*/ } mavlink_hil_sensor_t; @@ -100,7 +100,7 @@ typedef struct __mavlink_hil_sensor_t { * @param diff_pressure [hPa] Differential pressure (airspeed) * @param pressure_alt Altitude calculated from pressure * @param temperature [degC] Temperature - * @param fields_updated Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature, bit 31: full reset of attitude/position/velocities/etc was performed in sim. + * @param fields_updated Bitmap for fields that have updated since last message * @param id Sensor ID (zero indexed). Used for multiple sensor inputs * @return length of the message in bytes (excluding serial stream start sign) */ @@ -153,6 +153,84 @@ static inline uint16_t mavlink_msg_hil_sensor_pack(uint8_t system_id, uint8_t co return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); } +/** + * @brief Pack a hil_sensor message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param xacc [m/s/s] X acceleration + * @param yacc [m/s/s] Y acceleration + * @param zacc [m/s/s] Z acceleration + * @param xgyro [rad/s] Angular speed around X axis in body frame + * @param ygyro [rad/s] Angular speed around Y axis in body frame + * @param zgyro [rad/s] Angular speed around Z axis in body frame + * @param xmag [gauss] X Magnetic field + * @param ymag [gauss] Y Magnetic field + * @param zmag [gauss] Z Magnetic field + * @param abs_pressure [hPa] Absolute pressure + * @param diff_pressure [hPa] Differential pressure (airspeed) + * @param pressure_alt Altitude calculated from pressure + * @param temperature [degC] Temperature + * @param fields_updated Bitmap for fields that have updated since last message + * @param id Sensor ID (zero indexed). Used for multiple sensor inputs + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_sensor_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated, uint8_t id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_SENSOR_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint32_t(buf, 60, fields_updated); + _mav_put_uint8_t(buf, 64, id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#else + mavlink_hil_sensor_t packet; + packet.time_usec = time_usec; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + packet.abs_pressure = abs_pressure; + packet.diff_pressure = diff_pressure; + packet.pressure_alt = pressure_alt; + packet.temperature = temperature; + packet.fields_updated = fields_updated; + packet.id = id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_SENSOR; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIL_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIL_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +} + /** * @brief Pack a hil_sensor message on a channel * @param system_id ID of this system @@ -173,7 +251,7 @@ static inline uint16_t mavlink_msg_hil_sensor_pack(uint8_t system_id, uint8_t co * @param diff_pressure [hPa] Differential pressure (airspeed) * @param pressure_alt Altitude calculated from pressure * @param temperature [degC] Temperature - * @param fields_updated Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature, bit 31: full reset of attitude/position/velocities/etc was performed in sim. + * @param fields_updated Bitmap for fields that have updated since last message * @param id Sensor ID (zero indexed). Used for multiple sensor inputs * @return length of the message in bytes (excluding serial stream start sign) */ @@ -254,6 +332,20 @@ static inline uint16_t mavlink_msg_hil_sensor_encode_chan(uint8_t system_id, uin return mavlink_msg_hil_sensor_pack_chan(system_id, component_id, chan, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated, hil_sensor->id); } +/** + * @brief Encode a hil_sensor struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param hil_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_sensor_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_hil_sensor_t* hil_sensor) +{ + return mavlink_msg_hil_sensor_pack_status(system_id, component_id, _status, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated, hil_sensor->id); +} + /** * @brief Send a hil_sensor message * @param chan MAVLink channel to send the message @@ -272,7 +364,7 @@ static inline uint16_t mavlink_msg_hil_sensor_encode_chan(uint8_t system_id, uin * @param diff_pressure [hPa] Differential pressure (airspeed) * @param pressure_alt Altitude calculated from pressure * @param temperature [degC] Temperature - * @param fields_updated Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature, bit 31: full reset of attitude/position/velocities/etc was performed in sim. + * @param fields_updated Bitmap for fields that have updated since last message * @param id Sensor ID (zero indexed). Used for multiple sensor inputs */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -338,7 +430,7 @@ static inline void mavlink_msg_hil_sensor_send_struct(mavlink_channel_t chan, co #if MAVLINK_MSG_ID_HIL_SENSOR_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -538,7 +630,7 @@ static inline float mavlink_msg_hil_sensor_get_temperature(const mavlink_message /** * @brief Get field fields_updated from hil_sensor message * - * @return Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature, bit 31: full reset of attitude/position/velocities/etc was performed in sim. + * @return Bitmap for fields that have updated since last message */ static inline uint32_t mavlink_msg_hil_sensor_get_fields_updated(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_hil_state.h b/lib/main/MAVLink/common/mavlink_msg_hil_state.h index e5c4ed80973..3bbeb309dbf 100755 --- a/lib/main/MAVLink/common/mavlink_msg_hil_state.h +++ b/lib/main/MAVLink/common/mavlink_msg_hil_state.h @@ -153,6 +153,84 @@ static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t com return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_STATE_MIN_LEN, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); } +/** + * @brief Pack a hil_state message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param roll [rad] Roll angle + * @param pitch [rad] Pitch angle + * @param yaw [rad] Yaw angle + * @param rollspeed [rad/s] Body frame roll / phi angular speed + * @param pitchspeed [rad/s] Body frame pitch / theta angular speed + * @param yawspeed [rad/s] Body frame yaw / psi angular speed + * @param lat [degE7] Latitude + * @param lon [degE7] Longitude + * @param alt [mm] Altitude + * @param vx [cm/s] Ground X Speed (Latitude) + * @param vy [cm/s] Ground Y Speed (Longitude) + * @param vz [cm/s] Ground Z Speed (Altitude) + * @param xacc [mG] X acceleration + * @param yacc [mG] Y acceleration + * @param zacc [mG] Z acceleration + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_state_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_STATE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_int32_t(buf, 32, lat); + _mav_put_int32_t(buf, 36, lon); + _mav_put_int32_t(buf, 40, alt); + _mav_put_int16_t(buf, 44, vx); + _mav_put_int16_t(buf, 46, vy); + _mav_put_int16_t(buf, 48, vz); + _mav_put_int16_t(buf, 50, xacc); + _mav_put_int16_t(buf, 52, yacc); + _mav_put_int16_t(buf, 54, zacc); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_LEN); +#else + mavlink_hil_state_t packet; + packet.time_usec = time_usec; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_STATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIL_STATE_MIN_LEN, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIL_STATE_MIN_LEN, MAVLINK_MSG_ID_HIL_STATE_LEN); +#endif +} + /** * @brief Pack a hil_state message on a channel * @param system_id ID of this system @@ -254,6 +332,20 @@ static inline uint16_t mavlink_msg_hil_state_encode_chan(uint8_t system_id, uint return mavlink_msg_hil_state_pack_chan(system_id, component_id, chan, msg, hil_state->time_usec, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc); } +/** + * @brief Encode a hil_state struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param hil_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_state_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_hil_state_t* hil_state) +{ + return mavlink_msg_hil_state_pack_status(system_id, component_id, _status, msg, hil_state->time_usec, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc); +} + /** * @brief Send a hil_state message * @param chan MAVLink channel to send the message @@ -338,7 +430,7 @@ static inline void mavlink_msg_hil_state_send_struct(mavlink_channel_t chan, con #if MAVLINK_MSG_ID_HIL_STATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_hil_state_quaternion.h b/lib/main/MAVLink/common/mavlink_msg_hil_state_quaternion.h index a605f39febb..747a73e3096 100755 --- a/lib/main/MAVLink/common/mavlink_msg_hil_state_quaternion.h +++ b/lib/main/MAVLink/common/mavlink_msg_hil_state_quaternion.h @@ -143,7 +143,7 @@ static inline uint16_t mavlink_msg_hil_state_quaternion_pack(uint8_t system_id, packet.xacc = xacc; packet.yacc = yacc; packet.zacc = zacc; - mav_array_memcpy(packet.attitude_quaternion, attitude_quaternion, sizeof(float)*4); + mav_array_assign_float(packet.attitude_quaternion, attitude_quaternion, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); #endif @@ -151,6 +151,82 @@ static inline uint16_t mavlink_msg_hil_state_quaternion_pack(uint8_t system_id, return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); } +/** + * @brief Pack a hil_state_quaternion message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion in w, x, y, z order (with 1 0 0 0 being the null-rotation) + * @param rollspeed [rad/s] Body frame roll / phi angular speed + * @param pitchspeed [rad/s] Body frame pitch / theta angular speed + * @param yawspeed [rad/s] Body frame yaw / psi angular speed + * @param lat [degE7] Latitude + * @param lon [degE7] Longitude + * @param alt [mm] Altitude + * @param vx [cm/s] Ground X Speed (Latitude) + * @param vy [cm/s] Ground Y Speed (Longitude) + * @param vz [cm/s] Ground Z Speed (Altitude) + * @param ind_airspeed [cm/s] Indicated airspeed + * @param true_airspeed [cm/s] True airspeed + * @param xacc [mG] X acceleration + * @param yacc [mG] Y acceleration + * @param zacc [mG] Z acceleration + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_state_quaternion_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, const float *attitude_quaternion, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t ind_airspeed, uint16_t true_airspeed, int16_t xacc, int16_t yacc, int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 24, rollspeed); + _mav_put_float(buf, 28, pitchspeed); + _mav_put_float(buf, 32, yawspeed); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lon); + _mav_put_int32_t(buf, 44, alt); + _mav_put_int16_t(buf, 48, vx); + _mav_put_int16_t(buf, 50, vy); + _mav_put_int16_t(buf, 52, vz); + _mav_put_uint16_t(buf, 54, ind_airspeed); + _mav_put_uint16_t(buf, 56, true_airspeed); + _mav_put_int16_t(buf, 58, xacc); + _mav_put_int16_t(buf, 60, yacc); + _mav_put_int16_t(buf, 62, zacc); + _mav_put_float_array(buf, 8, attitude_quaternion, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#else + mavlink_hil_state_quaternion_t packet; + packet.time_usec = time_usec; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.ind_airspeed = ind_airspeed; + packet.true_airspeed = true_airspeed; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + mav_array_memcpy(packet.attitude_quaternion, attitude_quaternion, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_STATE_QUATERNION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +} + /** * @brief Pack a hil_state_quaternion message on a channel * @param system_id ID of this system @@ -215,7 +291,7 @@ static inline uint16_t mavlink_msg_hil_state_quaternion_pack_chan(uint8_t system packet.xacc = xacc; packet.yacc = yacc; packet.zacc = zacc; - mav_array_memcpy(packet.attitude_quaternion, attitude_quaternion, sizeof(float)*4); + mav_array_assign_float(packet.attitude_quaternion, attitude_quaternion, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); #endif @@ -250,6 +326,20 @@ static inline uint16_t mavlink_msg_hil_state_quaternion_encode_chan(uint8_t syst return mavlink_msg_hil_state_quaternion_pack_chan(system_id, component_id, chan, msg, hil_state_quaternion->time_usec, hil_state_quaternion->attitude_quaternion, hil_state_quaternion->rollspeed, hil_state_quaternion->pitchspeed, hil_state_quaternion->yawspeed, hil_state_quaternion->lat, hil_state_quaternion->lon, hil_state_quaternion->alt, hil_state_quaternion->vx, hil_state_quaternion->vy, hil_state_quaternion->vz, hil_state_quaternion->ind_airspeed, hil_state_quaternion->true_airspeed, hil_state_quaternion->xacc, hil_state_quaternion->yacc, hil_state_quaternion->zacc); } +/** + * @brief Encode a hil_state_quaternion struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param hil_state_quaternion C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_state_quaternion_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_hil_state_quaternion_t* hil_state_quaternion) +{ + return mavlink_msg_hil_state_quaternion_pack_status(system_id, component_id, _status, msg, hil_state_quaternion->time_usec, hil_state_quaternion->attitude_quaternion, hil_state_quaternion->rollspeed, hil_state_quaternion->pitchspeed, hil_state_quaternion->yawspeed, hil_state_quaternion->lat, hil_state_quaternion->lon, hil_state_quaternion->alt, hil_state_quaternion->vx, hil_state_quaternion->vy, hil_state_quaternion->vz, hil_state_quaternion->ind_airspeed, hil_state_quaternion->true_airspeed, hil_state_quaternion->xacc, hil_state_quaternion->yacc, hil_state_quaternion->zacc); +} + /** * @brief Send a hil_state_quaternion message * @param chan MAVLink channel to send the message @@ -311,7 +401,7 @@ static inline void mavlink_msg_hil_state_quaternion_send(mavlink_channel_t chan, packet.xacc = xacc; packet.yacc = yacc; packet.zacc = zacc; - mav_array_memcpy(packet.attitude_quaternion, attitude_quaternion, sizeof(float)*4); + mav_array_assign_float(packet.attitude_quaternion, attitude_quaternion, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)&packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); #endif } @@ -332,7 +422,7 @@ static inline void mavlink_msg_hil_state_quaternion_send_struct(mavlink_channel_ #if MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -376,7 +466,7 @@ static inline void mavlink_msg_hil_state_quaternion_send_buf(mavlink_message_t * packet->xacc = xacc; packet->yacc = yacc; packet->zacc = zacc; - mav_array_memcpy(packet->attitude_quaternion, attitude_quaternion, sizeof(float)*4); + mav_array_assign_float(packet->attitude_quaternion, attitude_quaternion, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_home_position.h b/lib/main/MAVLink/common/mavlink_msg_home_position.h index ab5130b8101..61dbebdfc03 100755 --- a/lib/main/MAVLink/common/mavlink_msg_home_position.h +++ b/lib/main/MAVLink/common/mavlink_msg_home_position.h @@ -8,10 +8,14 @@ typedef struct __mavlink_home_position_t { int32_t latitude; /*< [degE7] Latitude (WGS84)*/ int32_t longitude; /*< [degE7] Longitude (WGS84)*/ int32_t altitude; /*< [mm] Altitude (MSL). Positive for up.*/ - float x; /*< [m] Local X position of this position in the local coordinate frame*/ - float y; /*< [m] Local Y position of this position in the local coordinate frame*/ - float z; /*< [m] Local Z position of this position in the local coordinate frame*/ - float q[4]; /*< World to surface normal and heading transformation of the takeoff position. Used to indicate the heading and slope of the ground*/ + float x; /*< [m] Local X position of this position in the local coordinate frame (NED)*/ + float y; /*< [m] Local Y position of this position in the local coordinate frame (NED)*/ + float z; /*< [m] Local Z position of this position in the local coordinate frame (NED: positive "down")*/ + float q[4]; /*< + Quaternion indicating world-to-surface-normal and heading transformation of the takeoff position. + Used to indicate the heading and slope of the ground. + All fields should be set to NaN if an accurate quaternion for both heading and surface slope cannot be supplied. + */ float approach_x; /*< [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone.*/ float approach_y; /*< [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone.*/ float approach_z; /*< [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone.*/ @@ -74,10 +78,14 @@ typedef struct __mavlink_home_position_t { * @param latitude [degE7] Latitude (WGS84) * @param longitude [degE7] Longitude (WGS84) * @param altitude [mm] Altitude (MSL). Positive for up. - * @param x [m] Local X position of this position in the local coordinate frame - * @param y [m] Local Y position of this position in the local coordinate frame - * @param z [m] Local Z position of this position in the local coordinate frame - * @param q World to surface normal and heading transformation of the takeoff position. Used to indicate the heading and slope of the ground + * @param x [m] Local X position of this position in the local coordinate frame (NED) + * @param y [m] Local Y position of this position in the local coordinate frame (NED) + * @param z [m] Local Z position of this position in the local coordinate frame (NED: positive "down") + * @param q + Quaternion indicating world-to-surface-normal and heading transformation of the takeoff position. + Used to indicate the heading and slope of the ground. + All fields should be set to NaN if an accurate quaternion for both heading and surface slope cannot be supplied. + * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_z [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. @@ -113,7 +121,7 @@ static inline uint16_t mavlink_msg_home_position_pack(uint8_t system_id, uint8_t packet.approach_y = approach_y; packet.approach_z = approach_z; packet.time_usec = time_usec; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HOME_POSITION_LEN); #endif @@ -121,6 +129,71 @@ static inline uint16_t mavlink_msg_home_position_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_HOME_POSITION_LEN, MAVLINK_MSG_ID_HOME_POSITION_CRC); } +/** + * @brief Pack a home_position message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param latitude [degE7] Latitude (WGS84) + * @param longitude [degE7] Longitude (WGS84) + * @param altitude [mm] Altitude (MSL). Positive for up. + * @param x [m] Local X position of this position in the local coordinate frame (NED) + * @param y [m] Local Y position of this position in the local coordinate frame (NED) + * @param z [m] Local Z position of this position in the local coordinate frame (NED: positive "down") + * @param q + Quaternion indicating world-to-surface-normal and heading transformation of the takeoff position. + Used to indicate the heading and slope of the ground. + All fields should be set to NaN if an accurate quaternion for both heading and surface slope cannot be supplied. + + * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + * @param approach_z [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_home_position_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z, uint64_t time_usec) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HOME_POSITION_LEN]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_float(buf, 12, x); + _mav_put_float(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_float(buf, 40, approach_x); + _mav_put_float(buf, 44, approach_y); + _mav_put_float(buf, 48, approach_z); + _mav_put_uint64_t(buf, 52, time_usec); + _mav_put_float_array(buf, 24, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HOME_POSITION_LEN); +#else + mavlink_home_position_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.x = x; + packet.y = y; + packet.z = z; + packet.approach_x = approach_x; + packet.approach_y = approach_y; + packet.approach_z = approach_z; + packet.time_usec = time_usec; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HOME_POSITION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HOME_POSITION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_HOME_POSITION_LEN, MAVLINK_MSG_ID_HOME_POSITION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_HOME_POSITION_LEN); +#endif +} + /** * @brief Pack a home_position message on a channel * @param system_id ID of this system @@ -130,10 +203,14 @@ static inline uint16_t mavlink_msg_home_position_pack(uint8_t system_id, uint8_t * @param latitude [degE7] Latitude (WGS84) * @param longitude [degE7] Longitude (WGS84) * @param altitude [mm] Altitude (MSL). Positive for up. - * @param x [m] Local X position of this position in the local coordinate frame - * @param y [m] Local Y position of this position in the local coordinate frame - * @param z [m] Local Z position of this position in the local coordinate frame - * @param q World to surface normal and heading transformation of the takeoff position. Used to indicate the heading and slope of the ground + * @param x [m] Local X position of this position in the local coordinate frame (NED) + * @param y [m] Local Y position of this position in the local coordinate frame (NED) + * @param z [m] Local Z position of this position in the local coordinate frame (NED: positive "down") + * @param q + Quaternion indicating world-to-surface-normal and heading transformation of the takeoff position. + Used to indicate the heading and slope of the ground. + All fields should be set to NaN if an accurate quaternion for both heading and surface slope cannot be supplied. + * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_z [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. @@ -170,7 +247,7 @@ static inline uint16_t mavlink_msg_home_position_pack_chan(uint8_t system_id, ui packet.approach_y = approach_y; packet.approach_z = approach_z; packet.time_usec = time_usec; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HOME_POSITION_LEN); #endif @@ -205,6 +282,20 @@ static inline uint16_t mavlink_msg_home_position_encode_chan(uint8_t system_id, return mavlink_msg_home_position_pack_chan(system_id, component_id, chan, msg, home_position->latitude, home_position->longitude, home_position->altitude, home_position->x, home_position->y, home_position->z, home_position->q, home_position->approach_x, home_position->approach_y, home_position->approach_z, home_position->time_usec); } +/** + * @brief Encode a home_position struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param home_position C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_home_position_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_home_position_t* home_position) +{ + return mavlink_msg_home_position_pack_status(system_id, component_id, _status, msg, home_position->latitude, home_position->longitude, home_position->altitude, home_position->x, home_position->y, home_position->z, home_position->q, home_position->approach_x, home_position->approach_y, home_position->approach_z, home_position->time_usec); +} + /** * @brief Send a home_position message * @param chan MAVLink channel to send the message @@ -212,10 +303,14 @@ static inline uint16_t mavlink_msg_home_position_encode_chan(uint8_t system_id, * @param latitude [degE7] Latitude (WGS84) * @param longitude [degE7] Longitude (WGS84) * @param altitude [mm] Altitude (MSL). Positive for up. - * @param x [m] Local X position of this position in the local coordinate frame - * @param y [m] Local Y position of this position in the local coordinate frame - * @param z [m] Local Z position of this position in the local coordinate frame - * @param q World to surface normal and heading transformation of the takeoff position. Used to indicate the heading and slope of the ground + * @param x [m] Local X position of this position in the local coordinate frame (NED) + * @param y [m] Local Y position of this position in the local coordinate frame (NED) + * @param z [m] Local Z position of this position in the local coordinate frame (NED: positive "down") + * @param q + Quaternion indicating world-to-surface-normal and heading transformation of the takeoff position. + Used to indicate the heading and slope of the ground. + All fields should be set to NaN if an accurate quaternion for both heading and surface slope cannot be supplied. + * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_z [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. @@ -251,7 +346,7 @@ static inline void mavlink_msg_home_position_send(mavlink_channel_t chan, int32_ packet.approach_y = approach_y; packet.approach_z = approach_z; packet.time_usec = time_usec; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HOME_POSITION, (const char *)&packet, MAVLINK_MSG_ID_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_HOME_POSITION_LEN, MAVLINK_MSG_ID_HOME_POSITION_CRC); #endif } @@ -272,7 +367,7 @@ static inline void mavlink_msg_home_position_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_HOME_POSITION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -306,7 +401,7 @@ static inline void mavlink_msg_home_position_send_buf(mavlink_message_t *msgbuf, packet->approach_y = approach_y; packet->approach_z = approach_z; packet->time_usec = time_usec; - mav_array_memcpy(packet->q, q, sizeof(float)*4); + mav_array_assign_float(packet->q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HOME_POSITION, (const char *)packet, MAVLINK_MSG_ID_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_HOME_POSITION_LEN, MAVLINK_MSG_ID_HOME_POSITION_CRC); #endif } @@ -350,7 +445,7 @@ static inline int32_t mavlink_msg_home_position_get_altitude(const mavlink_messa /** * @brief Get field x from home_position message * - * @return [m] Local X position of this position in the local coordinate frame + * @return [m] Local X position of this position in the local coordinate frame (NED) */ static inline float mavlink_msg_home_position_get_x(const mavlink_message_t* msg) { @@ -360,7 +455,7 @@ static inline float mavlink_msg_home_position_get_x(const mavlink_message_t* msg /** * @brief Get field y from home_position message * - * @return [m] Local Y position of this position in the local coordinate frame + * @return [m] Local Y position of this position in the local coordinate frame (NED) */ static inline float mavlink_msg_home_position_get_y(const mavlink_message_t* msg) { @@ -370,7 +465,7 @@ static inline float mavlink_msg_home_position_get_y(const mavlink_message_t* msg /** * @brief Get field z from home_position message * - * @return [m] Local Z position of this position in the local coordinate frame + * @return [m] Local Z position of this position in the local coordinate frame (NED: positive "down") */ static inline float mavlink_msg_home_position_get_z(const mavlink_message_t* msg) { @@ -380,7 +475,11 @@ static inline float mavlink_msg_home_position_get_z(const mavlink_message_t* msg /** * @brief Get field q from home_position message * - * @return World to surface normal and heading transformation of the takeoff position. Used to indicate the heading and slope of the ground + * @return + Quaternion indicating world-to-surface-normal and heading transformation of the takeoff position. + Used to indicate the heading and slope of the ground. + All fields should be set to NaN if an accurate quaternion for both heading and surface slope cannot be supplied. + */ static inline uint16_t mavlink_msg_home_position_get_q(const mavlink_message_t* msg, float *q) { diff --git a/lib/main/MAVLink/common/mavlink_msg_hygrometer_sensor.h b/lib/main/MAVLink/common/mavlink_msg_hygrometer_sensor.h new file mode 100644 index 00000000000..b98713e9882 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_hygrometer_sensor.h @@ -0,0 +1,316 @@ +#pragma once +// MESSAGE HYGROMETER_SENSOR PACKING + +#define MAVLINK_MSG_ID_HYGROMETER_SENSOR 12920 + + +typedef struct __mavlink_hygrometer_sensor_t { + int16_t temperature; /*< [cdegC] Temperature*/ + uint16_t humidity; /*< [c%] Humidity*/ + uint8_t id; /*< Hygrometer ID*/ +} mavlink_hygrometer_sensor_t; + +#define MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN 5 +#define MAVLINK_MSG_ID_HYGROMETER_SENSOR_MIN_LEN 5 +#define MAVLINK_MSG_ID_12920_LEN 5 +#define MAVLINK_MSG_ID_12920_MIN_LEN 5 + +#define MAVLINK_MSG_ID_HYGROMETER_SENSOR_CRC 20 +#define MAVLINK_MSG_ID_12920_CRC 20 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_HYGROMETER_SENSOR { \ + 12920, \ + "HYGROMETER_SENSOR", \ + 3, \ + { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_hygrometer_sensor_t, id) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_hygrometer_sensor_t, temperature) }, \ + { "humidity", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_hygrometer_sensor_t, humidity) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_HYGROMETER_SENSOR { \ + "HYGROMETER_SENSOR", \ + 3, \ + { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_hygrometer_sensor_t, id) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_hygrometer_sensor_t, temperature) }, \ + { "humidity", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_hygrometer_sensor_t, humidity) }, \ + } \ +} +#endif + +/** + * @brief Pack a hygrometer_sensor message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param id Hygrometer ID + * @param temperature [cdegC] Temperature + * @param humidity [c%] Humidity + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hygrometer_sensor_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t id, int16_t temperature, uint16_t humidity) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN]; + _mav_put_int16_t(buf, 0, temperature); + _mav_put_uint16_t(buf, 2, humidity); + _mav_put_uint8_t(buf, 4, id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN); +#else + mavlink_hygrometer_sensor_t packet; + packet.temperature = temperature; + packet.humidity = humidity; + packet.id = id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HYGROMETER_SENSOR; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HYGROMETER_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN, MAVLINK_MSG_ID_HYGROMETER_SENSOR_CRC); +} + +/** + * @brief Pack a hygrometer_sensor message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param id Hygrometer ID + * @param temperature [cdegC] Temperature + * @param humidity [c%] Humidity + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hygrometer_sensor_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t id, int16_t temperature, uint16_t humidity) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN]; + _mav_put_int16_t(buf, 0, temperature); + _mav_put_uint16_t(buf, 2, humidity); + _mav_put_uint8_t(buf, 4, id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN); +#else + mavlink_hygrometer_sensor_t packet; + packet.temperature = temperature; + packet.humidity = humidity; + packet.id = id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HYGROMETER_SENSOR; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HYGROMETER_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN, MAVLINK_MSG_ID_HYGROMETER_SENSOR_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HYGROMETER_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN); +#endif +} + +/** + * @brief Pack a hygrometer_sensor message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param id Hygrometer ID + * @param temperature [cdegC] Temperature + * @param humidity [c%] Humidity + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hygrometer_sensor_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t id,int16_t temperature,uint16_t humidity) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN]; + _mav_put_int16_t(buf, 0, temperature); + _mav_put_uint16_t(buf, 2, humidity); + _mav_put_uint8_t(buf, 4, id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN); +#else + mavlink_hygrometer_sensor_t packet; + packet.temperature = temperature; + packet.humidity = humidity; + packet.id = id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HYGROMETER_SENSOR; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HYGROMETER_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN, MAVLINK_MSG_ID_HYGROMETER_SENSOR_CRC); +} + +/** + * @brief Encode a hygrometer_sensor struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param hygrometer_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hygrometer_sensor_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hygrometer_sensor_t* hygrometer_sensor) +{ + return mavlink_msg_hygrometer_sensor_pack(system_id, component_id, msg, hygrometer_sensor->id, hygrometer_sensor->temperature, hygrometer_sensor->humidity); +} + +/** + * @brief Encode a hygrometer_sensor struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hygrometer_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hygrometer_sensor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hygrometer_sensor_t* hygrometer_sensor) +{ + return mavlink_msg_hygrometer_sensor_pack_chan(system_id, component_id, chan, msg, hygrometer_sensor->id, hygrometer_sensor->temperature, hygrometer_sensor->humidity); +} + +/** + * @brief Encode a hygrometer_sensor struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param hygrometer_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hygrometer_sensor_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_hygrometer_sensor_t* hygrometer_sensor) +{ + return mavlink_msg_hygrometer_sensor_pack_status(system_id, component_id, _status, msg, hygrometer_sensor->id, hygrometer_sensor->temperature, hygrometer_sensor->humidity); +} + +/** + * @brief Send a hygrometer_sensor message + * @param chan MAVLink channel to send the message + * + * @param id Hygrometer ID + * @param temperature [cdegC] Temperature + * @param humidity [c%] Humidity + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_hygrometer_sensor_send(mavlink_channel_t chan, uint8_t id, int16_t temperature, uint16_t humidity) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN]; + _mav_put_int16_t(buf, 0, temperature); + _mav_put_uint16_t(buf, 2, humidity); + _mav_put_uint8_t(buf, 4, id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HYGROMETER_SENSOR, buf, MAVLINK_MSG_ID_HYGROMETER_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN, MAVLINK_MSG_ID_HYGROMETER_SENSOR_CRC); +#else + mavlink_hygrometer_sensor_t packet; + packet.temperature = temperature; + packet.humidity = humidity; + packet.id = id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HYGROMETER_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_HYGROMETER_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN, MAVLINK_MSG_ID_HYGROMETER_SENSOR_CRC); +#endif +} + +/** + * @brief Send a hygrometer_sensor message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_hygrometer_sensor_send_struct(mavlink_channel_t chan, const mavlink_hygrometer_sensor_t* hygrometer_sensor) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_hygrometer_sensor_send(chan, hygrometer_sensor->id, hygrometer_sensor->temperature, hygrometer_sensor->humidity); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HYGROMETER_SENSOR, (const char *)hygrometer_sensor, MAVLINK_MSG_ID_HYGROMETER_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN, MAVLINK_MSG_ID_HYGROMETER_SENSOR_CRC); +#endif +} + +#if MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hygrometer_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t id, int16_t temperature, uint16_t humidity) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, temperature); + _mav_put_uint16_t(buf, 2, humidity); + _mav_put_uint8_t(buf, 4, id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HYGROMETER_SENSOR, buf, MAVLINK_MSG_ID_HYGROMETER_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN, MAVLINK_MSG_ID_HYGROMETER_SENSOR_CRC); +#else + mavlink_hygrometer_sensor_t *packet = (mavlink_hygrometer_sensor_t *)msgbuf; + packet->temperature = temperature; + packet->humidity = humidity; + packet->id = id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HYGROMETER_SENSOR, (const char *)packet, MAVLINK_MSG_ID_HYGROMETER_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN, MAVLINK_MSG_ID_HYGROMETER_SENSOR_CRC); +#endif +} +#endif + +#endif + +// MESSAGE HYGROMETER_SENSOR UNPACKING + + +/** + * @brief Get field id from hygrometer_sensor message + * + * @return Hygrometer ID + */ +static inline uint8_t mavlink_msg_hygrometer_sensor_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field temperature from hygrometer_sensor message + * + * @return [cdegC] Temperature + */ +static inline int16_t mavlink_msg_hygrometer_sensor_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 0); +} + +/** + * @brief Get field humidity from hygrometer_sensor message + * + * @return [c%] Humidity + */ +static inline uint16_t mavlink_msg_hygrometer_sensor_get_humidity(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Decode a hygrometer_sensor message into a struct + * + * @param msg The message to decode + * @param hygrometer_sensor C-struct to decode the message contents into + */ +static inline void mavlink_msg_hygrometer_sensor_decode(const mavlink_message_t* msg, mavlink_hygrometer_sensor_t* hygrometer_sensor) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + hygrometer_sensor->temperature = mavlink_msg_hygrometer_sensor_get_temperature(msg); + hygrometer_sensor->humidity = mavlink_msg_hygrometer_sensor_get_humidity(msg); + hygrometer_sensor->id = mavlink_msg_hygrometer_sensor_get_id(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN? msg->len : MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN; + memset(hygrometer_sensor, 0, MAVLINK_MSG_ID_HYGROMETER_SENSOR_LEN); + memcpy(hygrometer_sensor, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_illuminator_status.h b/lib/main/MAVLink/common/mavlink_msg_illuminator_status.h new file mode 100644 index 00000000000..1dec13914c7 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_illuminator_status.h @@ -0,0 +1,540 @@ +#pragma once +// MESSAGE ILLUMINATOR_STATUS PACKING + +#define MAVLINK_MSG_ID_ILLUMINATOR_STATUS 440 + + +typedef struct __mavlink_illuminator_status_t { + uint32_t uptime_ms; /*< [ms] Time since the start-up of the illuminator in ms*/ + uint32_t error_status; /*< Errors*/ + float brightness; /*< [%] Illuminator brightness*/ + float strobe_period; /*< [s] Illuminator strobing period in seconds*/ + float strobe_duty_cycle; /*< [%] Illuminator strobing duty cycle*/ + float temp_c; /*< Temperature in Celsius*/ + float min_strobe_period; /*< [s] Minimum strobing period in seconds*/ + float max_strobe_period; /*< [s] Maximum strobing period in seconds*/ + uint8_t enable; /*< 0: Illuminators OFF, 1: Illuminators ON*/ + uint8_t mode_bitmask; /*< Supported illuminator modes*/ + uint8_t mode; /*< Illuminator mode*/ +} mavlink_illuminator_status_t; + +#define MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN 35 +#define MAVLINK_MSG_ID_ILLUMINATOR_STATUS_MIN_LEN 35 +#define MAVLINK_MSG_ID_440_LEN 35 +#define MAVLINK_MSG_ID_440_MIN_LEN 35 + +#define MAVLINK_MSG_ID_ILLUMINATOR_STATUS_CRC 66 +#define MAVLINK_MSG_ID_440_CRC 66 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_ILLUMINATOR_STATUS { \ + 440, \ + "ILLUMINATOR_STATUS", \ + 11, \ + { { "uptime_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_illuminator_status_t, uptime_ms) }, \ + { "enable", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_illuminator_status_t, enable) }, \ + { "mode_bitmask", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_illuminator_status_t, mode_bitmask) }, \ + { "error_status", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_illuminator_status_t, error_status) }, \ + { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_illuminator_status_t, mode) }, \ + { "brightness", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_illuminator_status_t, brightness) }, \ + { "strobe_period", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_illuminator_status_t, strobe_period) }, \ + { "strobe_duty_cycle", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_illuminator_status_t, strobe_duty_cycle) }, \ + { "temp_c", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_illuminator_status_t, temp_c) }, \ + { "min_strobe_period", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_illuminator_status_t, min_strobe_period) }, \ + { "max_strobe_period", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_illuminator_status_t, max_strobe_period) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_ILLUMINATOR_STATUS { \ + "ILLUMINATOR_STATUS", \ + 11, \ + { { "uptime_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_illuminator_status_t, uptime_ms) }, \ + { "enable", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_illuminator_status_t, enable) }, \ + { "mode_bitmask", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_illuminator_status_t, mode_bitmask) }, \ + { "error_status", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_illuminator_status_t, error_status) }, \ + { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_illuminator_status_t, mode) }, \ + { "brightness", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_illuminator_status_t, brightness) }, \ + { "strobe_period", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_illuminator_status_t, strobe_period) }, \ + { "strobe_duty_cycle", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_illuminator_status_t, strobe_duty_cycle) }, \ + { "temp_c", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_illuminator_status_t, temp_c) }, \ + { "min_strobe_period", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_illuminator_status_t, min_strobe_period) }, \ + { "max_strobe_period", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_illuminator_status_t, max_strobe_period) }, \ + } \ +} +#endif + +/** + * @brief Pack a illuminator_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param uptime_ms [ms] Time since the start-up of the illuminator in ms + * @param enable 0: Illuminators OFF, 1: Illuminators ON + * @param mode_bitmask Supported illuminator modes + * @param error_status Errors + * @param mode Illuminator mode + * @param brightness [%] Illuminator brightness + * @param strobe_period [s] Illuminator strobing period in seconds + * @param strobe_duty_cycle [%] Illuminator strobing duty cycle + * @param temp_c Temperature in Celsius + * @param min_strobe_period [s] Minimum strobing period in seconds + * @param max_strobe_period [s] Maximum strobing period in seconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_illuminator_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t uptime_ms, uint8_t enable, uint8_t mode_bitmask, uint32_t error_status, uint8_t mode, float brightness, float strobe_period, float strobe_duty_cycle, float temp_c, float min_strobe_period, float max_strobe_period) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, uptime_ms); + _mav_put_uint32_t(buf, 4, error_status); + _mav_put_float(buf, 8, brightness); + _mav_put_float(buf, 12, strobe_period); + _mav_put_float(buf, 16, strobe_duty_cycle); + _mav_put_float(buf, 20, temp_c); + _mav_put_float(buf, 24, min_strobe_period); + _mav_put_float(buf, 28, max_strobe_period); + _mav_put_uint8_t(buf, 32, enable); + _mav_put_uint8_t(buf, 33, mode_bitmask); + _mav_put_uint8_t(buf, 34, mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN); +#else + mavlink_illuminator_status_t packet; + packet.uptime_ms = uptime_ms; + packet.error_status = error_status; + packet.brightness = brightness; + packet.strobe_period = strobe_period; + packet.strobe_duty_cycle = strobe_duty_cycle; + packet.temp_c = temp_c; + packet.min_strobe_period = min_strobe_period; + packet.max_strobe_period = max_strobe_period; + packet.enable = enable; + packet.mode_bitmask = mode_bitmask; + packet.mode = mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ILLUMINATOR_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_CRC); +} + +/** + * @brief Pack a illuminator_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param uptime_ms [ms] Time since the start-up of the illuminator in ms + * @param enable 0: Illuminators OFF, 1: Illuminators ON + * @param mode_bitmask Supported illuminator modes + * @param error_status Errors + * @param mode Illuminator mode + * @param brightness [%] Illuminator brightness + * @param strobe_period [s] Illuminator strobing period in seconds + * @param strobe_duty_cycle [%] Illuminator strobing duty cycle + * @param temp_c Temperature in Celsius + * @param min_strobe_period [s] Minimum strobing period in seconds + * @param max_strobe_period [s] Maximum strobing period in seconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_illuminator_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t uptime_ms, uint8_t enable, uint8_t mode_bitmask, uint32_t error_status, uint8_t mode, float brightness, float strobe_period, float strobe_duty_cycle, float temp_c, float min_strobe_period, float max_strobe_period) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, uptime_ms); + _mav_put_uint32_t(buf, 4, error_status); + _mav_put_float(buf, 8, brightness); + _mav_put_float(buf, 12, strobe_period); + _mav_put_float(buf, 16, strobe_duty_cycle); + _mav_put_float(buf, 20, temp_c); + _mav_put_float(buf, 24, min_strobe_period); + _mav_put_float(buf, 28, max_strobe_period); + _mav_put_uint8_t(buf, 32, enable); + _mav_put_uint8_t(buf, 33, mode_bitmask); + _mav_put_uint8_t(buf, 34, mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN); +#else + mavlink_illuminator_status_t packet; + packet.uptime_ms = uptime_ms; + packet.error_status = error_status; + packet.brightness = brightness; + packet.strobe_period = strobe_period; + packet.strobe_duty_cycle = strobe_duty_cycle; + packet.temp_c = temp_c; + packet.min_strobe_period = min_strobe_period; + packet.max_strobe_period = max_strobe_period; + packet.enable = enable; + packet.mode_bitmask = mode_bitmask; + packet.mode = mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ILLUMINATOR_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN); +#endif +} + +/** + * @brief Pack a illuminator_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param uptime_ms [ms] Time since the start-up of the illuminator in ms + * @param enable 0: Illuminators OFF, 1: Illuminators ON + * @param mode_bitmask Supported illuminator modes + * @param error_status Errors + * @param mode Illuminator mode + * @param brightness [%] Illuminator brightness + * @param strobe_period [s] Illuminator strobing period in seconds + * @param strobe_duty_cycle [%] Illuminator strobing duty cycle + * @param temp_c Temperature in Celsius + * @param min_strobe_period [s] Minimum strobing period in seconds + * @param max_strobe_period [s] Maximum strobing period in seconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_illuminator_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t uptime_ms,uint8_t enable,uint8_t mode_bitmask,uint32_t error_status,uint8_t mode,float brightness,float strobe_period,float strobe_duty_cycle,float temp_c,float min_strobe_period,float max_strobe_period) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, uptime_ms); + _mav_put_uint32_t(buf, 4, error_status); + _mav_put_float(buf, 8, brightness); + _mav_put_float(buf, 12, strobe_period); + _mav_put_float(buf, 16, strobe_duty_cycle); + _mav_put_float(buf, 20, temp_c); + _mav_put_float(buf, 24, min_strobe_period); + _mav_put_float(buf, 28, max_strobe_period); + _mav_put_uint8_t(buf, 32, enable); + _mav_put_uint8_t(buf, 33, mode_bitmask); + _mav_put_uint8_t(buf, 34, mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN); +#else + mavlink_illuminator_status_t packet; + packet.uptime_ms = uptime_ms; + packet.error_status = error_status; + packet.brightness = brightness; + packet.strobe_period = strobe_period; + packet.strobe_duty_cycle = strobe_duty_cycle; + packet.temp_c = temp_c; + packet.min_strobe_period = min_strobe_period; + packet.max_strobe_period = max_strobe_period; + packet.enable = enable; + packet.mode_bitmask = mode_bitmask; + packet.mode = mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ILLUMINATOR_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_CRC); +} + +/** + * @brief Encode a illuminator_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param illuminator_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_illuminator_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_illuminator_status_t* illuminator_status) +{ + return mavlink_msg_illuminator_status_pack(system_id, component_id, msg, illuminator_status->uptime_ms, illuminator_status->enable, illuminator_status->mode_bitmask, illuminator_status->error_status, illuminator_status->mode, illuminator_status->brightness, illuminator_status->strobe_period, illuminator_status->strobe_duty_cycle, illuminator_status->temp_c, illuminator_status->min_strobe_period, illuminator_status->max_strobe_period); +} + +/** + * @brief Encode a illuminator_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param illuminator_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_illuminator_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_illuminator_status_t* illuminator_status) +{ + return mavlink_msg_illuminator_status_pack_chan(system_id, component_id, chan, msg, illuminator_status->uptime_ms, illuminator_status->enable, illuminator_status->mode_bitmask, illuminator_status->error_status, illuminator_status->mode, illuminator_status->brightness, illuminator_status->strobe_period, illuminator_status->strobe_duty_cycle, illuminator_status->temp_c, illuminator_status->min_strobe_period, illuminator_status->max_strobe_period); +} + +/** + * @brief Encode a illuminator_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param illuminator_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_illuminator_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_illuminator_status_t* illuminator_status) +{ + return mavlink_msg_illuminator_status_pack_status(system_id, component_id, _status, msg, illuminator_status->uptime_ms, illuminator_status->enable, illuminator_status->mode_bitmask, illuminator_status->error_status, illuminator_status->mode, illuminator_status->brightness, illuminator_status->strobe_period, illuminator_status->strobe_duty_cycle, illuminator_status->temp_c, illuminator_status->min_strobe_period, illuminator_status->max_strobe_period); +} + +/** + * @brief Send a illuminator_status message + * @param chan MAVLink channel to send the message + * + * @param uptime_ms [ms] Time since the start-up of the illuminator in ms + * @param enable 0: Illuminators OFF, 1: Illuminators ON + * @param mode_bitmask Supported illuminator modes + * @param error_status Errors + * @param mode Illuminator mode + * @param brightness [%] Illuminator brightness + * @param strobe_period [s] Illuminator strobing period in seconds + * @param strobe_duty_cycle [%] Illuminator strobing duty cycle + * @param temp_c Temperature in Celsius + * @param min_strobe_period [s] Minimum strobing period in seconds + * @param max_strobe_period [s] Maximum strobing period in seconds + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_illuminator_status_send(mavlink_channel_t chan, uint32_t uptime_ms, uint8_t enable, uint8_t mode_bitmask, uint32_t error_status, uint8_t mode, float brightness, float strobe_period, float strobe_duty_cycle, float temp_c, float min_strobe_period, float max_strobe_period) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, uptime_ms); + _mav_put_uint32_t(buf, 4, error_status); + _mav_put_float(buf, 8, brightness); + _mav_put_float(buf, 12, strobe_period); + _mav_put_float(buf, 16, strobe_duty_cycle); + _mav_put_float(buf, 20, temp_c); + _mav_put_float(buf, 24, min_strobe_period); + _mav_put_float(buf, 28, max_strobe_period); + _mav_put_uint8_t(buf, 32, enable); + _mav_put_uint8_t(buf, 33, mode_bitmask); + _mav_put_uint8_t(buf, 34, mode); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ILLUMINATOR_STATUS, buf, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_CRC); +#else + mavlink_illuminator_status_t packet; + packet.uptime_ms = uptime_ms; + packet.error_status = error_status; + packet.brightness = brightness; + packet.strobe_period = strobe_period; + packet.strobe_duty_cycle = strobe_duty_cycle; + packet.temp_c = temp_c; + packet.min_strobe_period = min_strobe_period; + packet.max_strobe_period = max_strobe_period; + packet.enable = enable; + packet.mode_bitmask = mode_bitmask; + packet.mode = mode; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ILLUMINATOR_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_CRC); +#endif +} + +/** + * @brief Send a illuminator_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_illuminator_status_send_struct(mavlink_channel_t chan, const mavlink_illuminator_status_t* illuminator_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_illuminator_status_send(chan, illuminator_status->uptime_ms, illuminator_status->enable, illuminator_status->mode_bitmask, illuminator_status->error_status, illuminator_status->mode, illuminator_status->brightness, illuminator_status->strobe_period, illuminator_status->strobe_duty_cycle, illuminator_status->temp_c, illuminator_status->min_strobe_period, illuminator_status->max_strobe_period); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ILLUMINATOR_STATUS, (const char *)illuminator_status, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_illuminator_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t uptime_ms, uint8_t enable, uint8_t mode_bitmask, uint32_t error_status, uint8_t mode, float brightness, float strobe_period, float strobe_duty_cycle, float temp_c, float min_strobe_period, float max_strobe_period) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, uptime_ms); + _mav_put_uint32_t(buf, 4, error_status); + _mav_put_float(buf, 8, brightness); + _mav_put_float(buf, 12, strobe_period); + _mav_put_float(buf, 16, strobe_duty_cycle); + _mav_put_float(buf, 20, temp_c); + _mav_put_float(buf, 24, min_strobe_period); + _mav_put_float(buf, 28, max_strobe_period); + _mav_put_uint8_t(buf, 32, enable); + _mav_put_uint8_t(buf, 33, mode_bitmask); + _mav_put_uint8_t(buf, 34, mode); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ILLUMINATOR_STATUS, buf, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_CRC); +#else + mavlink_illuminator_status_t *packet = (mavlink_illuminator_status_t *)msgbuf; + packet->uptime_ms = uptime_ms; + packet->error_status = error_status; + packet->brightness = brightness; + packet->strobe_period = strobe_period; + packet->strobe_duty_cycle = strobe_duty_cycle; + packet->temp_c = temp_c; + packet->min_strobe_period = min_strobe_period; + packet->max_strobe_period = max_strobe_period; + packet->enable = enable; + packet->mode_bitmask = mode_bitmask; + packet->mode = mode; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ILLUMINATOR_STATUS, (const char *)packet, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE ILLUMINATOR_STATUS UNPACKING + + +/** + * @brief Get field uptime_ms from illuminator_status message + * + * @return [ms] Time since the start-up of the illuminator in ms + */ +static inline uint32_t mavlink_msg_illuminator_status_get_uptime_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field enable from illuminator_status message + * + * @return 0: Illuminators OFF, 1: Illuminators ON + */ +static inline uint8_t mavlink_msg_illuminator_status_get_enable(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field mode_bitmask from illuminator_status message + * + * @return Supported illuminator modes + */ +static inline uint8_t mavlink_msg_illuminator_status_get_mode_bitmask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field error_status from illuminator_status message + * + * @return Errors + */ +static inline uint32_t mavlink_msg_illuminator_status_get_error_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field mode from illuminator_status message + * + * @return Illuminator mode + */ +static inline uint8_t mavlink_msg_illuminator_status_get_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field brightness from illuminator_status message + * + * @return [%] Illuminator brightness + */ +static inline float mavlink_msg_illuminator_status_get_brightness(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field strobe_period from illuminator_status message + * + * @return [s] Illuminator strobing period in seconds + */ +static inline float mavlink_msg_illuminator_status_get_strobe_period(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field strobe_duty_cycle from illuminator_status message + * + * @return [%] Illuminator strobing duty cycle + */ +static inline float mavlink_msg_illuminator_status_get_strobe_duty_cycle(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field temp_c from illuminator_status message + * + * @return Temperature in Celsius + */ +static inline float mavlink_msg_illuminator_status_get_temp_c(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field min_strobe_period from illuminator_status message + * + * @return [s] Minimum strobing period in seconds + */ +static inline float mavlink_msg_illuminator_status_get_min_strobe_period(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field max_strobe_period from illuminator_status message + * + * @return [s] Maximum strobing period in seconds + */ +static inline float mavlink_msg_illuminator_status_get_max_strobe_period(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Decode a illuminator_status message into a struct + * + * @param msg The message to decode + * @param illuminator_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_illuminator_status_decode(const mavlink_message_t* msg, mavlink_illuminator_status_t* illuminator_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + illuminator_status->uptime_ms = mavlink_msg_illuminator_status_get_uptime_ms(msg); + illuminator_status->error_status = mavlink_msg_illuminator_status_get_error_status(msg); + illuminator_status->brightness = mavlink_msg_illuminator_status_get_brightness(msg); + illuminator_status->strobe_period = mavlink_msg_illuminator_status_get_strobe_period(msg); + illuminator_status->strobe_duty_cycle = mavlink_msg_illuminator_status_get_strobe_duty_cycle(msg); + illuminator_status->temp_c = mavlink_msg_illuminator_status_get_temp_c(msg); + illuminator_status->min_strobe_period = mavlink_msg_illuminator_status_get_min_strobe_period(msg); + illuminator_status->max_strobe_period = mavlink_msg_illuminator_status_get_max_strobe_period(msg); + illuminator_status->enable = mavlink_msg_illuminator_status_get_enable(msg); + illuminator_status->mode_bitmask = mavlink_msg_illuminator_status_get_mode_bitmask(msg); + illuminator_status->mode = mavlink_msg_illuminator_status_get_mode(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN? msg->len : MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN; + memset(illuminator_status, 0, MAVLINK_MSG_ID_ILLUMINATOR_STATUS_LEN); + memcpy(illuminator_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_isbd_link_status.h b/lib/main/MAVLink/common/mavlink_msg_isbd_link_status.h index a0c53745f97..7f3060a6312 100644 --- a/lib/main/MAVLink/common/mavlink_msg_isbd_link_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_isbd_link_status.h @@ -105,6 +105,60 @@ static inline uint16_t mavlink_msg_isbd_link_status_pack(uint8_t system_id, uint return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_CRC); } +/** + * @brief Pack a isbd_link_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param timestamp [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param last_heartbeat [us] Timestamp of the last successful sbd session. The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param failed_sessions Number of failed SBD sessions. + * @param successful_sessions Number of successful SBD sessions. + * @param signal_quality Signal quality equal to the number of bars displayed on the ISU signal strength indicator. Range is 0 to 5, where 0 indicates no signal and 5 indicates maximum signal strength. + * @param ring_pending 1: Ring call pending, 0: No call pending. + * @param tx_session_pending 1: Transmission session pending, 0: No transmission session pending. + * @param rx_session_pending 1: Receiving session pending, 0: No receiving session pending. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_isbd_link_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t timestamp, uint64_t last_heartbeat, uint16_t failed_sessions, uint16_t successful_sessions, uint8_t signal_quality, uint8_t ring_pending, uint8_t tx_session_pending, uint8_t rx_session_pending) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, timestamp); + _mav_put_uint64_t(buf, 8, last_heartbeat); + _mav_put_uint16_t(buf, 16, failed_sessions); + _mav_put_uint16_t(buf, 18, successful_sessions); + _mav_put_uint8_t(buf, 20, signal_quality); + _mav_put_uint8_t(buf, 21, ring_pending); + _mav_put_uint8_t(buf, 22, tx_session_pending); + _mav_put_uint8_t(buf, 23, rx_session_pending); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN); +#else + mavlink_isbd_link_status_t packet; + packet.timestamp = timestamp; + packet.last_heartbeat = last_heartbeat; + packet.failed_sessions = failed_sessions; + packet.successful_sessions = successful_sessions; + packet.signal_quality = signal_quality; + packet.ring_pending = ring_pending; + packet.tx_session_pending = tx_session_pending; + packet.rx_session_pending = rx_session_pending; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ISBD_LINK_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN); +#endif +} + /** * @brief Pack a isbd_link_status message on a channel * @param system_id ID of this system @@ -182,6 +236,20 @@ static inline uint16_t mavlink_msg_isbd_link_status_encode_chan(uint8_t system_i return mavlink_msg_isbd_link_status_pack_chan(system_id, component_id, chan, msg, isbd_link_status->timestamp, isbd_link_status->last_heartbeat, isbd_link_status->failed_sessions, isbd_link_status->successful_sessions, isbd_link_status->signal_quality, isbd_link_status->ring_pending, isbd_link_status->tx_session_pending, isbd_link_status->rx_session_pending); } +/** + * @brief Encode a isbd_link_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param isbd_link_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_isbd_link_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_isbd_link_status_t* isbd_link_status) +{ + return mavlink_msg_isbd_link_status_pack_status(system_id, component_id, _status, msg, isbd_link_status->timestamp, isbd_link_status->last_heartbeat, isbd_link_status->failed_sessions, isbd_link_status->successful_sessions, isbd_link_status->signal_quality, isbd_link_status->ring_pending, isbd_link_status->tx_session_pending, isbd_link_status->rx_session_pending); +} + /** * @brief Send a isbd_link_status message * @param chan MAVLink channel to send the message @@ -242,7 +310,7 @@ static inline void mavlink_msg_isbd_link_status_send_struct(mavlink_channel_t ch #if MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_landing_target.h b/lib/main/MAVLink/common/mavlink_msg_landing_target.h index 1d1b41ed8e3..bb863dde203 100755 --- a/lib/main/MAVLink/common/mavlink_msg_landing_target.h +++ b/lib/main/MAVLink/common/mavlink_msg_landing_target.h @@ -18,7 +18,7 @@ typedef struct __mavlink_landing_target_t { float z; /*< [m] Z Position of the landing target in MAV_FRAME*/ float q[4]; /*< Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0)*/ uint8_t type; /*< Type of landing target*/ - uint8_t position_valid; /*< Boolean indicating whether the position fields (x, y, z, q, type) contain valid target position information (valid: 1, invalid: 0). Default is 0 (invalid).*/ + uint8_t position_valid; /*< Position fields (x, y, z, q, type) contain valid target position information (MAV_BOOL_FALSE: invalid values). Values not equal to 0 or 1 are invalid.*/ }) mavlink_landing_target_t; #define MAVLINK_MSG_ID_LANDING_TARGET_LEN 60 @@ -93,7 +93,7 @@ typedef struct __mavlink_landing_target_t { * @param z [m] Z Position of the landing target in MAV_FRAME * @param q Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param type Type of landing target - * @param position_valid Boolean indicating whether the position fields (x, y, z, q, type) contain valid target position information (valid: 1, invalid: 0). Default is 0 (invalid). + * @param position_valid Position fields (x, y, z, q, type) contain valid target position information (MAV_BOOL_FALSE: invalid values). Values not equal to 0 or 1 are invalid. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_landing_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -131,7 +131,7 @@ static inline uint16_t mavlink_msg_landing_target_pack(uint8_t system_id, uint8_ packet.z = z; packet.type = type; packet.position_valid = position_valid; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LANDING_TARGET_LEN); #endif @@ -139,6 +139,76 @@ static inline uint16_t mavlink_msg_landing_target_pack(uint8_t system_id, uint8_ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LANDING_TARGET_MIN_LEN, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); } +/** + * @brief Pack a landing_target message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param target_num The ID of the target if multiple targets are present + * @param frame Coordinate frame used for following fields. + * @param angle_x [rad] X-axis angular offset of the target from the center of the image + * @param angle_y [rad] Y-axis angular offset of the target from the center of the image + * @param distance [m] Distance to the target from the vehicle + * @param size_x [rad] Size of target along x-axis + * @param size_y [rad] Size of target along y-axis + * @param x [m] X Position of the landing target in MAV_FRAME + * @param y [m] Y Position of the landing target in MAV_FRAME + * @param z [m] Z Position of the landing target in MAV_FRAME + * @param q Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param type Type of landing target + * @param position_valid Position fields (x, y, z, q, type) contain valid target position information (MAV_BOOL_FALSE: invalid values). Values not equal to 0 or 1 are invalid. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_landing_target_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint8_t target_num, uint8_t frame, float angle_x, float angle_y, float distance, float size_x, float size_y, float x, float y, float z, const float *q, uint8_t type, uint8_t position_valid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LANDING_TARGET_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, angle_x); + _mav_put_float(buf, 12, angle_y); + _mav_put_float(buf, 16, distance); + _mav_put_float(buf, 20, size_x); + _mav_put_float(buf, 24, size_y); + _mav_put_uint8_t(buf, 28, target_num); + _mav_put_uint8_t(buf, 29, frame); + _mav_put_float(buf, 30, x); + _mav_put_float(buf, 34, y); + _mav_put_float(buf, 38, z); + _mav_put_uint8_t(buf, 58, type); + _mav_put_uint8_t(buf, 59, position_valid); + _mav_put_float_array(buf, 42, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LANDING_TARGET_LEN); +#else + mavlink_landing_target_t packet; + packet.time_usec = time_usec; + packet.angle_x = angle_x; + packet.angle_y = angle_y; + packet.distance = distance; + packet.size_x = size_x; + packet.size_y = size_y; + packet.target_num = target_num; + packet.frame = frame; + packet.x = x; + packet.y = y; + packet.z = z; + packet.type = type; + packet.position_valid = position_valid; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LANDING_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LANDING_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LANDING_TARGET_MIN_LEN, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LANDING_TARGET_MIN_LEN, MAVLINK_MSG_ID_LANDING_TARGET_LEN); +#endif +} + /** * @brief Pack a landing_target message on a channel * @param system_id ID of this system @@ -158,7 +228,7 @@ static inline uint16_t mavlink_msg_landing_target_pack(uint8_t system_id, uint8_ * @param z [m] Z Position of the landing target in MAV_FRAME * @param q Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param type Type of landing target - * @param position_valid Boolean indicating whether the position fields (x, y, z, q, type) contain valid target position information (valid: 1, invalid: 0). Default is 0 (invalid). + * @param position_valid Position fields (x, y, z, q, type) contain valid target position information (MAV_BOOL_FALSE: invalid values). Values not equal to 0 or 1 are invalid. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_landing_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -197,7 +267,7 @@ static inline uint16_t mavlink_msg_landing_target_pack_chan(uint8_t system_id, u packet.z = z; packet.type = type; packet.position_valid = position_valid; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LANDING_TARGET_LEN); #endif @@ -232,6 +302,20 @@ static inline uint16_t mavlink_msg_landing_target_encode_chan(uint8_t system_id, return mavlink_msg_landing_target_pack_chan(system_id, component_id, chan, msg, landing_target->time_usec, landing_target->target_num, landing_target->frame, landing_target->angle_x, landing_target->angle_y, landing_target->distance, landing_target->size_x, landing_target->size_y, landing_target->x, landing_target->y, landing_target->z, landing_target->q, landing_target->type, landing_target->position_valid); } +/** + * @brief Encode a landing_target struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param landing_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_landing_target_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_landing_target_t* landing_target) +{ + return mavlink_msg_landing_target_pack_status(system_id, component_id, _status, msg, landing_target->time_usec, landing_target->target_num, landing_target->frame, landing_target->angle_x, landing_target->angle_y, landing_target->distance, landing_target->size_x, landing_target->size_y, landing_target->x, landing_target->y, landing_target->z, landing_target->q, landing_target->type, landing_target->position_valid); +} + /** * @brief Send a landing_target message * @param chan MAVLink channel to send the message @@ -249,7 +333,7 @@ static inline uint16_t mavlink_msg_landing_target_encode_chan(uint8_t system_id, * @param z [m] Z Position of the landing target in MAV_FRAME * @param q Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param type Type of landing target - * @param position_valid Boolean indicating whether the position fields (x, y, z, q, type) contain valid target position information (valid: 1, invalid: 0). Default is 0 (invalid). + * @param position_valid Position fields (x, y, z, q, type) contain valid target position information (MAV_BOOL_FALSE: invalid values). Values not equal to 0 or 1 are invalid. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -287,7 +371,7 @@ static inline void mavlink_msg_landing_target_send(mavlink_channel_t chan, uint6 packet.z = z; packet.type = type; packet.position_valid = position_valid; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, (const char *)&packet, MAVLINK_MSG_ID_LANDING_TARGET_MIN_LEN, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); #endif } @@ -308,7 +392,7 @@ static inline void mavlink_msg_landing_target_send_struct(mavlink_channel_t chan #if MAVLINK_MSG_ID_LANDING_TARGET_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -348,7 +432,7 @@ static inline void mavlink_msg_landing_target_send_buf(mavlink_message_t *msgbuf packet->z = z; packet->type = type; packet->position_valid = position_valid; - mav_array_memcpy(packet->q, q, sizeof(float)*4); + mav_array_assign_float(packet->q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, (const char *)packet, MAVLINK_MSG_ID_LANDING_TARGET_MIN_LEN, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); #endif } @@ -492,7 +576,7 @@ static inline uint8_t mavlink_msg_landing_target_get_type(const mavlink_message_ /** * @brief Get field position_valid from landing_target message * - * @return Boolean indicating whether the position fields (x, y, z, q, type) contain valid target position information (valid: 1, invalid: 0). Default is 0 (invalid). + * @return Position fields (x, y, z, q, type) contain valid target position information (MAV_BOOL_FALSE: invalid values). Values not equal to 0 or 1 are invalid. */ static inline uint8_t mavlink_msg_landing_target_get_position_valid(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_link_node_status.h b/lib/main/MAVLink/common/mavlink_msg_link_node_status.h index 8da7b71d9e7..d3bb36b8a16 100644 --- a/lib/main/MAVLink/common/mavlink_msg_link_node_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_link_node_status.h @@ -123,6 +123,69 @@ static inline uint16_t mavlink_msg_link_node_status_pack(uint8_t system_id, uint return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LINK_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_LINK_NODE_STATUS_LEN, MAVLINK_MSG_ID_LINK_NODE_STATUS_CRC); } +/** + * @brief Pack a link_node_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param timestamp [ms] Timestamp (time since system boot). + * @param tx_buf [%] Remaining free transmit buffer space + * @param rx_buf [%] Remaining free receive buffer space + * @param tx_rate [bytes/s] Transmit rate + * @param rx_rate [bytes/s] Receive rate + * @param rx_parse_err [bytes] Number of bytes that could not be parsed correctly. + * @param tx_overflows [bytes] Transmit buffer overflows. This number wraps around as it reaches UINT16_MAX + * @param rx_overflows [bytes] Receive buffer overflows. This number wraps around as it reaches UINT16_MAX + * @param messages_sent Messages sent + * @param messages_received Messages received (estimated from counting seq) + * @param messages_lost Messages lost (estimated from counting seq) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_link_node_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t timestamp, uint8_t tx_buf, uint8_t rx_buf, uint32_t tx_rate, uint32_t rx_rate, uint16_t rx_parse_err, uint16_t tx_overflows, uint16_t rx_overflows, uint32_t messages_sent, uint32_t messages_received, uint32_t messages_lost) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LINK_NODE_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, timestamp); + _mav_put_uint32_t(buf, 8, tx_rate); + _mav_put_uint32_t(buf, 12, rx_rate); + _mav_put_uint32_t(buf, 16, messages_sent); + _mav_put_uint32_t(buf, 20, messages_received); + _mav_put_uint32_t(buf, 24, messages_lost); + _mav_put_uint16_t(buf, 28, rx_parse_err); + _mav_put_uint16_t(buf, 30, tx_overflows); + _mav_put_uint16_t(buf, 32, rx_overflows); + _mav_put_uint8_t(buf, 34, tx_buf); + _mav_put_uint8_t(buf, 35, rx_buf); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LINK_NODE_STATUS_LEN); +#else + mavlink_link_node_status_t packet; + packet.timestamp = timestamp; + packet.tx_rate = tx_rate; + packet.rx_rate = rx_rate; + packet.messages_sent = messages_sent; + packet.messages_received = messages_received; + packet.messages_lost = messages_lost; + packet.rx_parse_err = rx_parse_err; + packet.tx_overflows = tx_overflows; + packet.rx_overflows = rx_overflows; + packet.tx_buf = tx_buf; + packet.rx_buf = rx_buf; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LINK_NODE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LINK_NODE_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LINK_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_LINK_NODE_STATUS_LEN, MAVLINK_MSG_ID_LINK_NODE_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LINK_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_LINK_NODE_STATUS_LEN); +#endif +} + /** * @brief Pack a link_node_status message on a channel * @param system_id ID of this system @@ -209,6 +272,20 @@ static inline uint16_t mavlink_msg_link_node_status_encode_chan(uint8_t system_i return mavlink_msg_link_node_status_pack_chan(system_id, component_id, chan, msg, link_node_status->timestamp, link_node_status->tx_buf, link_node_status->rx_buf, link_node_status->tx_rate, link_node_status->rx_rate, link_node_status->rx_parse_err, link_node_status->tx_overflows, link_node_status->rx_overflows, link_node_status->messages_sent, link_node_status->messages_received, link_node_status->messages_lost); } +/** + * @brief Encode a link_node_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param link_node_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_link_node_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_link_node_status_t* link_node_status) +{ + return mavlink_msg_link_node_status_pack_status(system_id, component_id, _status, msg, link_node_status->timestamp, link_node_status->tx_buf, link_node_status->rx_buf, link_node_status->tx_rate, link_node_status->rx_rate, link_node_status->rx_parse_err, link_node_status->tx_overflows, link_node_status->rx_overflows, link_node_status->messages_sent, link_node_status->messages_received, link_node_status->messages_lost); +} + /** * @brief Send a link_node_status message * @param chan MAVLink channel to send the message @@ -278,7 +355,7 @@ static inline void mavlink_msg_link_node_status_send_struct(mavlink_channel_t ch #if MAVLINK_MSG_ID_LINK_NODE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_local_position_ned.h b/lib/main/MAVLink/common/mavlink_msg_local_position_ned.h index 48c71d90d70..a7874358675 100755 --- a/lib/main/MAVLink/common/mavlink_msg_local_position_ned.h +++ b/lib/main/MAVLink/common/mavlink_msg_local_position_ned.h @@ -99,6 +99,57 @@ static inline uint16_t mavlink_msg_local_position_ned_pack(uint8_t system_id, ui return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); } +/** + * @brief Pack a local_position_ned message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param x [m] X Position + * @param y [m] Y Position + * @param z [m] Z Position + * @param vx [m/s] X Speed + * @param vy [m/s] Y Speed + * @param vz [m/s] Z Speed + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_local_position_ned_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, float x, float y, float z, float vx, float vy, float vz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#else + mavlink_local_position_ned_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOCAL_POSITION_NED_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOCAL_POSITION_NED_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#endif +} + /** * @brief Pack a local_position_ned message on a channel * @param system_id ID of this system @@ -173,6 +224,20 @@ static inline uint16_t mavlink_msg_local_position_ned_encode_chan(uint8_t system return mavlink_msg_local_position_ned_pack_chan(system_id, component_id, chan, msg, local_position_ned->time_boot_ms, local_position_ned->x, local_position_ned->y, local_position_ned->z, local_position_ned->vx, local_position_ned->vy, local_position_ned->vz); } +/** + * @brief Encode a local_position_ned struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param local_position_ned C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_ned_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_local_position_ned_t* local_position_ned) +{ + return mavlink_msg_local_position_ned_pack_status(system_id, component_id, _status, msg, local_position_ned->time_boot_ms, local_position_ned->x, local_position_ned->y, local_position_ned->z, local_position_ned->vx, local_position_ned->vy, local_position_ned->vz); +} + /** * @brief Send a local_position_ned message * @param chan MAVLink channel to send the message @@ -230,7 +295,7 @@ static inline void mavlink_msg_local_position_ned_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_local_position_ned_cov.h b/lib/main/MAVLink/common/mavlink_msg_local_position_ned_cov.h index 8497c7d0afb..9af9f1694b1 100755 --- a/lib/main/MAVLink/common/mavlink_msg_local_position_ned_cov.h +++ b/lib/main/MAVLink/common/mavlink_msg_local_position_ned_cov.h @@ -119,7 +119,7 @@ static inline uint16_t mavlink_msg_local_position_ned_cov_pack(uint8_t system_id packet.ay = ay; packet.az = az; packet.estimator_type = estimator_type; - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*45); + mav_array_assign_float(packet.covariance, covariance, 45); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); #endif @@ -127,6 +127,70 @@ static inline uint16_t mavlink_msg_local_position_ned_cov_pack(uint8_t system_id return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC); } +/** + * @brief Pack a local_position_ned_cov message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param estimator_type Class id of the estimator this estimate originated from. + * @param x [m] X Position + * @param y [m] Y Position + * @param z [m] Z Position + * @param vx [m/s] X Speed + * @param vy [m/s] Y Speed + * @param vz [m/s] Z Speed + * @param ax [m/s/s] X Acceleration + * @param ay [m/s/s] Y Acceleration + * @param az [m/s/s] Z Acceleration + * @param covariance Row-major representation of position, velocity and acceleration 9x9 cross-covariance matrix upper right triangle (states: x, y, z, vx, vy, vz, ax, ay, az; first nine entries are the first ROW, next eight entries are the second row, etc.). If unknown, assign NaN value to first element in the array. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_local_position_ned_cov_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint8_t estimator_type, float x, float y, float z, float vx, float vy, float vz, float ax, float ay, float az, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, vx); + _mav_put_float(buf, 24, vy); + _mav_put_float(buf, 28, vz); + _mav_put_float(buf, 32, ax); + _mav_put_float(buf, 36, ay); + _mav_put_float(buf, 40, az); + _mav_put_uint8_t(buf, 224, estimator_type); + _mav_put_float_array(buf, 44, covariance, 45); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#else + mavlink_local_position_ned_cov_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.ax = ax; + packet.ay = ay; + packet.az = az; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*45); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); +#endif +} + /** * @brief Pack a local_position_ned_cov message on a channel * @param system_id ID of this system @@ -179,7 +243,7 @@ static inline uint16_t mavlink_msg_local_position_ned_cov_pack_chan(uint8_t syst packet.ay = ay; packet.az = az; packet.estimator_type = estimator_type; - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*45); + mav_array_assign_float(packet.covariance, covariance, 45); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN); #endif @@ -214,6 +278,20 @@ static inline uint16_t mavlink_msg_local_position_ned_cov_encode_chan(uint8_t sy return mavlink_msg_local_position_ned_cov_pack_chan(system_id, component_id, chan, msg, local_position_ned_cov->time_usec, local_position_ned_cov->estimator_type, local_position_ned_cov->x, local_position_ned_cov->y, local_position_ned_cov->z, local_position_ned_cov->vx, local_position_ned_cov->vy, local_position_ned_cov->vz, local_position_ned_cov->ax, local_position_ned_cov->ay, local_position_ned_cov->az, local_position_ned_cov->covariance); } +/** + * @brief Encode a local_position_ned_cov struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param local_position_ned_cov C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_ned_cov_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_local_position_ned_cov_t* local_position_ned_cov) +{ + return mavlink_msg_local_position_ned_cov_pack_status(system_id, component_id, _status, msg, local_position_ned_cov->time_usec, local_position_ned_cov->estimator_type, local_position_ned_cov->x, local_position_ned_cov->y, local_position_ned_cov->z, local_position_ned_cov->vx, local_position_ned_cov->vy, local_position_ned_cov->vz, local_position_ned_cov->ax, local_position_ned_cov->ay, local_position_ned_cov->az, local_position_ned_cov->covariance); +} + /** * @brief Send a local_position_ned_cov message * @param chan MAVLink channel to send the message @@ -263,7 +341,7 @@ static inline void mavlink_msg_local_position_ned_cov_send(mavlink_channel_t cha packet.ay = ay; packet.az = az; packet.estimator_type = estimator_type; - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*45); + mav_array_assign_float(packet.covariance, covariance, 45); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, (const char *)&packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC); #endif } @@ -284,7 +362,7 @@ static inline void mavlink_msg_local_position_ned_cov_send_struct(mavlink_channe #if MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -320,7 +398,7 @@ static inline void mavlink_msg_local_position_ned_cov_send_buf(mavlink_message_t packet->ay = ay; packet->az = az; packet->estimator_type = estimator_type; - mav_array_memcpy(packet->covariance, covariance, sizeof(float)*45); + mav_array_assign_float(packet->covariance, covariance, 45); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_local_position_ned_system_global_offset.h b/lib/main/MAVLink/common/mavlink_msg_local_position_ned_system_global_offset.h index 8ff2ba6526b..2b8b696b700 100755 --- a/lib/main/MAVLink/common/mavlink_msg_local_position_ned_system_global_offset.h +++ b/lib/main/MAVLink/common/mavlink_msg_local_position_ned_system_global_offset.h @@ -99,6 +99,57 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack( return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); } +/** + * @brief Pack a local_position_ned_system_global_offset message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param x [m] X Position + * @param y [m] Y Position + * @param z [m] Z Position + * @param roll [rad] Roll + * @param pitch [rad] Pitch + * @param yaw [rad] Yaw + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, roll); + _mav_put_float(buf, 20, pitch); + _mav_put_float(buf, 24, yaw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#else + mavlink_local_position_ned_system_global_offset_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_MIN_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#endif +} + /** * @brief Pack a local_position_ned_system_global_offset message on a channel * @param system_id ID of this system @@ -173,6 +224,20 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_encod return mavlink_msg_local_position_ned_system_global_offset_pack_chan(system_id, component_id, chan, msg, local_position_ned_system_global_offset->time_boot_ms, local_position_ned_system_global_offset->x, local_position_ned_system_global_offset->y, local_position_ned_system_global_offset->z, local_position_ned_system_global_offset->roll, local_position_ned_system_global_offset->pitch, local_position_ned_system_global_offset->yaw); } +/** + * @brief Encode a local_position_ned_system_global_offset struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param local_position_ned_system_global_offset C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_local_position_ned_system_global_offset_t* local_position_ned_system_global_offset) +{ + return mavlink_msg_local_position_ned_system_global_offset_pack_status(system_id, component_id, _status, msg, local_position_ned_system_global_offset->time_boot_ms, local_position_ned_system_global_offset->x, local_position_ned_system_global_offset->y, local_position_ned_system_global_offset->z, local_position_ned_system_global_offset->roll, local_position_ned_system_global_offset->pitch, local_position_ned_system_global_offset->yaw); +} + /** * @brief Send a local_position_ned_system_global_offset message * @param chan MAVLink channel to send the message @@ -230,7 +295,7 @@ static inline void mavlink_msg_local_position_ned_system_global_offset_send_stru #if MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_log_data.h b/lib/main/MAVLink/common/mavlink_msg_log_data.h index 37cd7777701..413d548267f 100755 --- a/lib/main/MAVLink/common/mavlink_msg_log_data.h +++ b/lib/main/MAVLink/common/mavlink_msg_log_data.h @@ -71,7 +71,7 @@ static inline uint16_t mavlink_msg_log_data_pack(uint8_t system_id, uint8_t comp packet.ofs = ofs; packet.id = id; packet.count = count; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*90); + mav_array_assign_uint8_t(packet.data, data, 90); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_DATA_LEN); #endif @@ -79,6 +79,46 @@ static inline uint16_t mavlink_msg_log_data_pack(uint8_t system_id, uint8_t comp return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_DATA_MIN_LEN, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); } +/** + * @brief Pack a log_data message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param id Log id (from LOG_ENTRY reply) + * @param ofs Offset into the log + * @param count [bytes] Number of bytes (zero for end of log) + * @param data log data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_data_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint16_t id, uint32_t ofs, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_DATA_LEN]; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint16_t(buf, 4, id); + _mav_put_uint8_t(buf, 6, count); + _mav_put_uint8_t_array(buf, 7, data, 90); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_DATA_LEN); +#else + mavlink_log_data_t packet; + packet.ofs = ofs; + packet.id = id; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*90); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOG_DATA_MIN_LEN, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOG_DATA_MIN_LEN, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +} + /** * @brief Pack a log_data message on a channel * @param system_id ID of this system @@ -107,7 +147,7 @@ static inline uint16_t mavlink_msg_log_data_pack_chan(uint8_t system_id, uint8_t packet.ofs = ofs; packet.id = id; packet.count = count; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*90); + mav_array_assign_uint8_t(packet.data, data, 90); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_DATA_LEN); #endif @@ -142,6 +182,20 @@ static inline uint16_t mavlink_msg_log_data_encode_chan(uint8_t system_id, uint8 return mavlink_msg_log_data_pack_chan(system_id, component_id, chan, msg, log_data->id, log_data->ofs, log_data->count, log_data->data); } +/** + * @brief Encode a log_data struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param log_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_data_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_log_data_t* log_data) +{ + return mavlink_msg_log_data_pack_status(system_id, component_id, _status, msg, log_data->id, log_data->ofs, log_data->count, log_data->data); +} + /** * @brief Send a log_data message * @param chan MAVLink channel to send the message @@ -167,7 +221,7 @@ static inline void mavlink_msg_log_data_send(mavlink_channel_t chan, uint16_t id packet.ofs = ofs; packet.id = id; packet.count = count; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*90); + mav_array_assign_uint8_t(packet.data, data, 90); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOG_DATA_MIN_LEN, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); #endif } @@ -188,7 +242,7 @@ static inline void mavlink_msg_log_data_send_struct(mavlink_channel_t chan, cons #if MAVLINK_MSG_ID_LOG_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -208,7 +262,7 @@ static inline void mavlink_msg_log_data_send_buf(mavlink_message_t *msgbuf, mavl packet->ofs = ofs; packet->id = id; packet->count = count; - mav_array_memcpy(packet->data, data, sizeof(uint8_t)*90); + mav_array_assign_uint8_t(packet->data, data, 90); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_DATA_MIN_LEN, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_log_entry.h b/lib/main/MAVLink/common/mavlink_msg_log_entry.h index 3020dc74e6f..c2d4502bc78 100755 --- a/lib/main/MAVLink/common/mavlink_msg_log_entry.h +++ b/lib/main/MAVLink/common/mavlink_msg_log_entry.h @@ -87,6 +87,51 @@ static inline uint16_t mavlink_msg_log_entry_pack(uint8_t system_id, uint8_t com return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ENTRY_MIN_LEN, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); } +/** + * @brief Pack a log_entry message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param id Log id + * @param num_logs Total number of logs + * @param last_log_num High log number + * @param time_utc [s] UTC timestamp of log since 1970, or 0 if not available + * @param size [bytes] Size of the log (may be approximate) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_entry_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint16_t id, uint16_t num_logs, uint16_t last_log_num, uint32_t time_utc, uint32_t size) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_ENTRY_LEN]; + _mav_put_uint32_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 4, size); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint16_t(buf, 10, num_logs); + _mav_put_uint16_t(buf, 12, last_log_num); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#else + mavlink_log_entry_t packet; + packet.time_utc = time_utc; + packet.size = size; + packet.id = id; + packet.num_logs = num_logs; + packet.last_log_num = last_log_num; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_ENTRY; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOG_ENTRY_MIN_LEN, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOG_ENTRY_MIN_LEN, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +} + /** * @brief Pack a log_entry message on a channel * @param system_id ID of this system @@ -155,6 +200,20 @@ static inline uint16_t mavlink_msg_log_entry_encode_chan(uint8_t system_id, uint return mavlink_msg_log_entry_pack_chan(system_id, component_id, chan, msg, log_entry->id, log_entry->num_logs, log_entry->last_log_num, log_entry->time_utc, log_entry->size); } +/** + * @brief Encode a log_entry struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param log_entry C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_entry_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_log_entry_t* log_entry) +{ + return mavlink_msg_log_entry_pack_status(system_id, component_id, _status, msg, log_entry->id, log_entry->num_logs, log_entry->last_log_num, log_entry->time_utc, log_entry->size); +} + /** * @brief Send a log_entry message * @param chan MAVLink channel to send the message @@ -206,7 +265,7 @@ static inline void mavlink_msg_log_entry_send_struct(mavlink_channel_t chan, con #if MAVLINK_MSG_ID_LOG_ENTRY_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_log_erase.h b/lib/main/MAVLink/common/mavlink_msg_log_erase.h index c3e4263e091..960709c5f3a 100755 --- a/lib/main/MAVLink/common/mavlink_msg_log_erase.h +++ b/lib/main/MAVLink/common/mavlink_msg_log_erase.h @@ -69,6 +69,42 @@ static inline uint16_t mavlink_msg_log_erase_pack(uint8_t system_id, uint8_t com return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_ERASE_MIN_LEN, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); } +/** + * @brief Pack a log_erase message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_erase_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_ERASE_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#else + mavlink_log_erase_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_ERASE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOG_ERASE_MIN_LEN, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOG_ERASE_MIN_LEN, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +} + /** * @brief Pack a log_erase message on a channel * @param system_id ID of this system @@ -128,6 +164,20 @@ static inline uint16_t mavlink_msg_log_erase_encode_chan(uint8_t system_id, uint return mavlink_msg_log_erase_pack_chan(system_id, component_id, chan, msg, log_erase->target_system, log_erase->target_component); } +/** + * @brief Encode a log_erase struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param log_erase C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_erase_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_log_erase_t* log_erase) +{ + return mavlink_msg_log_erase_pack_status(system_id, component_id, _status, msg, log_erase->target_system, log_erase->target_component); +} + /** * @brief Send a log_erase message * @param chan MAVLink channel to send the message @@ -170,7 +220,7 @@ static inline void mavlink_msg_log_erase_send_struct(mavlink_channel_t chan, con #if MAVLINK_MSG_ID_LOG_ERASE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_log_request_data.h b/lib/main/MAVLink/common/mavlink_msg_log_request_data.h index 521b8522cfa..31044e422fe 100755 --- a/lib/main/MAVLink/common/mavlink_msg_log_request_data.h +++ b/lib/main/MAVLink/common/mavlink_msg_log_request_data.h @@ -87,6 +87,51 @@ static inline uint16_t mavlink_msg_log_request_data_pack(uint8_t system_id, uint return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_DATA_MIN_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); } +/** + * @brief Pack a log_request_data message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param id Log id (from LOG_ENTRY reply) + * @param ofs Offset into the log + * @param count [bytes] Number of bytes + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_request_data_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t id, uint32_t ofs, uint32_t count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN]; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint32_t(buf, 4, count); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint8_t(buf, 10, target_system); + _mav_put_uint8_t(buf, 11, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#else + mavlink_log_request_data_t packet; + packet.ofs = ofs; + packet.count = count; + packet.id = id; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOG_REQUEST_DATA_MIN_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOG_REQUEST_DATA_MIN_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +} + /** * @brief Pack a log_request_data message on a channel * @param system_id ID of this system @@ -155,6 +200,20 @@ static inline uint16_t mavlink_msg_log_request_data_encode_chan(uint8_t system_i return mavlink_msg_log_request_data_pack_chan(system_id, component_id, chan, msg, log_request_data->target_system, log_request_data->target_component, log_request_data->id, log_request_data->ofs, log_request_data->count); } +/** + * @brief Encode a log_request_data struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param log_request_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_request_data_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_log_request_data_t* log_request_data) +{ + return mavlink_msg_log_request_data_pack_status(system_id, component_id, _status, msg, log_request_data->target_system, log_request_data->target_component, log_request_data->id, log_request_data->ofs, log_request_data->count); +} + /** * @brief Send a log_request_data message * @param chan MAVLink channel to send the message @@ -206,7 +265,7 @@ static inline void mavlink_msg_log_request_data_send_struct(mavlink_channel_t ch #if MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_log_request_end.h b/lib/main/MAVLink/common/mavlink_msg_log_request_end.h index a2a7fe361b4..b34c412abd3 100755 --- a/lib/main/MAVLink/common/mavlink_msg_log_request_end.h +++ b/lib/main/MAVLink/common/mavlink_msg_log_request_end.h @@ -69,6 +69,42 @@ static inline uint16_t mavlink_msg_log_request_end_pack(uint8_t system_id, uint8 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_END_MIN_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); } +/** + * @brief Pack a log_request_end message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_request_end_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_END_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#else + mavlink_log_request_end_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_END; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOG_REQUEST_END_MIN_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOG_REQUEST_END_MIN_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +} + /** * @brief Pack a log_request_end message on a channel * @param system_id ID of this system @@ -128,6 +164,20 @@ static inline uint16_t mavlink_msg_log_request_end_encode_chan(uint8_t system_id return mavlink_msg_log_request_end_pack_chan(system_id, component_id, chan, msg, log_request_end->target_system, log_request_end->target_component); } +/** + * @brief Encode a log_request_end struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param log_request_end C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_request_end_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_log_request_end_t* log_request_end) +{ + return mavlink_msg_log_request_end_pack_status(system_id, component_id, _status, msg, log_request_end->target_system, log_request_end->target_component); +} + /** * @brief Send a log_request_end message * @param chan MAVLink channel to send the message @@ -170,7 +220,7 @@ static inline void mavlink_msg_log_request_end_send_struct(mavlink_channel_t cha #if MAVLINK_MSG_ID_LOG_REQUEST_END_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_log_request_list.h b/lib/main/MAVLink/common/mavlink_msg_log_request_list.h index 4a6dfaa4eb1..0d9017d29ac 100755 --- a/lib/main/MAVLink/common/mavlink_msg_log_request_list.h +++ b/lib/main/MAVLink/common/mavlink_msg_log_request_list.h @@ -81,6 +81,48 @@ static inline uint16_t mavlink_msg_log_request_list_pack(uint8_t system_id, uint return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOG_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); } +/** + * @brief Pack a log_request_list message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param start First log id (0 for first available) + * @param end Last log id (0xffff for last available) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_log_request_list_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t start, uint16_t end) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN]; + _mav_put_uint16_t(buf, 0, start); + _mav_put_uint16_t(buf, 2, end); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#else + mavlink_log_request_list_t packet; + packet.start = start; + packet.end = end; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOG_REQUEST_LIST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOG_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOG_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +} + /** * @brief Pack a log_request_list message on a channel * @param system_id ID of this system @@ -146,6 +188,20 @@ static inline uint16_t mavlink_msg_log_request_list_encode_chan(uint8_t system_i return mavlink_msg_log_request_list_pack_chan(system_id, component_id, chan, msg, log_request_list->target_system, log_request_list->target_component, log_request_list->start, log_request_list->end); } +/** + * @brief Encode a log_request_list struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param log_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_log_request_list_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_log_request_list_t* log_request_list) +{ + return mavlink_msg_log_request_list_pack_status(system_id, component_id, _status, msg, log_request_list->target_system, log_request_list->target_component, log_request_list->start, log_request_list->end); +} + /** * @brief Send a log_request_list message * @param chan MAVLink channel to send the message @@ -194,7 +250,7 @@ static inline void mavlink_msg_log_request_list_send_struct(mavlink_channel_t ch #if MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_logging_ack.h b/lib/main/MAVLink/common/mavlink_msg_logging_ack.h index 56dab387de1..53d8ec2a7df 100644 --- a/lib/main/MAVLink/common/mavlink_msg_logging_ack.h +++ b/lib/main/MAVLink/common/mavlink_msg_logging_ack.h @@ -75,6 +75,45 @@ static inline uint16_t mavlink_msg_logging_ack_pack(uint8_t system_id, uint8_t c return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN, MAVLINK_MSG_ID_LOGGING_ACK_LEN, MAVLINK_MSG_ID_LOGGING_ACK_CRC); } +/** + * @brief Pack a logging_ack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (must match the one in LOGGING_DATA_ACKED) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_logging_ack_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t sequence) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_ACK_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOGGING_ACK_LEN); +#else + mavlink_logging_ack_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOGGING_ACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN, MAVLINK_MSG_ID_LOGGING_ACK_LEN, MAVLINK_MSG_ID_LOGGING_ACK_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN, MAVLINK_MSG_ID_LOGGING_ACK_LEN); +#endif +} + /** * @brief Pack a logging_ack message on a channel * @param system_id ID of this system @@ -137,6 +176,20 @@ static inline uint16_t mavlink_msg_logging_ack_encode_chan(uint8_t system_id, ui return mavlink_msg_logging_ack_pack_chan(system_id, component_id, chan, msg, logging_ack->target_system, logging_ack->target_component, logging_ack->sequence); } +/** + * @brief Encode a logging_ack struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param logging_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_logging_ack_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_logging_ack_t* logging_ack) +{ + return mavlink_msg_logging_ack_pack_status(system_id, component_id, _status, msg, logging_ack->target_system, logging_ack->target_component, logging_ack->sequence); +} + /** * @brief Send a logging_ack message * @param chan MAVLink channel to send the message @@ -182,7 +235,7 @@ static inline void mavlink_msg_logging_ack_send_struct(mavlink_channel_t chan, c #if MAVLINK_MSG_ID_LOGGING_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_logging_data.h b/lib/main/MAVLink/common/mavlink_msg_logging_data.h index c9bf0559ded..208df769718 100644 --- a/lib/main/MAVLink/common/mavlink_msg_logging_data.h +++ b/lib/main/MAVLink/common/mavlink_msg_logging_data.h @@ -9,7 +9,7 @@ typedef struct __mavlink_logging_data_t { uint8_t target_system; /*< system ID of the target*/ uint8_t target_component; /*< component ID of the target*/ uint8_t length; /*< [bytes] data length*/ - uint8_t first_message_offset; /*< [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists).*/ + uint8_t first_message_offset; /*< [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to UINT8_MAX if no start exists).*/ uint8_t data[249]; /*< logged data*/ } mavlink_logging_data_t; @@ -60,7 +60,7 @@ typedef struct __mavlink_logging_data_t { * @param target_component component ID of the target * @param sequence sequence number (can wrap) * @param length [bytes] data length - * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to UINT8_MAX if no start exists). * @param data logged data * @return length of the message in bytes (excluding serial stream start sign) */ @@ -83,7 +83,7 @@ static inline uint16_t mavlink_msg_logging_data_pack(uint8_t system_id, uint8_t packet.target_component = target_component; packet.length = length; packet.first_message_offset = first_message_offset; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + mav_array_assign_uint8_t(packet.data, data, 249); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_DATA_LEN); #endif @@ -91,6 +91,52 @@ static inline uint16_t mavlink_msg_logging_data_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN, MAVLINK_MSG_ID_LOGGING_DATA_CRC); } +/** + * @brief Pack a logging_data message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (can wrap) + * @param length [bytes] data length + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to UINT8_MAX if no start exists). + * @param data logged data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_logging_data_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message_offset, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_DATA_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, length); + _mav_put_uint8_t(buf, 5, first_message_offset); + _mav_put_uint8_t_array(buf, 6, data, 249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOGGING_DATA_LEN); +#else + mavlink_logging_data_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + packet.length = length; + packet.first_message_offset = first_message_offset; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOGGING_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN, MAVLINK_MSG_ID_LOGGING_DATA_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN); +#endif +} + /** * @brief Pack a logging_data message on a channel * @param system_id ID of this system @@ -101,7 +147,7 @@ static inline uint16_t mavlink_msg_logging_data_pack(uint8_t system_id, uint8_t * @param target_component component ID of the target * @param sequence sequence number (can wrap) * @param length [bytes] data length - * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to UINT8_MAX if no start exists). * @param data logged data * @return length of the message in bytes (excluding serial stream start sign) */ @@ -125,7 +171,7 @@ static inline uint16_t mavlink_msg_logging_data_pack_chan(uint8_t system_id, uin packet.target_component = target_component; packet.length = length; packet.first_message_offset = first_message_offset; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + mav_array_assign_uint8_t(packet.data, data, 249); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_DATA_LEN); #endif @@ -160,6 +206,20 @@ static inline uint16_t mavlink_msg_logging_data_encode_chan(uint8_t system_id, u return mavlink_msg_logging_data_pack_chan(system_id, component_id, chan, msg, logging_data->target_system, logging_data->target_component, logging_data->sequence, logging_data->length, logging_data->first_message_offset, logging_data->data); } +/** + * @brief Encode a logging_data struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param logging_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_logging_data_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_logging_data_t* logging_data) +{ + return mavlink_msg_logging_data_pack_status(system_id, component_id, _status, msg, logging_data->target_system, logging_data->target_component, logging_data->sequence, logging_data->length, logging_data->first_message_offset, logging_data->data); +} + /** * @brief Send a logging_data message * @param chan MAVLink channel to send the message @@ -168,7 +228,7 @@ static inline uint16_t mavlink_msg_logging_data_encode_chan(uint8_t system_id, u * @param target_component component ID of the target * @param sequence sequence number (can wrap) * @param length [bytes] data length - * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to UINT8_MAX if no start exists). * @param data logged data */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -191,7 +251,7 @@ static inline void mavlink_msg_logging_data_send(mavlink_channel_t chan, uint8_t packet.target_component = target_component; packet.length = length; packet.first_message_offset = first_message_offset; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + mav_array_assign_uint8_t(packet.data, data, 249); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN, MAVLINK_MSG_ID_LOGGING_DATA_CRC); #endif } @@ -212,7 +272,7 @@ static inline void mavlink_msg_logging_data_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_LOGGING_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -236,7 +296,7 @@ static inline void mavlink_msg_logging_data_send_buf(mavlink_message_t *msgbuf, packet->target_component = target_component; packet->length = length; packet->first_message_offset = first_message_offset; - mav_array_memcpy(packet->data, data, sizeof(uint8_t)*249); + mav_array_assign_uint8_t(packet->data, data, 249); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA, (const char *)packet, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN, MAVLINK_MSG_ID_LOGGING_DATA_CRC); #endif } @@ -290,7 +350,7 @@ static inline uint8_t mavlink_msg_logging_data_get_length(const mavlink_message_ /** * @brief Get field first_message_offset from logging_data message * - * @return [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @return [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to UINT8_MAX if no start exists). */ static inline uint8_t mavlink_msg_logging_data_get_first_message_offset(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_logging_data_acked.h b/lib/main/MAVLink/common/mavlink_msg_logging_data_acked.h index a179387c728..d560b570bed 100644 --- a/lib/main/MAVLink/common/mavlink_msg_logging_data_acked.h +++ b/lib/main/MAVLink/common/mavlink_msg_logging_data_acked.h @@ -9,7 +9,7 @@ typedef struct __mavlink_logging_data_acked_t { uint8_t target_system; /*< system ID of the target*/ uint8_t target_component; /*< component ID of the target*/ uint8_t length; /*< [bytes] data length*/ - uint8_t first_message_offset; /*< [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists).*/ + uint8_t first_message_offset; /*< [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to UINT8_MAX if no start exists).*/ uint8_t data[249]; /*< logged data*/ } mavlink_logging_data_acked_t; @@ -60,7 +60,7 @@ typedef struct __mavlink_logging_data_acked_t { * @param target_component component ID of the target * @param sequence sequence number (can wrap) * @param length [bytes] data length - * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to UINT8_MAX if no start exists). * @param data logged data * @return length of the message in bytes (excluding serial stream start sign) */ @@ -83,7 +83,7 @@ static inline uint16_t mavlink_msg_logging_data_acked_pack(uint8_t system_id, ui packet.target_component = target_component; packet.length = length; packet.first_message_offset = first_message_offset; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + mav_array_assign_uint8_t(packet.data, data, 249); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN); #endif @@ -91,6 +91,52 @@ static inline uint16_t mavlink_msg_logging_data_acked_pack(uint8_t system_id, ui return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC); } +/** + * @brief Pack a logging_data_acked message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (can wrap) + * @param length [bytes] data length + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to UINT8_MAX if no start exists). + * @param data logged data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_logging_data_acked_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message_offset, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, length); + _mav_put_uint8_t(buf, 5, first_message_offset); + _mav_put_uint8_t_array(buf, 6, data, 249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN); +#else + mavlink_logging_data_acked_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + packet.length = length; + packet.first_message_offset = first_message_offset; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOGGING_DATA_ACKED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN); +#endif +} + /** * @brief Pack a logging_data_acked message on a channel * @param system_id ID of this system @@ -101,7 +147,7 @@ static inline uint16_t mavlink_msg_logging_data_acked_pack(uint8_t system_id, ui * @param target_component component ID of the target * @param sequence sequence number (can wrap) * @param length [bytes] data length - * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to UINT8_MAX if no start exists). * @param data logged data * @return length of the message in bytes (excluding serial stream start sign) */ @@ -125,7 +171,7 @@ static inline uint16_t mavlink_msg_logging_data_acked_pack_chan(uint8_t system_i packet.target_component = target_component; packet.length = length; packet.first_message_offset = first_message_offset; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + mav_array_assign_uint8_t(packet.data, data, 249); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN); #endif @@ -160,6 +206,20 @@ static inline uint16_t mavlink_msg_logging_data_acked_encode_chan(uint8_t system return mavlink_msg_logging_data_acked_pack_chan(system_id, component_id, chan, msg, logging_data_acked->target_system, logging_data_acked->target_component, logging_data_acked->sequence, logging_data_acked->length, logging_data_acked->first_message_offset, logging_data_acked->data); } +/** + * @brief Encode a logging_data_acked struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param logging_data_acked C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_logging_data_acked_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_logging_data_acked_t* logging_data_acked) +{ + return mavlink_msg_logging_data_acked_pack_status(system_id, component_id, _status, msg, logging_data_acked->target_system, logging_data_acked->target_component, logging_data_acked->sequence, logging_data_acked->length, logging_data_acked->first_message_offset, logging_data_acked->data); +} + /** * @brief Send a logging_data_acked message * @param chan MAVLink channel to send the message @@ -168,7 +228,7 @@ static inline uint16_t mavlink_msg_logging_data_acked_encode_chan(uint8_t system * @param target_component component ID of the target * @param sequence sequence number (can wrap) * @param length [bytes] data length - * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to UINT8_MAX if no start exists). * @param data logged data */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -191,7 +251,7 @@ static inline void mavlink_msg_logging_data_acked_send(mavlink_channel_t chan, u packet.target_component = target_component; packet.length = length; packet.first_message_offset = first_message_offset; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + mav_array_assign_uint8_t(packet.data, data, 249); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA_ACKED, (const char *)&packet, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC); #endif } @@ -212,7 +272,7 @@ static inline void mavlink_msg_logging_data_acked_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -236,7 +296,7 @@ static inline void mavlink_msg_logging_data_acked_send_buf(mavlink_message_t *ms packet->target_component = target_component; packet->length = length; packet->first_message_offset = first_message_offset; - mav_array_memcpy(packet->data, data, sizeof(uint8_t)*249); + mav_array_assign_uint8_t(packet->data, data, 249); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA_ACKED, (const char *)packet, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC); #endif } @@ -290,7 +350,7 @@ static inline uint8_t mavlink_msg_logging_data_acked_get_length(const mavlink_me /** * @brief Get field first_message_offset from logging_data_acked message * - * @return [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @return [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to UINT8_MAX if no start exists). */ static inline uint8_t mavlink_msg_logging_data_acked_get_first_message_offset(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_mag_cal_report.h b/lib/main/MAVLink/common/mavlink_msg_mag_cal_report.h index 6694501e17f..4a2b1982cfc 100644 --- a/lib/main/MAVLink/common/mavlink_msg_mag_cal_report.h +++ b/lib/main/MAVLink/common/mavlink_msg_mag_cal_report.h @@ -165,6 +165,90 @@ static inline uint16_t mavlink_msg_mag_cal_report_pack(uint8_t system_id, uint8_ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); } +/** + * @brief Pack a mag_cal_report message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param compass_id Compass being calibrated. + * @param cal_mask Bitmask of compasses being calibrated. + * @param cal_status Calibration Status. + * @param autosaved 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters. + * @param fitness [mgauss] RMS milligauss residuals. + * @param ofs_x X offset. + * @param ofs_y Y offset. + * @param ofs_z Z offset. + * @param diag_x X diagonal (matrix 11). + * @param diag_y Y diagonal (matrix 22). + * @param diag_z Z diagonal (matrix 33). + * @param offdiag_x X off-diagonal (matrix 12 and 21). + * @param offdiag_y Y off-diagonal (matrix 13 and 31). + * @param offdiag_z Z off-diagonal (matrix 32 and 23). + * @param orientation_confidence Confidence in orientation (higher is better). + * @param old_orientation orientation before calibration. + * @param new_orientation orientation after calibration. + * @param scale_factor field radius correction factor + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_report_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z, float orientation_confidence, uint8_t old_orientation, uint8_t new_orientation, float scale_factor) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN]; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + _mav_put_float(buf, 44, orientation_confidence); + _mav_put_uint8_t(buf, 48, old_orientation); + _mav_put_uint8_t(buf, 49, new_orientation); + _mav_put_float(buf, 50, scale_factor); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#else + mavlink_mag_cal_report_t packet; + packet.fitness = fitness; + packet.ofs_x = ofs_x; + packet.ofs_y = ofs_y; + packet.ofs_z = ofs_z; + packet.diag_x = diag_x; + packet.diag_y = diag_y; + packet.diag_z = diag_z; + packet.offdiag_x = offdiag_x; + packet.offdiag_y = offdiag_y; + packet.offdiag_z = offdiag_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.autosaved = autosaved; + packet.orientation_confidence = orientation_confidence; + packet.old_orientation = old_orientation; + packet.new_orientation = new_orientation; + packet.scale_factor = scale_factor; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_REPORT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif +} + /** * @brief Pack a mag_cal_report message on a channel * @param system_id ID of this system @@ -272,6 +356,20 @@ static inline uint16_t mavlink_msg_mag_cal_report_encode_chan(uint8_t system_id, return mavlink_msg_mag_cal_report_pack_chan(system_id, component_id, chan, msg, mag_cal_report->compass_id, mag_cal_report->cal_mask, mag_cal_report->cal_status, mag_cal_report->autosaved, mag_cal_report->fitness, mag_cal_report->ofs_x, mag_cal_report->ofs_y, mag_cal_report->ofs_z, mag_cal_report->diag_x, mag_cal_report->diag_y, mag_cal_report->diag_z, mag_cal_report->offdiag_x, mag_cal_report->offdiag_y, mag_cal_report->offdiag_z, mag_cal_report->orientation_confidence, mag_cal_report->old_orientation, mag_cal_report->new_orientation, mag_cal_report->scale_factor); } +/** + * @brief Encode a mag_cal_report struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param mag_cal_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_report_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_mag_cal_report_t* mag_cal_report) +{ + return mavlink_msg_mag_cal_report_pack_status(system_id, component_id, _status, msg, mag_cal_report->compass_id, mag_cal_report->cal_mask, mag_cal_report->cal_status, mag_cal_report->autosaved, mag_cal_report->fitness, mag_cal_report->ofs_x, mag_cal_report->ofs_y, mag_cal_report->ofs_z, mag_cal_report->diag_x, mag_cal_report->diag_y, mag_cal_report->diag_z, mag_cal_report->offdiag_x, mag_cal_report->offdiag_y, mag_cal_report->offdiag_z, mag_cal_report->orientation_confidence, mag_cal_report->old_orientation, mag_cal_report->new_orientation, mag_cal_report->scale_factor); +} + /** * @brief Send a mag_cal_report message * @param chan MAVLink channel to send the message @@ -362,7 +460,7 @@ static inline void mavlink_msg_mag_cal_report_send_struct(mavlink_channel_t chan #if MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_manual_control.h b/lib/main/MAVLink/common/mavlink_msg_manual_control.h index 14e7603a43a..69c1139d01a 100755 --- a/lib/main/MAVLink/common/mavlink_msg_manual_control.h +++ b/lib/main/MAVLink/common/mavlink_msg_manual_control.h @@ -3,19 +3,29 @@ #define MAVLINK_MSG_ID_MANUAL_CONTROL 69 - +MAVPACKED( typedef struct __mavlink_manual_control_t { int16_t x; /*< X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle.*/ int16_t y; /*< Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle.*/ int16_t z; /*< Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. Positive values are positive thrust, negative values are negative thrust.*/ int16_t r; /*< R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle.*/ - uint16_t buttons; /*< A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1.*/ + uint16_t buttons; /*< A bitfield corresponding to the joystick buttons' 0-15 current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1.*/ uint8_t target; /*< The system to be controlled.*/ -} mavlink_manual_control_t; - -#define MAVLINK_MSG_ID_MANUAL_CONTROL_LEN 11 + uint16_t buttons2; /*< A bitfield corresponding to the joystick buttons' 16-31 current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 16.*/ + uint8_t enabled_extensions; /*< Set bits to 1 to indicate which of the following extension fields contain valid data: bit 0: pitch, bit 1: roll, bit 2: aux1, bit 3: aux2, bit 4: aux3, bit 5: aux4, bit 6: aux5, bit 7: aux6*/ + int16_t s; /*< Pitch-only-axis, normalized to the range [-1000,1000]. Generally corresponds to pitch on vehicles with additional degrees of freedom. Valid if bit 0 of enabled_extensions field is set. Set to 0 if invalid.*/ + int16_t t; /*< Roll-only-axis, normalized to the range [-1000,1000]. Generally corresponds to roll on vehicles with additional degrees of freedom. Valid if bit 1 of enabled_extensions field is set. Set to 0 if invalid.*/ + int16_t aux1; /*< Aux continuous input field 1. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 2 of enabled_extensions field is set. 0 if bit 2 is unset.*/ + int16_t aux2; /*< Aux continuous input field 2. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 3 of enabled_extensions field is set. 0 if bit 3 is unset.*/ + int16_t aux3; /*< Aux continuous input field 3. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 4 of enabled_extensions field is set. 0 if bit 4 is unset.*/ + int16_t aux4; /*< Aux continuous input field 4. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 5 of enabled_extensions field is set. 0 if bit 5 is unset.*/ + int16_t aux5; /*< Aux continuous input field 5. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 6 of enabled_extensions field is set. 0 if bit 6 is unset.*/ + int16_t aux6; /*< Aux continuous input field 6. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 7 of enabled_extensions field is set. 0 if bit 7 is unset.*/ +}) mavlink_manual_control_t; + +#define MAVLINK_MSG_ID_MANUAL_CONTROL_LEN 30 #define MAVLINK_MSG_ID_MANUAL_CONTROL_MIN_LEN 11 -#define MAVLINK_MSG_ID_69_LEN 11 +#define MAVLINK_MSG_ID_69_LEN 30 #define MAVLINK_MSG_ID_69_MIN_LEN 11 #define MAVLINK_MSG_ID_MANUAL_CONTROL_CRC 243 @@ -27,25 +37,45 @@ typedef struct __mavlink_manual_control_t { #define MAVLINK_MESSAGE_INFO_MANUAL_CONTROL { \ 69, \ "MANUAL_CONTROL", \ - 6, \ + 16, \ { { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_manual_control_t, target) }, \ { "x", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_manual_control_t, x) }, \ { "y", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_manual_control_t, y) }, \ { "z", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_manual_control_t, z) }, \ { "r", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_manual_control_t, r) }, \ { "buttons", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_manual_control_t, buttons) }, \ + { "buttons2", NULL, MAVLINK_TYPE_UINT16_T, 0, 11, offsetof(mavlink_manual_control_t, buttons2) }, \ + { "enabled_extensions", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_manual_control_t, enabled_extensions) }, \ + { "s", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_manual_control_t, s) }, \ + { "t", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_manual_control_t, t) }, \ + { "aux1", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_manual_control_t, aux1) }, \ + { "aux2", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_manual_control_t, aux2) }, \ + { "aux3", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_manual_control_t, aux3) }, \ + { "aux4", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_manual_control_t, aux4) }, \ + { "aux5", NULL, MAVLINK_TYPE_INT16_T, 0, 26, offsetof(mavlink_manual_control_t, aux5) }, \ + { "aux6", NULL, MAVLINK_TYPE_INT16_T, 0, 28, offsetof(mavlink_manual_control_t, aux6) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MANUAL_CONTROL { \ "MANUAL_CONTROL", \ - 6, \ + 16, \ { { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_manual_control_t, target) }, \ { "x", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_manual_control_t, x) }, \ { "y", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_manual_control_t, y) }, \ { "z", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_manual_control_t, z) }, \ { "r", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_manual_control_t, r) }, \ { "buttons", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_manual_control_t, buttons) }, \ + { "buttons2", NULL, MAVLINK_TYPE_UINT16_T, 0, 11, offsetof(mavlink_manual_control_t, buttons2) }, \ + { "enabled_extensions", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_manual_control_t, enabled_extensions) }, \ + { "s", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_manual_control_t, s) }, \ + { "t", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_manual_control_t, t) }, \ + { "aux1", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_manual_control_t, aux1) }, \ + { "aux2", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_manual_control_t, aux2) }, \ + { "aux3", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_manual_control_t, aux3) }, \ + { "aux4", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_manual_control_t, aux4) }, \ + { "aux5", NULL, MAVLINK_TYPE_INT16_T, 0, 26, offsetof(mavlink_manual_control_t, aux5) }, \ + { "aux6", NULL, MAVLINK_TYPE_INT16_T, 0, 28, offsetof(mavlink_manual_control_t, aux6) }, \ } \ } #endif @@ -61,11 +91,21 @@ typedef struct __mavlink_manual_control_t { * @param y Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle. * @param z Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. Positive values are positive thrust, negative values are negative thrust. * @param r R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. - * @param buttons A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. + * @param buttons A bitfield corresponding to the joystick buttons' 0-15 current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. + * @param buttons2 A bitfield corresponding to the joystick buttons' 16-31 current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 16. + * @param enabled_extensions Set bits to 1 to indicate which of the following extension fields contain valid data: bit 0: pitch, bit 1: roll, bit 2: aux1, bit 3: aux2, bit 4: aux3, bit 5: aux4, bit 6: aux5, bit 7: aux6 + * @param s Pitch-only-axis, normalized to the range [-1000,1000]. Generally corresponds to pitch on vehicles with additional degrees of freedom. Valid if bit 0 of enabled_extensions field is set. Set to 0 if invalid. + * @param t Roll-only-axis, normalized to the range [-1000,1000]. Generally corresponds to roll on vehicles with additional degrees of freedom. Valid if bit 1 of enabled_extensions field is set. Set to 0 if invalid. + * @param aux1 Aux continuous input field 1. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 2 of enabled_extensions field is set. 0 if bit 2 is unset. + * @param aux2 Aux continuous input field 2. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 3 of enabled_extensions field is set. 0 if bit 3 is unset. + * @param aux3 Aux continuous input field 3. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 4 of enabled_extensions field is set. 0 if bit 4 is unset. + * @param aux4 Aux continuous input field 4. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 5 of enabled_extensions field is set. 0 if bit 5 is unset. + * @param aux5 Aux continuous input field 5. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 6 of enabled_extensions field is set. 0 if bit 6 is unset. + * @param aux6 Aux continuous input field 6. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 7 of enabled_extensions field is set. 0 if bit 7 is unset. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target, int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) + uint8_t target, int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons, uint16_t buttons2, uint8_t enabled_extensions, int16_t s, int16_t t, int16_t aux1, int16_t aux2, int16_t aux3, int16_t aux4, int16_t aux5, int16_t aux6) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MANUAL_CONTROL_LEN]; @@ -75,6 +115,16 @@ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_ _mav_put_int16_t(buf, 6, r); _mav_put_uint16_t(buf, 8, buttons); _mav_put_uint8_t(buf, 10, target); + _mav_put_uint16_t(buf, 11, buttons2); + _mav_put_uint8_t(buf, 13, enabled_extensions); + _mav_put_int16_t(buf, 14, s); + _mav_put_int16_t(buf, 16, t); + _mav_put_int16_t(buf, 18, aux1); + _mav_put_int16_t(buf, 20, aux2); + _mav_put_int16_t(buf, 22, aux3); + _mav_put_int16_t(buf, 24, aux4); + _mav_put_int16_t(buf, 26, aux5); + _mav_put_int16_t(buf, 28, aux6); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); #else @@ -85,6 +135,16 @@ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_ packet.r = r; packet.buttons = buttons; packet.target = target; + packet.buttons2 = buttons2; + packet.enabled_extensions = enabled_extensions; + packet.s = s; + packet.t = t; + packet.aux1 = aux1; + packet.aux2 = aux2; + packet.aux3 = aux3; + packet.aux4 = aux4; + packet.aux5 = aux5; + packet.aux6 = aux6; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); #endif @@ -93,6 +153,84 @@ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); } +/** + * @brief Pack a manual_control message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target The system to be controlled. + * @param x X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. + * @param y Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle. + * @param z Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. Positive values are positive thrust, negative values are negative thrust. + * @param r R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. + * @param buttons A bitfield corresponding to the joystick buttons' 0-15 current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. + * @param buttons2 A bitfield corresponding to the joystick buttons' 16-31 current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 16. + * @param enabled_extensions Set bits to 1 to indicate which of the following extension fields contain valid data: bit 0: pitch, bit 1: roll, bit 2: aux1, bit 3: aux2, bit 4: aux3, bit 5: aux4, bit 6: aux5, bit 7: aux6 + * @param s Pitch-only-axis, normalized to the range [-1000,1000]. Generally corresponds to pitch on vehicles with additional degrees of freedom. Valid if bit 0 of enabled_extensions field is set. Set to 0 if invalid. + * @param t Roll-only-axis, normalized to the range [-1000,1000]. Generally corresponds to roll on vehicles with additional degrees of freedom. Valid if bit 1 of enabled_extensions field is set. Set to 0 if invalid. + * @param aux1 Aux continuous input field 1. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 2 of enabled_extensions field is set. 0 if bit 2 is unset. + * @param aux2 Aux continuous input field 2. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 3 of enabled_extensions field is set. 0 if bit 3 is unset. + * @param aux3 Aux continuous input field 3. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 4 of enabled_extensions field is set. 0 if bit 4 is unset. + * @param aux4 Aux continuous input field 4. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 5 of enabled_extensions field is set. 0 if bit 5 is unset. + * @param aux5 Aux continuous input field 5. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 6 of enabled_extensions field is set. 0 if bit 6 is unset. + * @param aux6 Aux continuous input field 6. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 7 of enabled_extensions field is set. 0 if bit 7 is unset. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_manual_control_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target, int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons, uint16_t buttons2, uint8_t enabled_extensions, int16_t s, int16_t t, int16_t aux1, int16_t aux2, int16_t aux3, int16_t aux4, int16_t aux5, int16_t aux6) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MANUAL_CONTROL_LEN]; + _mav_put_int16_t(buf, 0, x); + _mav_put_int16_t(buf, 2, y); + _mav_put_int16_t(buf, 4, z); + _mav_put_int16_t(buf, 6, r); + _mav_put_uint16_t(buf, 8, buttons); + _mav_put_uint8_t(buf, 10, target); + _mav_put_uint16_t(buf, 11, buttons2); + _mav_put_uint8_t(buf, 13, enabled_extensions); + _mav_put_int16_t(buf, 14, s); + _mav_put_int16_t(buf, 16, t); + _mav_put_int16_t(buf, 18, aux1); + _mav_put_int16_t(buf, 20, aux2); + _mav_put_int16_t(buf, 22, aux3); + _mav_put_int16_t(buf, 24, aux4); + _mav_put_int16_t(buf, 26, aux5); + _mav_put_int16_t(buf, 28, aux6); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#else + mavlink_manual_control_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.r = r; + packet.buttons = buttons; + packet.target = target; + packet.buttons2 = buttons2; + packet.enabled_extensions = enabled_extensions; + packet.s = s; + packet.t = t; + packet.aux1 = aux1; + packet.aux2 = aux2; + packet.aux3 = aux3; + packet.aux4 = aux4; + packet.aux5 = aux5; + packet.aux6 = aux6; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#endif +} + /** * @brief Pack a manual_control message on a channel * @param system_id ID of this system @@ -104,12 +242,22 @@ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_ * @param y Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle. * @param z Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. Positive values are positive thrust, negative values are negative thrust. * @param r R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. - * @param buttons A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. + * @param buttons A bitfield corresponding to the joystick buttons' 0-15 current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. + * @param buttons2 A bitfield corresponding to the joystick buttons' 16-31 current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 16. + * @param enabled_extensions Set bits to 1 to indicate which of the following extension fields contain valid data: bit 0: pitch, bit 1: roll, bit 2: aux1, bit 3: aux2, bit 4: aux3, bit 5: aux4, bit 6: aux5, bit 7: aux6 + * @param s Pitch-only-axis, normalized to the range [-1000,1000]. Generally corresponds to pitch on vehicles with additional degrees of freedom. Valid if bit 0 of enabled_extensions field is set. Set to 0 if invalid. + * @param t Roll-only-axis, normalized to the range [-1000,1000]. Generally corresponds to roll on vehicles with additional degrees of freedom. Valid if bit 1 of enabled_extensions field is set. Set to 0 if invalid. + * @param aux1 Aux continuous input field 1. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 2 of enabled_extensions field is set. 0 if bit 2 is unset. + * @param aux2 Aux continuous input field 2. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 3 of enabled_extensions field is set. 0 if bit 3 is unset. + * @param aux3 Aux continuous input field 3. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 4 of enabled_extensions field is set. 0 if bit 4 is unset. + * @param aux4 Aux continuous input field 4. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 5 of enabled_extensions field is set. 0 if bit 5 is unset. + * @param aux5 Aux continuous input field 5. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 6 of enabled_extensions field is set. 0 if bit 6 is unset. + * @param aux6 Aux continuous input field 6. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 7 of enabled_extensions field is set. 0 if bit 7 is unset. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target,int16_t x,int16_t y,int16_t z,int16_t r,uint16_t buttons) + uint8_t target,int16_t x,int16_t y,int16_t z,int16_t r,uint16_t buttons,uint16_t buttons2,uint8_t enabled_extensions,int16_t s,int16_t t,int16_t aux1,int16_t aux2,int16_t aux3,int16_t aux4,int16_t aux5,int16_t aux6) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MANUAL_CONTROL_LEN]; @@ -119,6 +267,16 @@ static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, u _mav_put_int16_t(buf, 6, r); _mav_put_uint16_t(buf, 8, buttons); _mav_put_uint8_t(buf, 10, target); + _mav_put_uint16_t(buf, 11, buttons2); + _mav_put_uint8_t(buf, 13, enabled_extensions); + _mav_put_int16_t(buf, 14, s); + _mav_put_int16_t(buf, 16, t); + _mav_put_int16_t(buf, 18, aux1); + _mav_put_int16_t(buf, 20, aux2); + _mav_put_int16_t(buf, 22, aux3); + _mav_put_int16_t(buf, 24, aux4); + _mav_put_int16_t(buf, 26, aux5); + _mav_put_int16_t(buf, 28, aux6); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); #else @@ -129,6 +287,16 @@ static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, u packet.r = r; packet.buttons = buttons; packet.target = target; + packet.buttons2 = buttons2; + packet.enabled_extensions = enabled_extensions; + packet.s = s; + packet.t = t; + packet.aux1 = aux1; + packet.aux2 = aux2; + packet.aux3 = aux3; + packet.aux4 = aux4; + packet.aux5 = aux5; + packet.aux6 = aux6; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); #endif @@ -147,7 +315,7 @@ static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, u */ static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_manual_control_t* manual_control) { - return mavlink_msg_manual_control_pack(system_id, component_id, msg, manual_control->target, manual_control->x, manual_control->y, manual_control->z, manual_control->r, manual_control->buttons); + return mavlink_msg_manual_control_pack(system_id, component_id, msg, manual_control->target, manual_control->x, manual_control->y, manual_control->z, manual_control->r, manual_control->buttons, manual_control->buttons2, manual_control->enabled_extensions, manual_control->s, manual_control->t, manual_control->aux1, manual_control->aux2, manual_control->aux3, manual_control->aux4, manual_control->aux5, manual_control->aux6); } /** @@ -161,7 +329,21 @@ static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_manual_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_manual_control_t* manual_control) { - return mavlink_msg_manual_control_pack_chan(system_id, component_id, chan, msg, manual_control->target, manual_control->x, manual_control->y, manual_control->z, manual_control->r, manual_control->buttons); + return mavlink_msg_manual_control_pack_chan(system_id, component_id, chan, msg, manual_control->target, manual_control->x, manual_control->y, manual_control->z, manual_control->r, manual_control->buttons, manual_control->buttons2, manual_control->enabled_extensions, manual_control->s, manual_control->t, manual_control->aux1, manual_control->aux2, manual_control->aux3, manual_control->aux4, manual_control->aux5, manual_control->aux6); +} + +/** + * @brief Encode a manual_control struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param manual_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_manual_control_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_manual_control_t* manual_control) +{ + return mavlink_msg_manual_control_pack_status(system_id, component_id, _status, msg, manual_control->target, manual_control->x, manual_control->y, manual_control->z, manual_control->r, manual_control->buttons, manual_control->buttons2, manual_control->enabled_extensions, manual_control->s, manual_control->t, manual_control->aux1, manual_control->aux2, manual_control->aux3, manual_control->aux4, manual_control->aux5, manual_control->aux6); } /** @@ -173,11 +355,21 @@ static inline uint16_t mavlink_msg_manual_control_encode_chan(uint8_t system_id, * @param y Y-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle. * @param z Z-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle. Positive values are positive thrust, negative values are negative thrust. * @param r R-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle. - * @param buttons A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. + * @param buttons A bitfield corresponding to the joystick buttons' 0-15 current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. + * @param buttons2 A bitfield corresponding to the joystick buttons' 16-31 current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 16. + * @param enabled_extensions Set bits to 1 to indicate which of the following extension fields contain valid data: bit 0: pitch, bit 1: roll, bit 2: aux1, bit 3: aux2, bit 4: aux3, bit 5: aux4, bit 6: aux5, bit 7: aux6 + * @param s Pitch-only-axis, normalized to the range [-1000,1000]. Generally corresponds to pitch on vehicles with additional degrees of freedom. Valid if bit 0 of enabled_extensions field is set. Set to 0 if invalid. + * @param t Roll-only-axis, normalized to the range [-1000,1000]. Generally corresponds to roll on vehicles with additional degrees of freedom. Valid if bit 1 of enabled_extensions field is set. Set to 0 if invalid. + * @param aux1 Aux continuous input field 1. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 2 of enabled_extensions field is set. 0 if bit 2 is unset. + * @param aux2 Aux continuous input field 2. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 3 of enabled_extensions field is set. 0 if bit 3 is unset. + * @param aux3 Aux continuous input field 3. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 4 of enabled_extensions field is set. 0 if bit 4 is unset. + * @param aux4 Aux continuous input field 4. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 5 of enabled_extensions field is set. 0 if bit 5 is unset. + * @param aux5 Aux continuous input field 5. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 6 of enabled_extensions field is set. 0 if bit 6 is unset. + * @param aux6 Aux continuous input field 6. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 7 of enabled_extensions field is set. 0 if bit 7 is unset. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8_t target, int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) +static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8_t target, int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons, uint16_t buttons2, uint8_t enabled_extensions, int16_t s, int16_t t, int16_t aux1, int16_t aux2, int16_t aux3, int16_t aux4, int16_t aux5, int16_t aux6) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MANUAL_CONTROL_LEN]; @@ -187,6 +379,16 @@ static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8 _mav_put_int16_t(buf, 6, r); _mav_put_uint16_t(buf, 8, buttons); _mav_put_uint8_t(buf, 10, target); + _mav_put_uint16_t(buf, 11, buttons2); + _mav_put_uint8_t(buf, 13, enabled_extensions); + _mav_put_int16_t(buf, 14, s); + _mav_put_int16_t(buf, 16, t); + _mav_put_int16_t(buf, 18, aux1); + _mav_put_int16_t(buf, 20, aux2); + _mav_put_int16_t(buf, 22, aux3); + _mav_put_int16_t(buf, 24, aux4); + _mav_put_int16_t(buf, 26, aux5); + _mav_put_int16_t(buf, 28, aux6); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); #else @@ -197,6 +399,16 @@ static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8 packet.r = r; packet.buttons = buttons; packet.target = target; + packet.buttons2 = buttons2; + packet.enabled_extensions = enabled_extensions; + packet.s = s; + packet.t = t; + packet.aux1 = aux1; + packet.aux2 = aux2; + packet.aux3 = aux3; + packet.aux4 = aux4; + packet.aux5 = aux5; + packet.aux6 = aux6; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); #endif @@ -210,7 +422,7 @@ static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8 static inline void mavlink_msg_manual_control_send_struct(mavlink_channel_t chan, const mavlink_manual_control_t* manual_control) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_manual_control_send(chan, manual_control->target, manual_control->x, manual_control->y, manual_control->z, manual_control->r, manual_control->buttons); + mavlink_msg_manual_control_send(chan, manual_control->target, manual_control->x, manual_control->y, manual_control->z, manual_control->r, manual_control->buttons, manual_control->buttons2, manual_control->enabled_extensions, manual_control->s, manual_control->t, manual_control->aux1, manual_control->aux2, manual_control->aux3, manual_control->aux4, manual_control->aux5, manual_control->aux6); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)manual_control, MAVLINK_MSG_ID_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); #endif @@ -218,13 +430,13 @@ static inline void mavlink_msg_manual_control_send_struct(mavlink_channel_t chan #if MAVLINK_MSG_ID_MANUAL_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_manual_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target, int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) +static inline void mavlink_msg_manual_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target, int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons, uint16_t buttons2, uint8_t enabled_extensions, int16_t s, int16_t t, int16_t aux1, int16_t aux2, int16_t aux3, int16_t aux4, int16_t aux5, int16_t aux6) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -234,6 +446,16 @@ static inline void mavlink_msg_manual_control_send_buf(mavlink_message_t *msgbuf _mav_put_int16_t(buf, 6, r); _mav_put_uint16_t(buf, 8, buttons); _mav_put_uint8_t(buf, 10, target); + _mav_put_uint16_t(buf, 11, buttons2); + _mav_put_uint8_t(buf, 13, enabled_extensions); + _mav_put_int16_t(buf, 14, s); + _mav_put_int16_t(buf, 16, t); + _mav_put_int16_t(buf, 18, aux1); + _mav_put_int16_t(buf, 20, aux2); + _mav_put_int16_t(buf, 22, aux3); + _mav_put_int16_t(buf, 24, aux4); + _mav_put_int16_t(buf, 26, aux5); + _mav_put_int16_t(buf, 28, aux6); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); #else @@ -244,6 +466,16 @@ static inline void mavlink_msg_manual_control_send_buf(mavlink_message_t *msgbuf packet->r = r; packet->buttons = buttons; packet->target = target; + packet->buttons2 = buttons2; + packet->enabled_extensions = enabled_extensions; + packet->s = s; + packet->t = t; + packet->aux1 = aux1; + packet->aux2 = aux2; + packet->aux3 = aux3; + packet->aux4 = aux4; + packet->aux5 = aux5; + packet->aux6 = aux6; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); #endif @@ -308,13 +540,113 @@ static inline int16_t mavlink_msg_manual_control_get_r(const mavlink_message_t* /** * @brief Get field buttons from manual_control message * - * @return A bitfield corresponding to the joystick buttons' current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. + * @return A bitfield corresponding to the joystick buttons' 0-15 current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 1. */ static inline uint16_t mavlink_msg_manual_control_get_buttons(const mavlink_message_t* msg) { return _MAV_RETURN_uint16_t(msg, 8); } +/** + * @brief Get field buttons2 from manual_control message + * + * @return A bitfield corresponding to the joystick buttons' 16-31 current state, 1 for pressed, 0 for released. The lowest bit corresponds to Button 16. + */ +static inline uint16_t mavlink_msg_manual_control_get_buttons2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 11); +} + +/** + * @brief Get field enabled_extensions from manual_control message + * + * @return Set bits to 1 to indicate which of the following extension fields contain valid data: bit 0: pitch, bit 1: roll, bit 2: aux1, bit 3: aux2, bit 4: aux3, bit 5: aux4, bit 6: aux5, bit 7: aux6 + */ +static inline uint8_t mavlink_msg_manual_control_get_enabled_extensions(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 13); +} + +/** + * @brief Get field s from manual_control message + * + * @return Pitch-only-axis, normalized to the range [-1000,1000]. Generally corresponds to pitch on vehicles with additional degrees of freedom. Valid if bit 0 of enabled_extensions field is set. Set to 0 if invalid. + */ +static inline int16_t mavlink_msg_manual_control_get_s(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + +/** + * @brief Get field t from manual_control message + * + * @return Roll-only-axis, normalized to the range [-1000,1000]. Generally corresponds to roll on vehicles with additional degrees of freedom. Valid if bit 1 of enabled_extensions field is set. Set to 0 if invalid. + */ +static inline int16_t mavlink_msg_manual_control_get_t(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 16); +} + +/** + * @brief Get field aux1 from manual_control message + * + * @return Aux continuous input field 1. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 2 of enabled_extensions field is set. 0 if bit 2 is unset. + */ +static inline int16_t mavlink_msg_manual_control_get_aux1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 18); +} + +/** + * @brief Get field aux2 from manual_control message + * + * @return Aux continuous input field 2. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 3 of enabled_extensions field is set. 0 if bit 3 is unset. + */ +static inline int16_t mavlink_msg_manual_control_get_aux2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 20); +} + +/** + * @brief Get field aux3 from manual_control message + * + * @return Aux continuous input field 3. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 4 of enabled_extensions field is set. 0 if bit 4 is unset. + */ +static inline int16_t mavlink_msg_manual_control_get_aux3(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 22); +} + +/** + * @brief Get field aux4 from manual_control message + * + * @return Aux continuous input field 4. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 5 of enabled_extensions field is set. 0 if bit 5 is unset. + */ +static inline int16_t mavlink_msg_manual_control_get_aux4(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 24); +} + +/** + * @brief Get field aux5 from manual_control message + * + * @return Aux continuous input field 5. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 6 of enabled_extensions field is set. 0 if bit 6 is unset. + */ +static inline int16_t mavlink_msg_manual_control_get_aux5(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 26); +} + +/** + * @brief Get field aux6 from manual_control message + * + * @return Aux continuous input field 6. Normalized in the range [-1000,1000]. Purpose defined by recipient. Valid data if bit 7 of enabled_extensions field is set. 0 if bit 7 is unset. + */ +static inline int16_t mavlink_msg_manual_control_get_aux6(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 28); +} + /** * @brief Decode a manual_control message into a struct * @@ -330,6 +662,16 @@ static inline void mavlink_msg_manual_control_decode(const mavlink_message_t* ms manual_control->r = mavlink_msg_manual_control_get_r(msg); manual_control->buttons = mavlink_msg_manual_control_get_buttons(msg); manual_control->target = mavlink_msg_manual_control_get_target(msg); + manual_control->buttons2 = mavlink_msg_manual_control_get_buttons2(msg); + manual_control->enabled_extensions = mavlink_msg_manual_control_get_enabled_extensions(msg); + manual_control->s = mavlink_msg_manual_control_get_s(msg); + manual_control->t = mavlink_msg_manual_control_get_t(msg); + manual_control->aux1 = mavlink_msg_manual_control_get_aux1(msg); + manual_control->aux2 = mavlink_msg_manual_control_get_aux2(msg); + manual_control->aux3 = mavlink_msg_manual_control_get_aux3(msg); + manual_control->aux4 = mavlink_msg_manual_control_get_aux4(msg); + manual_control->aux5 = mavlink_msg_manual_control_get_aux5(msg); + manual_control->aux6 = mavlink_msg_manual_control_get_aux6(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MANUAL_CONTROL_LEN? msg->len : MAVLINK_MSG_ID_MANUAL_CONTROL_LEN; memset(manual_control, 0, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_manual_setpoint.h b/lib/main/MAVLink/common/mavlink_msg_manual_setpoint.h index 4bc42b14004..935666f91a4 100755 --- a/lib/main/MAVLink/common/mavlink_msg_manual_setpoint.h +++ b/lib/main/MAVLink/common/mavlink_msg_manual_setpoint.h @@ -99,6 +99,57 @@ static inline uint16_t mavlink_msg_manual_setpoint_pack(uint8_t system_id, uint8 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MANUAL_SETPOINT_MIN_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); } +/** + * @brief Pack a manual_setpoint message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param roll [rad/s] Desired roll rate + * @param pitch [rad/s] Desired pitch rate + * @param yaw [rad/s] Desired yaw rate + * @param thrust Collective thrust, normalized to 0 .. 1 + * @param mode_switch Flight mode switch position, 0.. 255 + * @param manual_override_switch Override mode switch position, 0.. 255 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_manual_setpoint_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust, uint8_t mode_switch, uint8_t manual_override_switch) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, thrust); + _mav_put_uint8_t(buf, 20, mode_switch); + _mav_put_uint8_t(buf, 21, manual_override_switch); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#else + mavlink_manual_setpoint_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + packet.mode_switch = mode_switch; + packet.manual_override_switch = manual_override_switch; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MANUAL_SETPOINT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MANUAL_SETPOINT_MIN_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MANUAL_SETPOINT_MIN_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#endif +} + /** * @brief Pack a manual_setpoint message on a channel * @param system_id ID of this system @@ -173,6 +224,20 @@ static inline uint16_t mavlink_msg_manual_setpoint_encode_chan(uint8_t system_id return mavlink_msg_manual_setpoint_pack_chan(system_id, component_id, chan, msg, manual_setpoint->time_boot_ms, manual_setpoint->roll, manual_setpoint->pitch, manual_setpoint->yaw, manual_setpoint->thrust, manual_setpoint->mode_switch, manual_setpoint->manual_override_switch); } +/** + * @brief Encode a manual_setpoint struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param manual_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_manual_setpoint_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_manual_setpoint_t* manual_setpoint) +{ + return mavlink_msg_manual_setpoint_pack_status(system_id, component_id, _status, msg, manual_setpoint->time_boot_ms, manual_setpoint->roll, manual_setpoint->pitch, manual_setpoint->yaw, manual_setpoint->thrust, manual_setpoint->mode_switch, manual_setpoint->manual_override_switch); +} + /** * @brief Send a manual_setpoint message * @param chan MAVLink channel to send the message @@ -230,7 +295,7 @@ static inline void mavlink_msg_manual_setpoint_send_struct(mavlink_channel_t cha #if MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_memory_vect.h b/lib/main/MAVLink/common/mavlink_msg_memory_vect.h index c3315474737..e1459ad6e94 100755 --- a/lib/main/MAVLink/common/mavlink_msg_memory_vect.h +++ b/lib/main/MAVLink/common/mavlink_msg_memory_vect.h @@ -71,7 +71,7 @@ static inline uint16_t mavlink_msg_memory_vect_pack(uint8_t system_id, uint8_t c packet.address = address; packet.ver = ver; packet.type = type; - mav_array_memcpy(packet.value, value, sizeof(int8_t)*32); + mav_array_assign_int8_t(packet.value, value, 32); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN); #endif @@ -79,6 +79,46 @@ static inline uint16_t mavlink_msg_memory_vect_pack(uint8_t system_id, uint8_t c return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMORY_VECT_MIN_LEN, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); } +/** + * @brief Pack a memory_vect message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param address Starting address of the debug variables + * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + * @param type Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 + * @param value Memory contents at specified address + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_memory_vect_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint16_t address, uint8_t ver, uint8_t type, const int8_t *value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MEMORY_VECT_LEN]; + _mav_put_uint16_t(buf, 0, address); + _mav_put_uint8_t(buf, 2, ver); + _mav_put_uint8_t(buf, 3, type); + _mav_put_int8_t_array(buf, 4, value, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#else + mavlink_memory_vect_t packet; + packet.address = address; + packet.ver = ver; + packet.type = type; + mav_array_memcpy(packet.value, value, sizeof(int8_t)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MEMORY_VECT_MIN_LEN, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MEMORY_VECT_MIN_LEN, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#endif +} + /** * @brief Pack a memory_vect message on a channel * @param system_id ID of this system @@ -107,7 +147,7 @@ static inline uint16_t mavlink_msg_memory_vect_pack_chan(uint8_t system_id, uint packet.address = address; packet.ver = ver; packet.type = type; - mav_array_memcpy(packet.value, value, sizeof(int8_t)*32); + mav_array_assign_int8_t(packet.value, value, 32); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN); #endif @@ -142,6 +182,20 @@ static inline uint16_t mavlink_msg_memory_vect_encode_chan(uint8_t system_id, ui return mavlink_msg_memory_vect_pack_chan(system_id, component_id, chan, msg, memory_vect->address, memory_vect->ver, memory_vect->type, memory_vect->value); } +/** + * @brief Encode a memory_vect struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param memory_vect C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_memory_vect_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_memory_vect_t* memory_vect) +{ + return mavlink_msg_memory_vect_pack_status(system_id, component_id, _status, msg, memory_vect->address, memory_vect->ver, memory_vect->type, memory_vect->value); +} + /** * @brief Send a memory_vect message * @param chan MAVLink channel to send the message @@ -167,7 +221,7 @@ static inline void mavlink_msg_memory_vect_send(mavlink_channel_t chan, uint16_t packet.address = address; packet.ver = ver; packet.type = type; - mav_array_memcpy(packet.value, value, sizeof(int8_t)*32); + mav_array_assign_int8_t(packet.value, value, 32); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)&packet, MAVLINK_MSG_ID_MEMORY_VECT_MIN_LEN, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); #endif } @@ -188,7 +242,7 @@ static inline void mavlink_msg_memory_vect_send_struct(mavlink_channel_t chan, c #if MAVLINK_MSG_ID_MEMORY_VECT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -208,7 +262,7 @@ static inline void mavlink_msg_memory_vect_send_buf(mavlink_message_t *msgbuf, m packet->address = address; packet->ver = ver; packet->type = type; - mav_array_memcpy(packet->value, value, sizeof(int8_t)*32); + mav_array_assign_int8_t(packet->value, value, 32); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)packet, MAVLINK_MSG_ID_MEMORY_VECT_MIN_LEN, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_message_interval.h b/lib/main/MAVLink/common/mavlink_msg_message_interval.h index a6a52b1fdc8..9816d33d58d 100755 --- a/lib/main/MAVLink/common/mavlink_msg_message_interval.h +++ b/lib/main/MAVLink/common/mavlink_msg_message_interval.h @@ -69,6 +69,42 @@ static inline uint16_t mavlink_msg_message_interval_pack(uint8_t system_id, uint return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MESSAGE_INTERVAL_MIN_LEN, MAVLINK_MSG_ID_MESSAGE_INTERVAL_LEN, MAVLINK_MSG_ID_MESSAGE_INTERVAL_CRC); } +/** + * @brief Pack a message_interval message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param message_id The ID of the requested MAVLink message. v1.0 is limited to 254 messages. + * @param interval_us [us] The interval between two messages. A value of -1 indicates this stream is disabled, 0 indicates it is not available, > 0 indicates the interval at which it is sent. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_message_interval_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint16_t message_id, int32_t interval_us) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MESSAGE_INTERVAL_LEN]; + _mav_put_int32_t(buf, 0, interval_us); + _mav_put_uint16_t(buf, 4, message_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MESSAGE_INTERVAL_LEN); +#else + mavlink_message_interval_t packet; + packet.interval_us = interval_us; + packet.message_id = message_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MESSAGE_INTERVAL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MESSAGE_INTERVAL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MESSAGE_INTERVAL_MIN_LEN, MAVLINK_MSG_ID_MESSAGE_INTERVAL_LEN, MAVLINK_MSG_ID_MESSAGE_INTERVAL_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MESSAGE_INTERVAL_MIN_LEN, MAVLINK_MSG_ID_MESSAGE_INTERVAL_LEN); +#endif +} + /** * @brief Pack a message_interval message on a channel * @param system_id ID of this system @@ -128,6 +164,20 @@ static inline uint16_t mavlink_msg_message_interval_encode_chan(uint8_t system_i return mavlink_msg_message_interval_pack_chan(system_id, component_id, chan, msg, message_interval->message_id, message_interval->interval_us); } +/** + * @brief Encode a message_interval struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param message_interval C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_message_interval_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_message_interval_t* message_interval) +{ + return mavlink_msg_message_interval_pack_status(system_id, component_id, _status, msg, message_interval->message_id, message_interval->interval_us); +} + /** * @brief Send a message_interval message * @param chan MAVLink channel to send the message @@ -170,7 +220,7 @@ static inline void mavlink_msg_message_interval_send_struct(mavlink_channel_t ch #if MAVLINK_MSG_ID_MESSAGE_INTERVAL_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_ack.h b/lib/main/MAVLink/common/mavlink_msg_mission_ack.h index 916b94d38d6..a69f22a08fe 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_ack.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_ack.h @@ -9,11 +9,18 @@ typedef struct __mavlink_mission_ack_t { uint8_t target_component; /*< Component ID*/ uint8_t type; /*< Mission result.*/ uint8_t mission_type; /*< Mission type.*/ + uint32_t opaque_id; /*< Id of new on-vehicle mission, fence, or rally point plan (on upload to vehicle). + The id is calculated and returned by a vehicle when a new plan is uploaded by a GCS. + The only requirement on the id is that it must change when there is any change to the on-vehicle plan type (there is no requirement that the id be globally unique). + 0 on download from the vehicle to the GCS (on download the ID is set in MISSION_COUNT). + 0 if plan ids are not supported. + The current on-vehicle plan ids are streamed in `MISSION_CURRENT`, allowing a GCS to determine if any part of the plan has changed and needs to be re-uploaded. + */ } mavlink_mission_ack_t; -#define MAVLINK_MSG_ID_MISSION_ACK_LEN 4 +#define MAVLINK_MSG_ID_MISSION_ACK_LEN 8 #define MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN 3 -#define MAVLINK_MSG_ID_47_LEN 4 +#define MAVLINK_MSG_ID_47_LEN 8 #define MAVLINK_MSG_ID_47_MIN_LEN 3 #define MAVLINK_MSG_ID_MISSION_ACK_CRC 153 @@ -25,21 +32,23 @@ typedef struct __mavlink_mission_ack_t { #define MAVLINK_MESSAGE_INFO_MISSION_ACK { \ 47, \ "MISSION_ACK", \ - 4, \ + 5, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_ack_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_ack_t, target_component) }, \ { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_ack_t, type) }, \ { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_ack_t, mission_type) }, \ + { "opaque_id", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_mission_ack_t, opaque_id) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_ACK { \ "MISSION_ACK", \ - 4, \ + 5, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_ack_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_ack_t, target_component) }, \ { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_ack_t, type) }, \ { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_ack_t, mission_type) }, \ + { "opaque_id", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_mission_ack_t, opaque_id) }, \ } \ } #endif @@ -54,10 +63,17 @@ typedef struct __mavlink_mission_ack_t { * @param target_component Component ID * @param type Mission result. * @param mission_type Mission type. + * @param opaque_id Id of new on-vehicle mission, fence, or rally point plan (on upload to vehicle). + The id is calculated and returned by a vehicle when a new plan is uploaded by a GCS. + The only requirement on the id is that it must change when there is any change to the on-vehicle plan type (there is no requirement that the id be globally unique). + 0 on download from the vehicle to the GCS (on download the ID is set in MISSION_COUNT). + 0 if plan ids are not supported. + The current on-vehicle plan ids are streamed in `MISSION_CURRENT`, allowing a GCS to determine if any part of the plan has changed and needs to be re-uploaded. + * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t type, uint8_t mission_type) + uint8_t target_system, uint8_t target_component, uint8_t type, uint8_t mission_type, uint32_t opaque_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ACK_LEN]; @@ -65,6 +81,7 @@ static inline uint16_t mavlink_msg_mission_ack_pack(uint8_t system_id, uint8_t c _mav_put_uint8_t(buf, 1, target_component); _mav_put_uint8_t(buf, 2, type); _mav_put_uint8_t(buf, 3, mission_type); + _mav_put_uint32_t(buf, 4, opaque_id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ACK_LEN); #else @@ -73,6 +90,7 @@ static inline uint16_t mavlink_msg_mission_ack_pack(uint8_t system_id, uint8_t c packet.target_component = target_component; packet.type = type; packet.mission_type = mission_type; + packet.opaque_id = opaque_id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ACK_LEN); #endif @@ -81,6 +99,57 @@ static inline uint16_t mavlink_msg_mission_ack_pack(uint8_t system_id, uint8_t c return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); } +/** + * @brief Pack a mission_ack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param type Mission result. + * @param mission_type Mission type. + * @param opaque_id Id of new on-vehicle mission, fence, or rally point plan (on upload to vehicle). + The id is calculated and returned by a vehicle when a new plan is uploaded by a GCS. + The only requirement on the id is that it must change when there is any change to the on-vehicle plan type (there is no requirement that the id be globally unique). + 0 on download from the vehicle to the GCS (on download the ID is set in MISSION_COUNT). + 0 if plan ids are not supported. + The current on-vehicle plan ids are streamed in `MISSION_CURRENT`, allowing a GCS to determine if any part of the plan has changed and needs to be re-uploaded. + + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_ack_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t type, uint8_t mission_type, uint32_t opaque_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ACK_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, type); + _mav_put_uint8_t(buf, 3, mission_type); + _mav_put_uint32_t(buf, 4, opaque_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#else + mavlink_mission_ack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.type = type; + packet.mission_type = mission_type; + packet.opaque_id = opaque_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_ACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#endif +} + /** * @brief Pack a mission_ack message on a channel * @param system_id ID of this system @@ -91,11 +160,18 @@ static inline uint16_t mavlink_msg_mission_ack_pack(uint8_t system_id, uint8_t c * @param target_component Component ID * @param type Mission result. * @param mission_type Mission type. + * @param opaque_id Id of new on-vehicle mission, fence, or rally point plan (on upload to vehicle). + The id is calculated and returned by a vehicle when a new plan is uploaded by a GCS. + The only requirement on the id is that it must change when there is any change to the on-vehicle plan type (there is no requirement that the id be globally unique). + 0 on download from the vehicle to the GCS (on download the ID is set in MISSION_COUNT). + 0 if plan ids are not supported. + The current on-vehicle plan ids are streamed in `MISSION_CURRENT`, allowing a GCS to determine if any part of the plan has changed and needs to be re-uploaded. + * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t type,uint8_t mission_type) + uint8_t target_system,uint8_t target_component,uint8_t type,uint8_t mission_type,uint32_t opaque_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ACK_LEN]; @@ -103,6 +179,7 @@ static inline uint16_t mavlink_msg_mission_ack_pack_chan(uint8_t system_id, uint _mav_put_uint8_t(buf, 1, target_component); _mav_put_uint8_t(buf, 2, type); _mav_put_uint8_t(buf, 3, mission_type); + _mav_put_uint32_t(buf, 4, opaque_id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ACK_LEN); #else @@ -111,6 +188,7 @@ static inline uint16_t mavlink_msg_mission_ack_pack_chan(uint8_t system_id, uint packet.target_component = target_component; packet.type = type; packet.mission_type = mission_type; + packet.opaque_id = opaque_id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ACK_LEN); #endif @@ -129,7 +207,7 @@ static inline uint16_t mavlink_msg_mission_ack_pack_chan(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_mission_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_ack_t* mission_ack) { - return mavlink_msg_mission_ack_pack(system_id, component_id, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type, mission_ack->mission_type); + return mavlink_msg_mission_ack_pack(system_id, component_id, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type, mission_ack->mission_type, mission_ack->opaque_id); } /** @@ -143,7 +221,21 @@ static inline uint16_t mavlink_msg_mission_ack_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_mission_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_ack_t* mission_ack) { - return mavlink_msg_mission_ack_pack_chan(system_id, component_id, chan, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type, mission_ack->mission_type); + return mavlink_msg_mission_ack_pack_chan(system_id, component_id, chan, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type, mission_ack->mission_type, mission_ack->opaque_id); +} + +/** + * @brief Encode a mission_ack struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param mission_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_ack_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_mission_ack_t* mission_ack) +{ + return mavlink_msg_mission_ack_pack_status(system_id, component_id, _status, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type, mission_ack->mission_type, mission_ack->opaque_id); } /** @@ -154,10 +246,17 @@ static inline uint16_t mavlink_msg_mission_ack_encode_chan(uint8_t system_id, ui * @param target_component Component ID * @param type Mission result. * @param mission_type Mission type. + * @param opaque_id Id of new on-vehicle mission, fence, or rally point plan (on upload to vehicle). + The id is calculated and returned by a vehicle when a new plan is uploaded by a GCS. + The only requirement on the id is that it must change when there is any change to the on-vehicle plan type (there is no requirement that the id be globally unique). + 0 on download from the vehicle to the GCS (on download the ID is set in MISSION_COUNT). + 0 if plan ids are not supported. + The current on-vehicle plan ids are streamed in `MISSION_CURRENT`, allowing a GCS to determine if any part of the plan has changed and needs to be re-uploaded. + */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type, uint8_t mission_type) +static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type, uint8_t mission_type, uint32_t opaque_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ACK_LEN]; @@ -165,6 +264,7 @@ static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t _mav_put_uint8_t(buf, 1, target_component); _mav_put_uint8_t(buf, 2, type); _mav_put_uint8_t(buf, 3, mission_type); + _mav_put_uint32_t(buf, 4, opaque_id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); #else @@ -173,6 +273,7 @@ static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t packet.target_component = target_component; packet.type = type; packet.mission_type = mission_type; + packet.opaque_id = opaque_id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); #endif @@ -186,7 +287,7 @@ static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t static inline void mavlink_msg_mission_ack_send_struct(mavlink_channel_t chan, const mavlink_mission_ack_t* mission_ack) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_ack_send(chan, mission_ack->target_system, mission_ack->target_component, mission_ack->type, mission_ack->mission_type); + mavlink_msg_mission_ack_send(chan, mission_ack->target_system, mission_ack->target_component, mission_ack->type, mission_ack->mission_type, mission_ack->opaque_id); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)mission_ack, MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); #endif @@ -194,13 +295,13 @@ static inline void mavlink_msg_mission_ack_send_struct(mavlink_channel_t chan, c #if MAVLINK_MSG_ID_MISSION_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type, uint8_t mission_type) +static inline void mavlink_msg_mission_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type, uint8_t mission_type, uint32_t opaque_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,6 +309,7 @@ static inline void mavlink_msg_mission_ack_send_buf(mavlink_message_t *msgbuf, m _mav_put_uint8_t(buf, 1, target_component); _mav_put_uint8_t(buf, 2, type); _mav_put_uint8_t(buf, 3, mission_type); + _mav_put_uint32_t(buf, 4, opaque_id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); #else @@ -216,6 +318,7 @@ static inline void mavlink_msg_mission_ack_send_buf(mavlink_message_t *msgbuf, m packet->target_component = target_component; packet->type = type; packet->mission_type = mission_type; + packet->opaque_id = opaque_id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)packet, MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); #endif @@ -267,6 +370,22 @@ static inline uint8_t mavlink_msg_mission_ack_get_mission_type(const mavlink_mes return _MAV_RETURN_uint8_t(msg, 3); } +/** + * @brief Get field opaque_id from mission_ack message + * + * @return Id of new on-vehicle mission, fence, or rally point plan (on upload to vehicle). + The id is calculated and returned by a vehicle when a new plan is uploaded by a GCS. + The only requirement on the id is that it must change when there is any change to the on-vehicle plan type (there is no requirement that the id be globally unique). + 0 on download from the vehicle to the GCS (on download the ID is set in MISSION_COUNT). + 0 if plan ids are not supported. + The current on-vehicle plan ids are streamed in `MISSION_CURRENT`, allowing a GCS to determine if any part of the plan has changed and needs to be re-uploaded. + + */ +static inline uint32_t mavlink_msg_mission_ack_get_opaque_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + /** * @brief Decode a mission_ack message into a struct * @@ -280,6 +399,7 @@ static inline void mavlink_msg_mission_ack_decode(const mavlink_message_t* msg, mission_ack->target_component = mavlink_msg_mission_ack_get_target_component(msg); mission_ack->type = mavlink_msg_mission_ack_get_type(msg); mission_ack->mission_type = mavlink_msg_mission_ack_get_mission_type(msg); + mission_ack->opaque_id = mavlink_msg_mission_ack_get_opaque_id(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_ACK_LEN? msg->len : MAVLINK_MSG_ID_MISSION_ACK_LEN; memset(mission_ack, 0, MAVLINK_MSG_ID_MISSION_ACK_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_changed.h b/lib/main/MAVLink/common/mavlink_msg_mission_changed.h deleted file mode 100644 index 28f75098e46..00000000000 --- a/lib/main/MAVLink/common/mavlink_msg_mission_changed.h +++ /dev/null @@ -1,313 +0,0 @@ -#pragma once -// MESSAGE MISSION_CHANGED PACKING - -#define MAVLINK_MSG_ID_MISSION_CHANGED 52 - - -typedef struct __mavlink_mission_changed_t { - int16_t start_index; /*< Start index for partial mission change (-1 for all items).*/ - int16_t end_index; /*< End index of a partial mission change. -1 is a synonym for the last mission item (i.e. selects all items from start_index). Ignore field if start_index=-1.*/ - uint8_t origin_sysid; /*< System ID of the author of the new mission.*/ - uint8_t origin_compid; /*< Compnent ID of the author of the new mission.*/ - uint8_t mission_type; /*< Mission type.*/ -} mavlink_mission_changed_t; - -#define MAVLINK_MSG_ID_MISSION_CHANGED_LEN 7 -#define MAVLINK_MSG_ID_MISSION_CHANGED_MIN_LEN 7 -#define MAVLINK_MSG_ID_52_LEN 7 -#define MAVLINK_MSG_ID_52_MIN_LEN 7 - -#define MAVLINK_MSG_ID_MISSION_CHANGED_CRC 132 -#define MAVLINK_MSG_ID_52_CRC 132 - - - -#if MAVLINK_COMMAND_24BIT -#define MAVLINK_MESSAGE_INFO_MISSION_CHANGED { \ - 52, \ - "MISSION_CHANGED", \ - 5, \ - { { "start_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_mission_changed_t, start_index) }, \ - { "end_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_mission_changed_t, end_index) }, \ - { "origin_sysid", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_changed_t, origin_sysid) }, \ - { "origin_compid", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_changed_t, origin_compid) }, \ - { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_mission_changed_t, mission_type) }, \ - } \ -} -#else -#define MAVLINK_MESSAGE_INFO_MISSION_CHANGED { \ - "MISSION_CHANGED", \ - 5, \ - { { "start_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_mission_changed_t, start_index) }, \ - { "end_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_mission_changed_t, end_index) }, \ - { "origin_sysid", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_changed_t, origin_sysid) }, \ - { "origin_compid", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_changed_t, origin_compid) }, \ - { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_mission_changed_t, mission_type) }, \ - } \ -} -#endif - -/** - * @brief Pack a mission_changed message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param start_index Start index for partial mission change (-1 for all items). - * @param end_index End index of a partial mission change. -1 is a synonym for the last mission item (i.e. selects all items from start_index). Ignore field if start_index=-1. - * @param origin_sysid System ID of the author of the new mission. - * @param origin_compid Compnent ID of the author of the new mission. - * @param mission_type Mission type. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_changed_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - int16_t start_index, int16_t end_index, uint8_t origin_sysid, uint8_t origin_compid, uint8_t mission_type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_CHANGED_LEN]; - _mav_put_int16_t(buf, 0, start_index); - _mav_put_int16_t(buf, 2, end_index); - _mav_put_uint8_t(buf, 4, origin_sysid); - _mav_put_uint8_t(buf, 5, origin_compid); - _mav_put_uint8_t(buf, 6, mission_type); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CHANGED_LEN); -#else - mavlink_mission_changed_t packet; - packet.start_index = start_index; - packet.end_index = end_index; - packet.origin_sysid = origin_sysid; - packet.origin_compid = origin_compid; - packet.mission_type = mission_type; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CHANGED_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_CHANGED; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_CHANGED_MIN_LEN, MAVLINK_MSG_ID_MISSION_CHANGED_LEN, MAVLINK_MSG_ID_MISSION_CHANGED_CRC); -} - -/** - * @brief Pack a mission_changed message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param start_index Start index for partial mission change (-1 for all items). - * @param end_index End index of a partial mission change. -1 is a synonym for the last mission item (i.e. selects all items from start_index). Ignore field if start_index=-1. - * @param origin_sysid System ID of the author of the new mission. - * @param origin_compid Compnent ID of the author of the new mission. - * @param mission_type Mission type. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_mission_changed_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - int16_t start_index,int16_t end_index,uint8_t origin_sysid,uint8_t origin_compid,uint8_t mission_type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_CHANGED_LEN]; - _mav_put_int16_t(buf, 0, start_index); - _mav_put_int16_t(buf, 2, end_index); - _mav_put_uint8_t(buf, 4, origin_sysid); - _mav_put_uint8_t(buf, 5, origin_compid); - _mav_put_uint8_t(buf, 6, mission_type); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CHANGED_LEN); -#else - mavlink_mission_changed_t packet; - packet.start_index = start_index; - packet.end_index = end_index; - packet.origin_sysid = origin_sysid; - packet.origin_compid = origin_compid; - packet.mission_type = mission_type; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CHANGED_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_MISSION_CHANGED; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_CHANGED_MIN_LEN, MAVLINK_MSG_ID_MISSION_CHANGED_LEN, MAVLINK_MSG_ID_MISSION_CHANGED_CRC); -} - -/** - * @brief Encode a mission_changed struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param mission_changed C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_changed_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_changed_t* mission_changed) -{ - return mavlink_msg_mission_changed_pack(system_id, component_id, msg, mission_changed->start_index, mission_changed->end_index, mission_changed->origin_sysid, mission_changed->origin_compid, mission_changed->mission_type); -} - -/** - * @brief Encode a mission_changed struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param mission_changed C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_mission_changed_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_changed_t* mission_changed) -{ - return mavlink_msg_mission_changed_pack_chan(system_id, component_id, chan, msg, mission_changed->start_index, mission_changed->end_index, mission_changed->origin_sysid, mission_changed->origin_compid, mission_changed->mission_type); -} - -/** - * @brief Send a mission_changed message - * @param chan MAVLink channel to send the message - * - * @param start_index Start index for partial mission change (-1 for all items). - * @param end_index End index of a partial mission change. -1 is a synonym for the last mission item (i.e. selects all items from start_index). Ignore field if start_index=-1. - * @param origin_sysid System ID of the author of the new mission. - * @param origin_compid Compnent ID of the author of the new mission. - * @param mission_type Mission type. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_mission_changed_send(mavlink_channel_t chan, int16_t start_index, int16_t end_index, uint8_t origin_sysid, uint8_t origin_compid, uint8_t mission_type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_MISSION_CHANGED_LEN]; - _mav_put_int16_t(buf, 0, start_index); - _mav_put_int16_t(buf, 2, end_index); - _mav_put_uint8_t(buf, 4, origin_sysid); - _mav_put_uint8_t(buf, 5, origin_compid); - _mav_put_uint8_t(buf, 6, mission_type); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CHANGED, buf, MAVLINK_MSG_ID_MISSION_CHANGED_MIN_LEN, MAVLINK_MSG_ID_MISSION_CHANGED_LEN, MAVLINK_MSG_ID_MISSION_CHANGED_CRC); -#else - mavlink_mission_changed_t packet; - packet.start_index = start_index; - packet.end_index = end_index; - packet.origin_sysid = origin_sysid; - packet.origin_compid = origin_compid; - packet.mission_type = mission_type; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CHANGED, (const char *)&packet, MAVLINK_MSG_ID_MISSION_CHANGED_MIN_LEN, MAVLINK_MSG_ID_MISSION_CHANGED_LEN, MAVLINK_MSG_ID_MISSION_CHANGED_CRC); -#endif -} - -/** - * @brief Send a mission_changed message - * @param chan MAVLink channel to send the message - * @param struct The MAVLink struct to serialize - */ -static inline void mavlink_msg_mission_changed_send_struct(mavlink_channel_t chan, const mavlink_mission_changed_t* mission_changed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_changed_send(chan, mission_changed->start_index, mission_changed->end_index, mission_changed->origin_sysid, mission_changed->origin_compid, mission_changed->mission_type); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CHANGED, (const char *)mission_changed, MAVLINK_MSG_ID_MISSION_CHANGED_MIN_LEN, MAVLINK_MSG_ID_MISSION_CHANGED_LEN, MAVLINK_MSG_ID_MISSION_CHANGED_CRC); -#endif -} - -#if MAVLINK_MSG_ID_MISSION_CHANGED_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_mission_changed_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int16_t start_index, int16_t end_index, uint8_t origin_sysid, uint8_t origin_compid, uint8_t mission_type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_int16_t(buf, 0, start_index); - _mav_put_int16_t(buf, 2, end_index); - _mav_put_uint8_t(buf, 4, origin_sysid); - _mav_put_uint8_t(buf, 5, origin_compid); - _mav_put_uint8_t(buf, 6, mission_type); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CHANGED, buf, MAVLINK_MSG_ID_MISSION_CHANGED_MIN_LEN, MAVLINK_MSG_ID_MISSION_CHANGED_LEN, MAVLINK_MSG_ID_MISSION_CHANGED_CRC); -#else - mavlink_mission_changed_t *packet = (mavlink_mission_changed_t *)msgbuf; - packet->start_index = start_index; - packet->end_index = end_index; - packet->origin_sysid = origin_sysid; - packet->origin_compid = origin_compid; - packet->mission_type = mission_type; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CHANGED, (const char *)packet, MAVLINK_MSG_ID_MISSION_CHANGED_MIN_LEN, MAVLINK_MSG_ID_MISSION_CHANGED_LEN, MAVLINK_MSG_ID_MISSION_CHANGED_CRC); -#endif -} -#endif - -#endif - -// MESSAGE MISSION_CHANGED UNPACKING - - -/** - * @brief Get field start_index from mission_changed message - * - * @return Start index for partial mission change (-1 for all items). - */ -static inline int16_t mavlink_msg_mission_changed_get_start_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 0); -} - -/** - * @brief Get field end_index from mission_changed message - * - * @return End index of a partial mission change. -1 is a synonym for the last mission item (i.e. selects all items from start_index). Ignore field if start_index=-1. - */ -static inline int16_t mavlink_msg_mission_changed_get_end_index(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int16_t(msg, 2); -} - -/** - * @brief Get field origin_sysid from mission_changed message - * - * @return System ID of the author of the new mission. - */ -static inline uint8_t mavlink_msg_mission_changed_get_origin_sysid(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field origin_compid from mission_changed message - * - * @return Compnent ID of the author of the new mission. - */ -static inline uint8_t mavlink_msg_mission_changed_get_origin_compid(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field mission_type from mission_changed message - * - * @return Mission type. - */ -static inline uint8_t mavlink_msg_mission_changed_get_mission_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @brief Decode a mission_changed message into a struct - * - * @param msg The message to decode - * @param mission_changed C-struct to decode the message contents into - */ -static inline void mavlink_msg_mission_changed_decode(const mavlink_message_t* msg, mavlink_mission_changed_t* mission_changed) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mission_changed->start_index = mavlink_msg_mission_changed_get_start_index(msg); - mission_changed->end_index = mavlink_msg_mission_changed_get_end_index(msg); - mission_changed->origin_sysid = mavlink_msg_mission_changed_get_origin_sysid(msg); - mission_changed->origin_compid = mavlink_msg_mission_changed_get_origin_compid(msg); - mission_changed->mission_type = mavlink_msg_mission_changed_get_mission_type(msg); -#else - uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_CHANGED_LEN? msg->len : MAVLINK_MSG_ID_MISSION_CHANGED_LEN; - memset(mission_changed, 0, MAVLINK_MSG_ID_MISSION_CHANGED_LEN); - memcpy(mission_changed, _MAV_PAYLOAD(msg), len); -#endif -} diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_clear_all.h b/lib/main/MAVLink/common/mavlink_msg_mission_clear_all.h index 1e585f9fcae..524785d9ccf 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_clear_all.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_clear_all.h @@ -75,6 +75,45 @@ static inline uint16_t mavlink_msg_mission_clear_all_pack(uint8_t system_id, uin return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_MIN_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); } +/** + * @brief Pack a mission_clear_all message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param mission_type Mission type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_clear_all_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t mission_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mission_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#else + mavlink_mission_clear_all_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mission_type = mission_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_CLEAR_ALL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_MIN_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_MIN_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#endif +} + /** * @brief Pack a mission_clear_all message on a channel * @param system_id ID of this system @@ -137,6 +176,20 @@ static inline uint16_t mavlink_msg_mission_clear_all_encode_chan(uint8_t system_ return mavlink_msg_mission_clear_all_pack_chan(system_id, component_id, chan, msg, mission_clear_all->target_system, mission_clear_all->target_component, mission_clear_all->mission_type); } +/** + * @brief Encode a mission_clear_all struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param mission_clear_all C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_clear_all_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_mission_clear_all_t* mission_clear_all) +{ + return mavlink_msg_mission_clear_all_pack_status(system_id, component_id, _status, msg, mission_clear_all->target_system, mission_clear_all->target_component, mission_clear_all->mission_type); +} + /** * @brief Send a mission_clear_all message * @param chan MAVLink channel to send the message @@ -182,7 +235,7 @@ static inline void mavlink_msg_mission_clear_all_send_struct(mavlink_channel_t c #if MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_count.h b/lib/main/MAVLink/common/mavlink_msg_mission_count.h index 53afd490021..0e46831cccc 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_count.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_count.h @@ -3,17 +3,24 @@ #define MAVLINK_MSG_ID_MISSION_COUNT 44 - +MAVPACKED( typedef struct __mavlink_mission_count_t { uint16_t count; /*< Number of mission items in the sequence*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ uint8_t mission_type; /*< Mission type.*/ -} mavlink_mission_count_t; - -#define MAVLINK_MSG_ID_MISSION_COUNT_LEN 5 + uint32_t opaque_id; /*< Id of current on-vehicle mission, fence, or rally point plan (on download from vehicle). + This field is used when downloading a plan from a vehicle to a GCS. + 0 on upload to the vehicle from GCS. + 0 if plan ids are not supported. + The current on-vehicle plan ids are streamed in `MISSION_CURRENT`, allowing a GCS to determine if any part of the plan has changed and needs to be re-uploaded. + The ids are recalculated by the vehicle when any part of the on-vehicle plan changes (when a new plan is uploaded, the vehicle returns the new id to the GCS in MISSION_ACK). + */ +}) mavlink_mission_count_t; + +#define MAVLINK_MSG_ID_MISSION_COUNT_LEN 9 #define MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN 4 -#define MAVLINK_MSG_ID_44_LEN 5 +#define MAVLINK_MSG_ID_44_LEN 9 #define MAVLINK_MSG_ID_44_MIN_LEN 4 #define MAVLINK_MSG_ID_MISSION_COUNT_CRC 221 @@ -25,21 +32,23 @@ typedef struct __mavlink_mission_count_t { #define MAVLINK_MESSAGE_INFO_MISSION_COUNT { \ 44, \ "MISSION_COUNT", \ - 4, \ + 5, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_count_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_count_t, target_component) }, \ { "count", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_count_t, count) }, \ { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_count_t, mission_type) }, \ + { "opaque_id", NULL, MAVLINK_TYPE_UINT32_T, 0, 5, offsetof(mavlink_mission_count_t, opaque_id) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_COUNT { \ "MISSION_COUNT", \ - 4, \ + 5, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_count_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_count_t, target_component) }, \ { "count", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_count_t, count) }, \ { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_count_t, mission_type) }, \ + { "opaque_id", NULL, MAVLINK_TYPE_UINT32_T, 0, 5, offsetof(mavlink_mission_count_t, opaque_id) }, \ } \ } #endif @@ -54,10 +63,17 @@ typedef struct __mavlink_mission_count_t { * @param target_component Component ID * @param count Number of mission items in the sequence * @param mission_type Mission type. + * @param opaque_id Id of current on-vehicle mission, fence, or rally point plan (on download from vehicle). + This field is used when downloading a plan from a vehicle to a GCS. + 0 on upload to the vehicle from GCS. + 0 if plan ids are not supported. + The current on-vehicle plan ids are streamed in `MISSION_CURRENT`, allowing a GCS to determine if any part of the plan has changed and needs to be re-uploaded. + The ids are recalculated by the vehicle when any part of the on-vehicle plan changes (when a new plan is uploaded, the vehicle returns the new id to the GCS in MISSION_ACK). + * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t count, uint8_t mission_type) + uint8_t target_system, uint8_t target_component, uint16_t count, uint8_t mission_type, uint32_t opaque_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_COUNT_LEN]; @@ -65,6 +81,7 @@ static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); _mav_put_uint8_t(buf, 4, mission_type); + _mav_put_uint32_t(buf, 5, opaque_id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN); #else @@ -73,6 +90,7 @@ static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t packet.target_system = target_system; packet.target_component = target_component; packet.mission_type = mission_type; + packet.opaque_id = opaque_id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN); #endif @@ -81,6 +99,57 @@ static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); } +/** + * @brief Pack a mission_count message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param count Number of mission items in the sequence + * @param mission_type Mission type. + * @param opaque_id Id of current on-vehicle mission, fence, or rally point plan (on download from vehicle). + This field is used when downloading a plan from a vehicle to a GCS. + 0 on upload to the vehicle from GCS. + 0 if plan ids are not supported. + The current on-vehicle plan ids are streamed in `MISSION_CURRENT`, allowing a GCS to determine if any part of the plan has changed and needs to be re-uploaded. + The ids are recalculated by the vehicle when any part of the on-vehicle plan changes (when a new plan is uploaded, the vehicle returns the new id to the GCS in MISSION_ACK). + + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_count_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t count, uint8_t mission_type, uint32_t opaque_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_COUNT_LEN]; + _mav_put_uint16_t(buf, 0, count); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); + _mav_put_uint32_t(buf, 5, opaque_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#else + mavlink_mission_count_t packet; + packet.count = count; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mission_type = mission_type; + packet.opaque_id = opaque_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_COUNT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#endif +} + /** * @brief Pack a mission_count message on a channel * @param system_id ID of this system @@ -91,11 +160,18 @@ static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t * @param target_component Component ID * @param count Number of mission items in the sequence * @param mission_type Mission type. + * @param opaque_id Id of current on-vehicle mission, fence, or rally point plan (on download from vehicle). + This field is used when downloading a plan from a vehicle to a GCS. + 0 on upload to the vehicle from GCS. + 0 if plan ids are not supported. + The current on-vehicle plan ids are streamed in `MISSION_CURRENT`, allowing a GCS to determine if any part of the plan has changed and needs to be re-uploaded. + The ids are recalculated by the vehicle when any part of the on-vehicle plan changes (when a new plan is uploaded, the vehicle returns the new id to the GCS in MISSION_ACK). + * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_count_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t count,uint8_t mission_type) + uint8_t target_system,uint8_t target_component,uint16_t count,uint8_t mission_type,uint32_t opaque_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_COUNT_LEN]; @@ -103,6 +179,7 @@ static inline uint16_t mavlink_msg_mission_count_pack_chan(uint8_t system_id, ui _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); _mav_put_uint8_t(buf, 4, mission_type); + _mav_put_uint32_t(buf, 5, opaque_id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN); #else @@ -111,6 +188,7 @@ static inline uint16_t mavlink_msg_mission_count_pack_chan(uint8_t system_id, ui packet.target_system = target_system; packet.target_component = target_component; packet.mission_type = mission_type; + packet.opaque_id = opaque_id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN); #endif @@ -129,7 +207,7 @@ static inline uint16_t mavlink_msg_mission_count_pack_chan(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_mission_count_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_count_t* mission_count) { - return mavlink_msg_mission_count_pack(system_id, component_id, msg, mission_count->target_system, mission_count->target_component, mission_count->count, mission_count->mission_type); + return mavlink_msg_mission_count_pack(system_id, component_id, msg, mission_count->target_system, mission_count->target_component, mission_count->count, mission_count->mission_type, mission_count->opaque_id); } /** @@ -143,7 +221,21 @@ static inline uint16_t mavlink_msg_mission_count_encode(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_mission_count_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_count_t* mission_count) { - return mavlink_msg_mission_count_pack_chan(system_id, component_id, chan, msg, mission_count->target_system, mission_count->target_component, mission_count->count, mission_count->mission_type); + return mavlink_msg_mission_count_pack_chan(system_id, component_id, chan, msg, mission_count->target_system, mission_count->target_component, mission_count->count, mission_count->mission_type, mission_count->opaque_id); +} + +/** + * @brief Encode a mission_count struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param mission_count C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_count_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_mission_count_t* mission_count) +{ + return mavlink_msg_mission_count_pack_status(system_id, component_id, _status, msg, mission_count->target_system, mission_count->target_component, mission_count->count, mission_count->mission_type, mission_count->opaque_id); } /** @@ -154,10 +246,17 @@ static inline uint16_t mavlink_msg_mission_count_encode_chan(uint8_t system_id, * @param target_component Component ID * @param count Number of mission items in the sequence * @param mission_type Mission type. + * @param opaque_id Id of current on-vehicle mission, fence, or rally point plan (on download from vehicle). + This field is used when downloading a plan from a vehicle to a GCS. + 0 on upload to the vehicle from GCS. + 0 if plan ids are not supported. + The current on-vehicle plan ids are streamed in `MISSION_CURRENT`, allowing a GCS to determine if any part of the plan has changed and needs to be re-uploaded. + The ids are recalculated by the vehicle when any part of the on-vehicle plan changes (when a new plan is uploaded, the vehicle returns the new id to the GCS in MISSION_ACK). + */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count, uint8_t mission_type) +static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count, uint8_t mission_type, uint32_t opaque_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_COUNT_LEN]; @@ -165,6 +264,7 @@ static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_ _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); _mav_put_uint8_t(buf, 4, mission_type); + _mav_put_uint32_t(buf, 5, opaque_id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); #else @@ -173,6 +273,7 @@ static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_ packet.target_system = target_system; packet.target_component = target_component; packet.mission_type = mission_type; + packet.opaque_id = opaque_id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); #endif @@ -186,7 +287,7 @@ static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_ static inline void mavlink_msg_mission_count_send_struct(mavlink_channel_t chan, const mavlink_mission_count_t* mission_count) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_count_send(chan, mission_count->target_system, mission_count->target_component, mission_count->count, mission_count->mission_type); + mavlink_msg_mission_count_send(chan, mission_count->target_system, mission_count->target_component, mission_count->count, mission_count->mission_type, mission_count->opaque_id); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)mission_count, MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); #endif @@ -194,13 +295,13 @@ static inline void mavlink_msg_mission_count_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_MISSION_COUNT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_count_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count, uint8_t mission_type) +static inline void mavlink_msg_mission_count_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count, uint8_t mission_type, uint32_t opaque_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,6 +309,7 @@ static inline void mavlink_msg_mission_count_send_buf(mavlink_message_t *msgbuf, _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); _mav_put_uint8_t(buf, 4, mission_type); + _mav_put_uint32_t(buf, 5, opaque_id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); #else @@ -216,6 +318,7 @@ static inline void mavlink_msg_mission_count_send_buf(mavlink_message_t *msgbuf, packet->target_system = target_system; packet->target_component = target_component; packet->mission_type = mission_type; + packet->opaque_id = opaque_id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)packet, MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); #endif @@ -267,6 +370,22 @@ static inline uint8_t mavlink_msg_mission_count_get_mission_type(const mavlink_m return _MAV_RETURN_uint8_t(msg, 4); } +/** + * @brief Get field opaque_id from mission_count message + * + * @return Id of current on-vehicle mission, fence, or rally point plan (on download from vehicle). + This field is used when downloading a plan from a vehicle to a GCS. + 0 on upload to the vehicle from GCS. + 0 if plan ids are not supported. + The current on-vehicle plan ids are streamed in `MISSION_CURRENT`, allowing a GCS to determine if any part of the plan has changed and needs to be re-uploaded. + The ids are recalculated by the vehicle when any part of the on-vehicle plan changes (when a new plan is uploaded, the vehicle returns the new id to the GCS in MISSION_ACK). + + */ +static inline uint32_t mavlink_msg_mission_count_get_opaque_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 5); +} + /** * @brief Decode a mission_count message into a struct * @@ -280,6 +399,7 @@ static inline void mavlink_msg_mission_count_decode(const mavlink_message_t* msg mission_count->target_system = mavlink_msg_mission_count_get_target_system(msg); mission_count->target_component = mavlink_msg_mission_count_get_target_component(msg); mission_count->mission_type = mavlink_msg_mission_count_get_mission_type(msg); + mission_count->opaque_id = mavlink_msg_mission_count_get_opaque_id(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_COUNT_LEN? msg->len : MAVLINK_MSG_ID_MISSION_COUNT_LEN; memset(mission_count, 0, MAVLINK_MSG_ID_MISSION_COUNT_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_current.h b/lib/main/MAVLink/common/mavlink_msg_mission_current.h index 13f0fb62bc1..e7a402b91df 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_current.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_current.h @@ -3,14 +3,20 @@ #define MAVLINK_MSG_ID_MISSION_CURRENT 42 - +MAVPACKED( typedef struct __mavlink_mission_current_t { uint16_t seq; /*< Sequence*/ -} mavlink_mission_current_t; + uint16_t total; /*< Total number of mission items on vehicle (on last item, sequence == total). If the autopilot stores its home location as part of the mission this will be excluded from the total. 0: Not supported, UINT16_MAX if no mission is present on the vehicle.*/ + uint8_t mission_state; /*< Mission state machine state. MISSION_STATE_UNKNOWN if state reporting not supported.*/ + uint8_t mission_mode; /*< Vehicle is in a mode that can execute mission items or suspended. 0: Unknown, 1: In mission mode, 2: Suspended (not in mission mode).*/ + uint32_t mission_id; /*< Id of current on-vehicle mission plan, or 0 if IDs are not supported or there is no mission loaded. GCS can use this to track changes to the mission plan type. The same value is returned on mission upload (in the MISSION_ACK).*/ + uint32_t fence_id; /*< Id of current on-vehicle fence plan, or 0 if IDs are not supported or there is no fence loaded. GCS can use this to track changes to the fence plan type. The same value is returned on fence upload (in the MISSION_ACK).*/ + uint32_t rally_points_id; /*< Id of current on-vehicle rally point plan, or 0 if IDs are not supported or there are no rally points loaded. GCS can use this to track changes to the rally point plan type. The same value is returned on rally point upload (in the MISSION_ACK).*/ +}) mavlink_mission_current_t; -#define MAVLINK_MSG_ID_MISSION_CURRENT_LEN 2 +#define MAVLINK_MSG_ID_MISSION_CURRENT_LEN 18 #define MAVLINK_MSG_ID_MISSION_CURRENT_MIN_LEN 2 -#define MAVLINK_MSG_ID_42_LEN 2 +#define MAVLINK_MSG_ID_42_LEN 18 #define MAVLINK_MSG_ID_42_MIN_LEN 2 #define MAVLINK_MSG_ID_MISSION_CURRENT_CRC 28 @@ -22,15 +28,27 @@ typedef struct __mavlink_mission_current_t { #define MAVLINK_MESSAGE_INFO_MISSION_CURRENT { \ 42, \ "MISSION_CURRENT", \ - 1, \ + 7, \ { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_current_t, seq) }, \ + { "total", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_mission_current_t, total) }, \ + { "mission_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_current_t, mission_state) }, \ + { "mission_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_current_t, mission_mode) }, \ + { "mission_id", NULL, MAVLINK_TYPE_UINT32_T, 0, 6, offsetof(mavlink_mission_current_t, mission_id) }, \ + { "fence_id", NULL, MAVLINK_TYPE_UINT32_T, 0, 10, offsetof(mavlink_mission_current_t, fence_id) }, \ + { "rally_points_id", NULL, MAVLINK_TYPE_UINT32_T, 0, 14, offsetof(mavlink_mission_current_t, rally_points_id) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_CURRENT { \ "MISSION_CURRENT", \ - 1, \ + 7, \ { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_current_t, seq) }, \ + { "total", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_mission_current_t, total) }, \ + { "mission_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_current_t, mission_state) }, \ + { "mission_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_current_t, mission_mode) }, \ + { "mission_id", NULL, MAVLINK_TYPE_UINT32_T, 0, 6, offsetof(mavlink_mission_current_t, mission_id) }, \ + { "fence_id", NULL, MAVLINK_TYPE_UINT32_T, 0, 10, offsetof(mavlink_mission_current_t, fence_id) }, \ + { "rally_points_id", NULL, MAVLINK_TYPE_UINT32_T, 0, 14, offsetof(mavlink_mission_current_t, rally_points_id) }, \ } \ } #endif @@ -42,19 +60,37 @@ typedef struct __mavlink_mission_current_t { * @param msg The MAVLink message to compress the data into * * @param seq Sequence + * @param total Total number of mission items on vehicle (on last item, sequence == total). If the autopilot stores its home location as part of the mission this will be excluded from the total. 0: Not supported, UINT16_MAX if no mission is present on the vehicle. + * @param mission_state Mission state machine state. MISSION_STATE_UNKNOWN if state reporting not supported. + * @param mission_mode Vehicle is in a mode that can execute mission items or suspended. 0: Unknown, 1: In mission mode, 2: Suspended (not in mission mode). + * @param mission_id Id of current on-vehicle mission plan, or 0 if IDs are not supported or there is no mission loaded. GCS can use this to track changes to the mission plan type. The same value is returned on mission upload (in the MISSION_ACK). + * @param fence_id Id of current on-vehicle fence plan, or 0 if IDs are not supported or there is no fence loaded. GCS can use this to track changes to the fence plan type. The same value is returned on fence upload (in the MISSION_ACK). + * @param rally_points_id Id of current on-vehicle rally point plan, or 0 if IDs are not supported or there are no rally points loaded. GCS can use this to track changes to the rally point plan type. The same value is returned on rally point upload (in the MISSION_ACK). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t seq) + uint16_t seq, uint16_t total, uint8_t mission_state, uint8_t mission_mode, uint32_t mission_id, uint32_t fence_id, uint32_t rally_points_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN]; _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint16_t(buf, 2, total); + _mav_put_uint8_t(buf, 4, mission_state); + _mav_put_uint8_t(buf, 5, mission_mode); + _mav_put_uint32_t(buf, 6, mission_id); + _mav_put_uint32_t(buf, 10, fence_id); + _mav_put_uint32_t(buf, 14, rally_points_id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); #else mavlink_mission_current_t packet; packet.seq = seq; + packet.total = total; + packet.mission_state = mission_state; + packet.mission_mode = mission_mode; + packet.mission_id = mission_id; + packet.fence_id = fence_id; + packet.rally_points_id = rally_points_id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); #endif @@ -63,6 +99,57 @@ static inline uint16_t mavlink_msg_mission_current_pack(uint8_t system_id, uint8 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_CURRENT_MIN_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); } +/** + * @brief Pack a mission_current message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param seq Sequence + * @param total Total number of mission items on vehicle (on last item, sequence == total). If the autopilot stores its home location as part of the mission this will be excluded from the total. 0: Not supported, UINT16_MAX if no mission is present on the vehicle. + * @param mission_state Mission state machine state. MISSION_STATE_UNKNOWN if state reporting not supported. + * @param mission_mode Vehicle is in a mode that can execute mission items or suspended. 0: Unknown, 1: In mission mode, 2: Suspended (not in mission mode). + * @param mission_id Id of current on-vehicle mission plan, or 0 if IDs are not supported or there is no mission loaded. GCS can use this to track changes to the mission plan type. The same value is returned on mission upload (in the MISSION_ACK). + * @param fence_id Id of current on-vehicle fence plan, or 0 if IDs are not supported or there is no fence loaded. GCS can use this to track changes to the fence plan type. The same value is returned on fence upload (in the MISSION_ACK). + * @param rally_points_id Id of current on-vehicle rally point plan, or 0 if IDs are not supported or there are no rally points loaded. GCS can use this to track changes to the rally point plan type. The same value is returned on rally point upload (in the MISSION_ACK). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_current_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint16_t seq, uint16_t total, uint8_t mission_state, uint8_t mission_mode, uint32_t mission_id, uint32_t fence_id, uint32_t rally_points_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN]; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint16_t(buf, 2, total); + _mav_put_uint8_t(buf, 4, mission_state); + _mav_put_uint8_t(buf, 5, mission_mode); + _mav_put_uint32_t(buf, 6, mission_id); + _mav_put_uint32_t(buf, 10, fence_id); + _mav_put_uint32_t(buf, 14, rally_points_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#else + mavlink_mission_current_t packet; + packet.seq = seq; + packet.total = total; + packet.mission_state = mission_state; + packet.mission_mode = mission_mode; + packet.mission_id = mission_id; + packet.fence_id = fence_id; + packet.rally_points_id = rally_points_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_CURRENT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_CURRENT_MIN_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_CURRENT_MIN_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#endif +} + /** * @brief Pack a mission_current message on a channel * @param system_id ID of this system @@ -70,20 +157,38 @@ static inline uint16_t mavlink_msg_mission_current_pack(uint8_t system_id, uint8 * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param seq Sequence + * @param total Total number of mission items on vehicle (on last item, sequence == total). If the autopilot stores its home location as part of the mission this will be excluded from the total. 0: Not supported, UINT16_MAX if no mission is present on the vehicle. + * @param mission_state Mission state machine state. MISSION_STATE_UNKNOWN if state reporting not supported. + * @param mission_mode Vehicle is in a mode that can execute mission items or suspended. 0: Unknown, 1: In mission mode, 2: Suspended (not in mission mode). + * @param mission_id Id of current on-vehicle mission plan, or 0 if IDs are not supported or there is no mission loaded. GCS can use this to track changes to the mission plan type. The same value is returned on mission upload (in the MISSION_ACK). + * @param fence_id Id of current on-vehicle fence plan, or 0 if IDs are not supported or there is no fence loaded. GCS can use this to track changes to the fence plan type. The same value is returned on fence upload (in the MISSION_ACK). + * @param rally_points_id Id of current on-vehicle rally point plan, or 0 if IDs are not supported or there are no rally points loaded. GCS can use this to track changes to the rally point plan type. The same value is returned on rally point upload (in the MISSION_ACK). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_current_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint16_t seq) + uint16_t seq,uint16_t total,uint8_t mission_state,uint8_t mission_mode,uint32_t mission_id,uint32_t fence_id,uint32_t rally_points_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN]; _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint16_t(buf, 2, total); + _mav_put_uint8_t(buf, 4, mission_state); + _mav_put_uint8_t(buf, 5, mission_mode); + _mav_put_uint32_t(buf, 6, mission_id); + _mav_put_uint32_t(buf, 10, fence_id); + _mav_put_uint32_t(buf, 14, rally_points_id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); #else mavlink_mission_current_t packet; packet.seq = seq; + packet.total = total; + packet.mission_state = mission_state; + packet.mission_mode = mission_mode; + packet.mission_id = mission_id; + packet.fence_id = fence_id; + packet.rally_points_id = rally_points_id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); #endif @@ -102,7 +207,7 @@ static inline uint16_t mavlink_msg_mission_current_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_mission_current_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_current_t* mission_current) { - return mavlink_msg_mission_current_pack(system_id, component_id, msg, mission_current->seq); + return mavlink_msg_mission_current_pack(system_id, component_id, msg, mission_current->seq, mission_current->total, mission_current->mission_state, mission_current->mission_mode, mission_current->mission_id, mission_current->fence_id, mission_current->rally_points_id); } /** @@ -116,7 +221,21 @@ static inline uint16_t mavlink_msg_mission_current_encode(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_mission_current_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_current_t* mission_current) { - return mavlink_msg_mission_current_pack_chan(system_id, component_id, chan, msg, mission_current->seq); + return mavlink_msg_mission_current_pack_chan(system_id, component_id, chan, msg, mission_current->seq, mission_current->total, mission_current->mission_state, mission_current->mission_mode, mission_current->mission_id, mission_current->fence_id, mission_current->rally_points_id); +} + +/** + * @brief Encode a mission_current struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param mission_current C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_current_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_mission_current_t* mission_current) +{ + return mavlink_msg_mission_current_pack_status(system_id, component_id, _status, msg, mission_current->seq, mission_current->total, mission_current->mission_state, mission_current->mission_mode, mission_current->mission_id, mission_current->fence_id, mission_current->rally_points_id); } /** @@ -124,19 +243,37 @@ static inline uint16_t mavlink_msg_mission_current_encode_chan(uint8_t system_id * @param chan MAVLink channel to send the message * * @param seq Sequence + * @param total Total number of mission items on vehicle (on last item, sequence == total). If the autopilot stores its home location as part of the mission this will be excluded from the total. 0: Not supported, UINT16_MAX if no mission is present on the vehicle. + * @param mission_state Mission state machine state. MISSION_STATE_UNKNOWN if state reporting not supported. + * @param mission_mode Vehicle is in a mode that can execute mission items or suspended. 0: Unknown, 1: In mission mode, 2: Suspended (not in mission mode). + * @param mission_id Id of current on-vehicle mission plan, or 0 if IDs are not supported or there is no mission loaded. GCS can use this to track changes to the mission plan type. The same value is returned on mission upload (in the MISSION_ACK). + * @param fence_id Id of current on-vehicle fence plan, or 0 if IDs are not supported or there is no fence loaded. GCS can use this to track changes to the fence plan type. The same value is returned on fence upload (in the MISSION_ACK). + * @param rally_points_id Id of current on-vehicle rally point plan, or 0 if IDs are not supported or there are no rally points loaded. GCS can use this to track changes to the rally point plan type. The same value is returned on rally point upload (in the MISSION_ACK). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_current_send(mavlink_channel_t chan, uint16_t seq) +static inline void mavlink_msg_mission_current_send(mavlink_channel_t chan, uint16_t seq, uint16_t total, uint8_t mission_state, uint8_t mission_mode, uint32_t mission_id, uint32_t fence_id, uint32_t rally_points_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_CURRENT_LEN]; _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint16_t(buf, 2, total); + _mav_put_uint8_t(buf, 4, mission_state); + _mav_put_uint8_t(buf, 5, mission_mode); + _mav_put_uint32_t(buf, 6, mission_id); + _mav_put_uint32_t(buf, 10, fence_id); + _mav_put_uint32_t(buf, 14, rally_points_id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_MIN_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); #else mavlink_mission_current_t packet; packet.seq = seq; + packet.total = total; + packet.mission_state = mission_state; + packet.mission_mode = mission_mode; + packet.mission_id = mission_id; + packet.fence_id = fence_id; + packet.rally_points_id = rally_points_id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_CURRENT_MIN_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); #endif @@ -150,7 +287,7 @@ static inline void mavlink_msg_mission_current_send(mavlink_channel_t chan, uint static inline void mavlink_msg_mission_current_send_struct(mavlink_channel_t chan, const mavlink_mission_current_t* mission_current) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_current_send(chan, mission_current->seq); + mavlink_msg_mission_current_send(chan, mission_current->seq, mission_current->total, mission_current->mission_state, mission_current->mission_mode, mission_current->mission_id, mission_current->fence_id, mission_current->rally_points_id); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)mission_current, MAVLINK_MSG_ID_MISSION_CURRENT_MIN_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); #endif @@ -158,22 +295,34 @@ static inline void mavlink_msg_mission_current_send_struct(mavlink_channel_t cha #if MAVLINK_MSG_ID_MISSION_CURRENT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_current_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seq) +static inline void mavlink_msg_mission_current_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seq, uint16_t total, uint8_t mission_state, uint8_t mission_mode, uint32_t mission_id, uint32_t fence_id, uint32_t rally_points_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint16_t(buf, 2, total); + _mav_put_uint8_t(buf, 4, mission_state); + _mav_put_uint8_t(buf, 5, mission_mode); + _mav_put_uint32_t(buf, 6, mission_id); + _mav_put_uint32_t(buf, 10, fence_id); + _mav_put_uint32_t(buf, 14, rally_points_id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_MIN_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); #else mavlink_mission_current_t *packet = (mavlink_mission_current_t *)msgbuf; packet->seq = seq; + packet->total = total; + packet->mission_state = mission_state; + packet->mission_mode = mission_mode; + packet->mission_id = mission_id; + packet->fence_id = fence_id; + packet->rally_points_id = rally_points_id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_CURRENT_MIN_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); #endif @@ -195,6 +344,66 @@ static inline uint16_t mavlink_msg_mission_current_get_seq(const mavlink_message return _MAV_RETURN_uint16_t(msg, 0); } +/** + * @brief Get field total from mission_current message + * + * @return Total number of mission items on vehicle (on last item, sequence == total). If the autopilot stores its home location as part of the mission this will be excluded from the total. 0: Not supported, UINT16_MAX if no mission is present on the vehicle. + */ +static inline uint16_t mavlink_msg_mission_current_get_total(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field mission_state from mission_current message + * + * @return Mission state machine state. MISSION_STATE_UNKNOWN if state reporting not supported. + */ +static inline uint8_t mavlink_msg_mission_current_get_mission_state(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field mission_mode from mission_current message + * + * @return Vehicle is in a mode that can execute mission items or suspended. 0: Unknown, 1: In mission mode, 2: Suspended (not in mission mode). + */ +static inline uint8_t mavlink_msg_mission_current_get_mission_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field mission_id from mission_current message + * + * @return Id of current on-vehicle mission plan, or 0 if IDs are not supported or there is no mission loaded. GCS can use this to track changes to the mission plan type. The same value is returned on mission upload (in the MISSION_ACK). + */ +static inline uint32_t mavlink_msg_mission_current_get_mission_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 6); +} + +/** + * @brief Get field fence_id from mission_current message + * + * @return Id of current on-vehicle fence plan, or 0 if IDs are not supported or there is no fence loaded. GCS can use this to track changes to the fence plan type. The same value is returned on fence upload (in the MISSION_ACK). + */ +static inline uint32_t mavlink_msg_mission_current_get_fence_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 10); +} + +/** + * @brief Get field rally_points_id from mission_current message + * + * @return Id of current on-vehicle rally point plan, or 0 if IDs are not supported or there are no rally points loaded. GCS can use this to track changes to the rally point plan type. The same value is returned on rally point upload (in the MISSION_ACK). + */ +static inline uint32_t mavlink_msg_mission_current_get_rally_points_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 14); +} + /** * @brief Decode a mission_current message into a struct * @@ -205,6 +414,12 @@ static inline void mavlink_msg_mission_current_decode(const mavlink_message_t* m { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS mission_current->seq = mavlink_msg_mission_current_get_seq(msg); + mission_current->total = mavlink_msg_mission_current_get_total(msg); + mission_current->mission_state = mavlink_msg_mission_current_get_mission_state(msg); + mission_current->mission_mode = mavlink_msg_mission_current_get_mission_mode(msg); + mission_current->mission_id = mavlink_msg_mission_current_get_mission_id(msg); + mission_current->fence_id = mavlink_msg_mission_current_get_fence_id(msg); + mission_current->rally_points_id = mavlink_msg_mission_current_get_rally_points_id(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_CURRENT_LEN? msg->len : MAVLINK_MSG_ID_MISSION_CURRENT_LEN; memset(mission_current, 0, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_item.h b/lib/main/MAVLink/common/mavlink_msg_mission_item.h index b14d151b287..55cabb1c3d1 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_item.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_item.h @@ -18,7 +18,7 @@ typedef struct __mavlink_mission_item_t { uint8_t target_component; /*< Component ID*/ uint8_t frame; /*< The coordinate system of the waypoint.*/ uint8_t current; /*< false:0, true:1*/ - uint8_t autocontinue; /*< Autocontinue to next waypoint*/ + uint8_t autocontinue; /*< Autocontinue to next waypoint. 0: false, 1: true. Set false to pause mission after the item completes.*/ uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_item_t; @@ -89,7 +89,7 @@ typedef struct __mavlink_mission_item_t { * @param frame The coordinate system of the waypoint. * @param command The scheduled action for the waypoint. * @param current false:0, true:1 - * @param autocontinue Autocontinue to next waypoint + * @param autocontinue Autocontinue to next waypoint. 0: false, 1: true. Set false to pause mission after the item completes. * @param param1 PARAM1, see MAV_CMD enum * @param param2 PARAM2, see MAV_CMD enum * @param param3 PARAM3, see MAV_CMD enum @@ -147,6 +147,81 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); } +/** + * @brief Pack a mission_item message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param seq Sequence + * @param frame The coordinate system of the waypoint. + * @param command The scheduled action for the waypoint. + * @param current false:0, true:1 + * @param autocontinue Autocontinue to next waypoint. 0: false, 1: true. Set false to pause mission after the item completes. + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum + * @param x PARAM5 / local: X coordinate, global: latitude + * @param y PARAM6 / local: Y coordinate, global: longitude + * @param z PARAM7 / local: Z coordinate, global: altitude (relative or absolute, depending on frame). + * @param mission_type Mission type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_item_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z, uint8_t mission_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ITEM_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, x); + _mav_put_float(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint16_t(buf, 30, command); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, frame); + _mav_put_uint8_t(buf, 35, current); + _mav_put_uint8_t(buf, 36, autocontinue); + _mav_put_uint8_t(buf, 37, mission_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#else + mavlink_mission_item_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.seq = seq; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.current = current; + packet.autocontinue = autocontinue; + packet.mission_type = mission_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#endif +} + /** * @brief Pack a mission_item message on a channel * @param system_id ID of this system @@ -159,7 +234,7 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t * @param frame The coordinate system of the waypoint. * @param command The scheduled action for the waypoint. * @param current false:0, true:1 - * @param autocontinue Autocontinue to next waypoint + * @param autocontinue Autocontinue to next waypoint. 0: false, 1: true. Set false to pause mission after the item completes. * @param param1 PARAM1, see MAV_CMD enum * @param param2 PARAM2, see MAV_CMD enum * @param param3 PARAM3, see MAV_CMD enum @@ -245,6 +320,20 @@ static inline uint16_t mavlink_msg_mission_item_encode_chan(uint8_t system_id, u return mavlink_msg_mission_item_pack_chan(system_id, component_id, chan, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z, mission_item->mission_type); } +/** + * @brief Encode a mission_item struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param mission_item C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_item_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_mission_item_t* mission_item) +{ + return mavlink_msg_mission_item_pack_status(system_id, component_id, _status, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z, mission_item->mission_type); +} + /** * @brief Send a mission_item message * @param chan MAVLink channel to send the message @@ -255,7 +344,7 @@ static inline uint16_t mavlink_msg_mission_item_encode_chan(uint8_t system_id, u * @param frame The coordinate system of the waypoint. * @param command The scheduled action for the waypoint. * @param current false:0, true:1 - * @param autocontinue Autocontinue to next waypoint + * @param autocontinue Autocontinue to next waypoint. 0: false, 1: true. Set false to pause mission after the item completes. * @param param1 PARAM1, see MAV_CMD enum * @param param2 PARAM2, see MAV_CMD enum * @param param3 PARAM3, see MAV_CMD enum @@ -326,7 +415,7 @@ static inline void mavlink_msg_mission_item_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_MISSION_ITEM_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -444,7 +533,7 @@ static inline uint8_t mavlink_msg_mission_item_get_current(const mavlink_message /** * @brief Get field autocontinue from mission_item message * - * @return Autocontinue to next waypoint + * @return Autocontinue to next waypoint. 0: false, 1: true. Set false to pause mission after the item completes. */ static inline uint8_t mavlink_msg_mission_item_get_autocontinue(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_item_int.h b/lib/main/MAVLink/common/mavlink_msg_mission_item_int.h index 74ad3a5ed53..eb9b423fc3f 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_item_int.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_item_int.h @@ -18,7 +18,7 @@ typedef struct __mavlink_mission_item_int_t { uint8_t target_component; /*< Component ID*/ uint8_t frame; /*< The coordinate system of the waypoint.*/ uint8_t current; /*< false:0, true:1*/ - uint8_t autocontinue; /*< Autocontinue to next waypoint*/ + uint8_t autocontinue; /*< Autocontinue to next waypoint. 0: false, 1: true. Set false to pause mission after the item completes.*/ uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_item_int_t; @@ -89,7 +89,7 @@ typedef struct __mavlink_mission_item_int_t { * @param frame The coordinate system of the waypoint. * @param command The scheduled action for the waypoint. * @param current false:0, true:1 - * @param autocontinue Autocontinue to next waypoint + * @param autocontinue Autocontinue to next waypoint. 0: false, 1: true. Set false to pause mission after the item completes. * @param param1 PARAM1, see MAV_CMD enum * @param param2 PARAM2, see MAV_CMD enum * @param param3 PARAM3, see MAV_CMD enum @@ -147,6 +147,81 @@ static inline uint16_t mavlink_msg_mission_item_int_pack(uint8_t system_id, uint return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); } +/** + * @brief Pack a mission_item_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param seq Waypoint ID (sequence number). Starts at zero. Increases monotonically for each waypoint, no gaps in the sequence (0,1,2,3,4). + * @param frame The coordinate system of the waypoint. + * @param command The scheduled action for the waypoint. + * @param current false:0, true:1 + * @param autocontinue Autocontinue to next waypoint. 0: false, 1: true. Set false to pause mission after the item completes. + * @param param1 PARAM1, see MAV_CMD enum + * @param param2 PARAM2, see MAV_CMD enum + * @param param3 PARAM3, see MAV_CMD enum + * @param param4 PARAM4, see MAV_CMD enum + * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + * @param y PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 + * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + * @param mission_type Mission type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_item_int_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z, uint8_t mission_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_int32_t(buf, 16, x); + _mav_put_int32_t(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint16_t(buf, 30, command); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, frame); + _mav_put_uint8_t(buf, 35, current); + _mav_put_uint8_t(buf, 36, autocontinue); + _mav_put_uint8_t(buf, 37, mission_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#else + mavlink_mission_item_int_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.seq = seq; + packet.command = command; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.current = current; + packet.autocontinue = autocontinue; + packet.mission_type = mission_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_ITEM_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_ITEM_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); +#endif +} + /** * @brief Pack a mission_item_int message on a channel * @param system_id ID of this system @@ -159,7 +234,7 @@ static inline uint16_t mavlink_msg_mission_item_int_pack(uint8_t system_id, uint * @param frame The coordinate system of the waypoint. * @param command The scheduled action for the waypoint. * @param current false:0, true:1 - * @param autocontinue Autocontinue to next waypoint + * @param autocontinue Autocontinue to next waypoint. 0: false, 1: true. Set false to pause mission after the item completes. * @param param1 PARAM1, see MAV_CMD enum * @param param2 PARAM2, see MAV_CMD enum * @param param3 PARAM3, see MAV_CMD enum @@ -245,6 +320,20 @@ static inline uint16_t mavlink_msg_mission_item_int_encode_chan(uint8_t system_i return mavlink_msg_mission_item_int_pack_chan(system_id, component_id, chan, msg, mission_item_int->target_system, mission_item_int->target_component, mission_item_int->seq, mission_item_int->frame, mission_item_int->command, mission_item_int->current, mission_item_int->autocontinue, mission_item_int->param1, mission_item_int->param2, mission_item_int->param3, mission_item_int->param4, mission_item_int->x, mission_item_int->y, mission_item_int->z, mission_item_int->mission_type); } +/** + * @brief Encode a mission_item_int struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param mission_item_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_item_int_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_mission_item_int_t* mission_item_int) +{ + return mavlink_msg_mission_item_int_pack_status(system_id, component_id, _status, msg, mission_item_int->target_system, mission_item_int->target_component, mission_item_int->seq, mission_item_int->frame, mission_item_int->command, mission_item_int->current, mission_item_int->autocontinue, mission_item_int->param1, mission_item_int->param2, mission_item_int->param3, mission_item_int->param4, mission_item_int->x, mission_item_int->y, mission_item_int->z, mission_item_int->mission_type); +} + /** * @brief Send a mission_item_int message * @param chan MAVLink channel to send the message @@ -255,7 +344,7 @@ static inline uint16_t mavlink_msg_mission_item_int_encode_chan(uint8_t system_i * @param frame The coordinate system of the waypoint. * @param command The scheduled action for the waypoint. * @param current false:0, true:1 - * @param autocontinue Autocontinue to next waypoint + * @param autocontinue Autocontinue to next waypoint. 0: false, 1: true. Set false to pause mission after the item completes. * @param param1 PARAM1, see MAV_CMD enum * @param param2 PARAM2, see MAV_CMD enum * @param param3 PARAM3, see MAV_CMD enum @@ -326,7 +415,7 @@ static inline void mavlink_msg_mission_item_int_send_struct(mavlink_channel_t ch #if MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -444,7 +533,7 @@ static inline uint8_t mavlink_msg_mission_item_int_get_current(const mavlink_mes /** * @brief Get field autocontinue from mission_item_int message * - * @return Autocontinue to next waypoint + * @return Autocontinue to next waypoint. 0: false, 1: true. Set false to pause mission after the item completes. */ static inline uint8_t mavlink_msg_mission_item_int_get_autocontinue(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_item_reached.h b/lib/main/MAVLink/common/mavlink_msg_mission_item_reached.h index 647176b9cd8..cf7090ab877 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_item_reached.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_item_reached.h @@ -63,6 +63,39 @@ static inline uint16_t mavlink_msg_mission_item_reached_pack(uint8_t system_id, return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); } +/** + * @brief Pack a mission_item_reached message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param seq Sequence + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_item_reached_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN]; + _mav_put_uint16_t(buf, 0, seq); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#else + mavlink_mission_item_reached_t packet; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_ITEM_REACHED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#endif +} + /** * @brief Pack a mission_item_reached message on a channel * @param system_id ID of this system @@ -119,6 +152,20 @@ static inline uint16_t mavlink_msg_mission_item_reached_encode_chan(uint8_t syst return mavlink_msg_mission_item_reached_pack_chan(system_id, component_id, chan, msg, mission_item_reached->seq); } +/** + * @brief Encode a mission_item_reached struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param mission_item_reached C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_item_reached_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_mission_item_reached_t* mission_item_reached) +{ + return mavlink_msg_mission_item_reached_pack_status(system_id, component_id, _status, msg, mission_item_reached->seq); +} + /** * @brief Send a mission_item_reached message * @param chan MAVLink channel to send the message @@ -158,7 +205,7 @@ static inline void mavlink_msg_mission_item_reached_send_struct(mavlink_channel_ #if MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_request.h b/lib/main/MAVLink/common/mavlink_msg_mission_request.h index 4f8c0fe77c3..323e66f6432 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_request.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_request.h @@ -81,6 +81,48 @@ static inline uint16_t mavlink_msg_mission_request_pack(uint8_t system_id, uint8 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); } +/** + * @brief Pack a mission_request message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param seq Sequence + * @param mission_type Mission type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_request_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t mission_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LEN]; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#else + mavlink_mission_request_t packet; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mission_type = mission_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_REQUEST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_REQUEST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#endif +} + /** * @brief Pack a mission_request message on a channel * @param system_id ID of this system @@ -146,6 +188,20 @@ static inline uint16_t mavlink_msg_mission_request_encode_chan(uint8_t system_id return mavlink_msg_mission_request_pack_chan(system_id, component_id, chan, msg, mission_request->target_system, mission_request->target_component, mission_request->seq, mission_request->mission_type); } +/** + * @brief Encode a mission_request struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param mission_request C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_mission_request_t* mission_request) +{ + return mavlink_msg_mission_request_pack_status(system_id, component_id, _status, msg, mission_request->target_system, mission_request->target_component, mission_request->seq, mission_request->mission_type); +} + /** * @brief Send a mission_request message * @param chan MAVLink channel to send the message @@ -194,7 +250,7 @@ static inline void mavlink_msg_mission_request_send_struct(mavlink_channel_t cha #if MAVLINK_MSG_ID_MISSION_REQUEST_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_request_int.h b/lib/main/MAVLink/common/mavlink_msg_mission_request_int.h index c131f78f7b4..70da4590e4c 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_request_int.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_request_int.h @@ -81,6 +81,48 @@ static inline uint16_t mavlink_msg_mission_request_int_pack(uint8_t system_id, u return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_CRC); } +/** + * @brief Pack a mission_request_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param seq Sequence + * @param mission_type Mission type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_request_int_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t mission_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN]; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN); +#else + mavlink_mission_request_int_t packet; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mission_type = mission_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_REQUEST_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_REQUEST_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN); +#endif +} + /** * @brief Pack a mission_request_int message on a channel * @param system_id ID of this system @@ -146,6 +188,20 @@ static inline uint16_t mavlink_msg_mission_request_int_encode_chan(uint8_t syste return mavlink_msg_mission_request_int_pack_chan(system_id, component_id, chan, msg, mission_request_int->target_system, mission_request_int->target_component, mission_request_int->seq, mission_request_int->mission_type); } +/** + * @brief Encode a mission_request_int struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param mission_request_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_int_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_mission_request_int_t* mission_request_int) +{ + return mavlink_msg_mission_request_int_pack_status(system_id, component_id, _status, msg, mission_request_int->target_system, mission_request_int->target_component, mission_request_int->seq, mission_request_int->mission_type); +} + /** * @brief Send a mission_request_int message * @param chan MAVLink channel to send the message @@ -194,7 +250,7 @@ static inline void mavlink_msg_mission_request_int_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_request_list.h b/lib/main/MAVLink/common/mavlink_msg_mission_request_list.h index a1633f43f41..69885c89b43 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_request_list.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_request_list.h @@ -75,6 +75,45 @@ static inline uint16_t mavlink_msg_mission_request_list_pack(uint8_t system_id, return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); } +/** + * @brief Pack a mission_request_list message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param mission_type Mission type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_request_list_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t mission_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mission_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#else + mavlink_mission_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mission_type = mission_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_LIST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#endif +} + /** * @brief Pack a mission_request_list message on a channel * @param system_id ID of this system @@ -137,6 +176,20 @@ static inline uint16_t mavlink_msg_mission_request_list_encode_chan(uint8_t syst return mavlink_msg_mission_request_list_pack_chan(system_id, component_id, chan, msg, mission_request_list->target_system, mission_request_list->target_component, mission_request_list->mission_type); } +/** + * @brief Encode a mission_request_list struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param mission_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_list_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_mission_request_list_t* mission_request_list) +{ + return mavlink_msg_mission_request_list_pack_status(system_id, component_id, _status, msg, mission_request_list->target_system, mission_request_list->target_component, mission_request_list->mission_type); +} + /** * @brief Send a mission_request_list message * @param chan MAVLink channel to send the message @@ -182,7 +235,7 @@ static inline void mavlink_msg_mission_request_list_send_struct(mavlink_channel_ #if MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_request_partial_list.h b/lib/main/MAVLink/common/mavlink_msg_mission_request_partial_list.h index 93906934b90..7f5f102ed53 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_request_partial_list.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_request_partial_list.h @@ -87,6 +87,51 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack(uint8_t sys return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); } +/** + * @brief Pack a mission_request_partial_list message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param start_index Start index + * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list + * @param mission_type Mission type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_request_partial_list_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index, uint8_t mission_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN]; + _mav_put_int16_t(buf, 0, start_index); + _mav_put_int16_t(buf, 2, end_index); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, mission_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#else + mavlink_mission_request_partial_list_t packet; + packet.start_index = start_index; + packet.end_index = end_index; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mission_type = mission_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#endif +} + /** * @brief Pack a mission_request_partial_list message on a channel * @param system_id ID of this system @@ -155,6 +200,20 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_encode_chan(uint return mavlink_msg_mission_request_partial_list_pack_chan(system_id, component_id, chan, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index, mission_request_partial_list->mission_type); } +/** + * @brief Encode a mission_request_partial_list struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param mission_request_partial_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_partial_list_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_mission_request_partial_list_t* mission_request_partial_list) +{ + return mavlink_msg_mission_request_partial_list_pack_status(system_id, component_id, _status, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index, mission_request_partial_list->mission_type); +} + /** * @brief Send a mission_request_partial_list message * @param chan MAVLink channel to send the message @@ -206,7 +265,7 @@ static inline void mavlink_msg_mission_request_partial_list_send_struct(mavlink_ #if MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_set_current.h b/lib/main/MAVLink/common/mavlink_msg_mission_set_current.h index 41eeba3f584..e92d98e39c1 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_set_current.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_set_current.h @@ -75,6 +75,45 @@ static inline uint16_t mavlink_msg_mission_set_current_pack(uint8_t system_id, u return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_SET_CURRENT_MIN_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); } +/** + * @brief Pack a mission_set_current message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param seq Sequence + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_set_current_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN]; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#else + mavlink_mission_set_current_t packet; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_SET_CURRENT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_SET_CURRENT_MIN_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_SET_CURRENT_MIN_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#endif +} + /** * @brief Pack a mission_set_current message on a channel * @param system_id ID of this system @@ -137,6 +176,20 @@ static inline uint16_t mavlink_msg_mission_set_current_encode_chan(uint8_t syste return mavlink_msg_mission_set_current_pack_chan(system_id, component_id, chan, msg, mission_set_current->target_system, mission_set_current->target_component, mission_set_current->seq); } +/** + * @brief Encode a mission_set_current struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param mission_set_current C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_set_current_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_mission_set_current_t* mission_set_current) +{ + return mavlink_msg_mission_set_current_pack_status(system_id, component_id, _status, msg, mission_set_current->target_system, mission_set_current->target_component, mission_set_current->seq); +} + /** * @brief Send a mission_set_current message * @param chan MAVLink channel to send the message @@ -182,7 +235,7 @@ static inline void mavlink_msg_mission_set_current_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_write_partial_list.h b/lib/main/MAVLink/common/mavlink_msg_mission_write_partial_list.h index 9f8bd08f8c0..4df36c627be 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_write_partial_list.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_write_partial_list.h @@ -87,6 +87,51 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack(uint8_t syste return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); } +/** + * @brief Pack a mission_write_partial_list message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param start_index Start index. Must be smaller / equal to the largest index of the current onboard list. + * @param end_index End index, equal or greater than start index. + * @param mission_type Mission type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mission_write_partial_list_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index, uint8_t mission_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN]; + _mav_put_int16_t(buf, 0, start_index); + _mav_put_int16_t(buf, 2, end_index); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, mission_type); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#else + mavlink_mission_write_partial_list_t packet; + packet.start_index = start_index; + packet.end_index = end_index; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mission_type = mission_type; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#endif +} + /** * @brief Pack a mission_write_partial_list message on a channel * @param system_id ID of this system @@ -155,6 +200,20 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_encode_chan(uint8_ return mavlink_msg_mission_write_partial_list_pack_chan(system_id, component_id, chan, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index, mission_write_partial_list->mission_type); } +/** + * @brief Encode a mission_write_partial_list struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param mission_write_partial_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_write_partial_list_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_mission_write_partial_list_t* mission_write_partial_list) +{ + return mavlink_msg_mission_write_partial_list_pack_status(system_id, component_id, _status, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index, mission_write_partial_list->mission_type); +} + /** * @brief Send a mission_write_partial_list message * @param chan MAVLink channel to send the message @@ -206,7 +265,7 @@ static inline void mavlink_msg_mission_write_partial_list_send_struct(mavlink_ch #if MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_mount_orientation.h b/lib/main/MAVLink/common/mavlink_msg_mount_orientation.h index 96bfda25015..474e478026f 100644 --- a/lib/main/MAVLink/common/mavlink_msg_mount_orientation.h +++ b/lib/main/MAVLink/common/mavlink_msg_mount_orientation.h @@ -87,6 +87,51 @@ static inline uint16_t mavlink_msg_mount_orientation_pack(uint8_t system_id, uin return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_CRC); } +/** + * @brief Pack a mount_orientation message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param roll [deg] Roll in global frame (set to NaN for invalid). + * @param pitch [deg] Pitch in global frame (set to NaN for invalid). + * @param yaw [deg] Yaw relative to vehicle (set to NaN for invalid). + * @param yaw_absolute [deg] Yaw in absolute frame relative to Earth's North, north is 0 (set to NaN for invalid). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mount_orientation_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, float roll, float pitch, float yaw, float yaw_absolute) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, yaw_absolute); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN); +#else + mavlink_mount_orientation_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.yaw_absolute = yaw_absolute; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_ORIENTATION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN); +#endif +} + /** * @brief Pack a mount_orientation message on a channel * @param system_id ID of this system @@ -155,6 +200,20 @@ static inline uint16_t mavlink_msg_mount_orientation_encode_chan(uint8_t system_ return mavlink_msg_mount_orientation_pack_chan(system_id, component_id, chan, msg, mount_orientation->time_boot_ms, mount_orientation->roll, mount_orientation->pitch, mount_orientation->yaw, mount_orientation->yaw_absolute); } +/** + * @brief Encode a mount_orientation struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param mount_orientation C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_orientation_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_mount_orientation_t* mount_orientation) +{ + return mavlink_msg_mount_orientation_pack_status(system_id, component_id, _status, msg, mount_orientation->time_boot_ms, mount_orientation->roll, mount_orientation->pitch, mount_orientation->yaw, mount_orientation->yaw_absolute); +} + /** * @brief Send a mount_orientation message * @param chan MAVLink channel to send the message @@ -206,7 +265,7 @@ static inline void mavlink_msg_mount_orientation_send_struct(mavlink_channel_t c #if MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_named_value_float.h b/lib/main/MAVLink/common/mavlink_msg_named_value_float.h index ac3e8382ffc..0ded95fcb47 100755 --- a/lib/main/MAVLink/common/mavlink_msg_named_value_float.h +++ b/lib/main/MAVLink/common/mavlink_msg_named_value_float.h @@ -65,7 +65,7 @@ static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uin mavlink_named_value_float_t packet; packet.time_boot_ms = time_boot_ms; packet.value = value; - mav_array_memcpy(packet.name, name, sizeof(char)*10); + mav_array_assign_char(packet.name, name, 10); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); #endif @@ -73,6 +73,43 @@ static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uin return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_MIN_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); } +/** + * @brief Pack a named_value_float message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param name Name of the debug variable + * @param value Floating point value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_named_value_float_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, const char *name, float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#else + mavlink_named_value_float_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_MIN_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_MIN_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#endif +} + /** * @brief Pack a named_value_float message on a channel * @param system_id ID of this system @@ -98,7 +135,7 @@ static inline uint16_t mavlink_msg_named_value_float_pack_chan(uint8_t system_id mavlink_named_value_float_t packet; packet.time_boot_ms = time_boot_ms; packet.value = value; - mav_array_memcpy(packet.name, name, sizeof(char)*10); + mav_array_assign_char(packet.name, name, 10); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); #endif @@ -133,6 +170,20 @@ static inline uint16_t mavlink_msg_named_value_float_encode_chan(uint8_t system_ return mavlink_msg_named_value_float_pack_chan(system_id, component_id, chan, msg, named_value_float->time_boot_ms, named_value_float->name, named_value_float->value); } +/** + * @brief Encode a named_value_float struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param named_value_float C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_named_value_float_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_named_value_float_t* named_value_float) +{ + return mavlink_msg_named_value_float_pack_status(system_id, component_id, _status, msg, named_value_float->time_boot_ms, named_value_float->name, named_value_float->value); +} + /** * @brief Send a named_value_float message * @param chan MAVLink channel to send the message @@ -155,7 +206,7 @@ static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, ui mavlink_named_value_float_t packet; packet.time_boot_ms = time_boot_ms; packet.value = value; - mav_array_memcpy(packet.name, name, sizeof(char)*10); + mav_array_assign_char(packet.name, name, 10); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)&packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_MIN_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); #endif } @@ -176,7 +227,7 @@ static inline void mavlink_msg_named_value_float_send_struct(mavlink_channel_t c #if MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -194,7 +245,7 @@ static inline void mavlink_msg_named_value_float_send_buf(mavlink_message_t *msg mavlink_named_value_float_t *packet = (mavlink_named_value_float_t *)msgbuf; packet->time_boot_ms = time_boot_ms; packet->value = value; - mav_array_memcpy(packet->name, name, sizeof(char)*10); + mav_array_assign_char(packet->name, name, 10); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_MIN_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_named_value_int.h b/lib/main/MAVLink/common/mavlink_msg_named_value_int.h index 32dbdc91ce4..c5cbe7e8043 100755 --- a/lib/main/MAVLink/common/mavlink_msg_named_value_int.h +++ b/lib/main/MAVLink/common/mavlink_msg_named_value_int.h @@ -65,7 +65,7 @@ static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8 mavlink_named_value_int_t packet; packet.time_boot_ms = time_boot_ms; packet.value = value; - mav_array_memcpy(packet.name, name, sizeof(char)*10); + mav_array_assign_char(packet.name, name, 10); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); #endif @@ -73,6 +73,43 @@ static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_INT_MIN_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); } +/** + * @brief Pack a named_value_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param name Name of the debug variable + * @param value Signed integer value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_named_value_int_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, const char *name, int32_t value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#else + mavlink_named_value_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_NAMED_VALUE_INT_MIN_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_NAMED_VALUE_INT_MIN_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#endif +} + /** * @brief Pack a named_value_int message on a channel * @param system_id ID of this system @@ -98,7 +135,7 @@ static inline uint16_t mavlink_msg_named_value_int_pack_chan(uint8_t system_id, mavlink_named_value_int_t packet; packet.time_boot_ms = time_boot_ms; packet.value = value; - mav_array_memcpy(packet.name, name, sizeof(char)*10); + mav_array_assign_char(packet.name, name, 10); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); #endif @@ -133,6 +170,20 @@ static inline uint16_t mavlink_msg_named_value_int_encode_chan(uint8_t system_id return mavlink_msg_named_value_int_pack_chan(system_id, component_id, chan, msg, named_value_int->time_boot_ms, named_value_int->name, named_value_int->value); } +/** + * @brief Encode a named_value_int struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param named_value_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_named_value_int_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_named_value_int_t* named_value_int) +{ + return mavlink_msg_named_value_int_pack_status(system_id, component_id, _status, msg, named_value_int->time_boot_ms, named_value_int->name, named_value_int->value); +} + /** * @brief Send a named_value_int message * @param chan MAVLink channel to send the message @@ -155,7 +206,7 @@ static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, uint mavlink_named_value_int_t packet; packet.time_boot_ms = time_boot_ms; packet.value = value; - mav_array_memcpy(packet.name, name, sizeof(char)*10); + mav_array_assign_char(packet.name, name, 10); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)&packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_MIN_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); #endif } @@ -176,7 +227,7 @@ static inline void mavlink_msg_named_value_int_send_struct(mavlink_channel_t cha #if MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -194,7 +245,7 @@ static inline void mavlink_msg_named_value_int_send_buf(mavlink_message_t *msgbu mavlink_named_value_int_t *packet = (mavlink_named_value_int_t *)msgbuf; packet->time_boot_ms = time_boot_ms; packet->value = value; - mav_array_memcpy(packet->name, name, sizeof(char)*10); + mav_array_assign_char(packet->name, name, 10); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_MIN_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_nav_controller_output.h b/lib/main/MAVLink/common/mavlink_msg_nav_controller_output.h index 2d9e8c0c5c2..0ede7d2f502 100755 --- a/lib/main/MAVLink/common/mavlink_msg_nav_controller_output.h +++ b/lib/main/MAVLink/common/mavlink_msg_nav_controller_output.h @@ -105,6 +105,60 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id, return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_MIN_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); } +/** + * @brief Pack a nav_controller_output message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param nav_roll [deg] Current desired roll + * @param nav_pitch [deg] Current desired pitch + * @param nav_bearing [deg] Current desired heading + * @param target_bearing [deg] Bearing to current waypoint/target + * @param wp_dist [m] Distance to active waypoint + * @param alt_error [m] Current altitude error + * @param aspd_error [m/s] Current airspeed error + * @param xtrack_error [m] Current crosstrack error on x-y plane + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_nav_controller_output_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN]; + _mav_put_float(buf, 0, nav_roll); + _mav_put_float(buf, 4, nav_pitch); + _mav_put_float(buf, 8, alt_error); + _mav_put_float(buf, 12, aspd_error); + _mav_put_float(buf, 16, xtrack_error); + _mav_put_int16_t(buf, 20, nav_bearing); + _mav_put_int16_t(buf, 22, target_bearing); + _mav_put_uint16_t(buf, 24, wp_dist); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#else + mavlink_nav_controller_output_t packet; + packet.nav_roll = nav_roll; + packet.nav_pitch = nav_pitch; + packet.alt_error = alt_error; + packet.aspd_error = aspd_error; + packet.xtrack_error = xtrack_error; + packet.nav_bearing = nav_bearing; + packet.target_bearing = target_bearing; + packet.wp_dist = wp_dist; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_MIN_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_MIN_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#endif +} + /** * @brief Pack a nav_controller_output message on a channel * @param system_id ID of this system @@ -182,6 +236,20 @@ static inline uint16_t mavlink_msg_nav_controller_output_encode_chan(uint8_t sys return mavlink_msg_nav_controller_output_pack_chan(system_id, component_id, chan, msg, nav_controller_output->nav_roll, nav_controller_output->nav_pitch, nav_controller_output->nav_bearing, nav_controller_output->target_bearing, nav_controller_output->wp_dist, nav_controller_output->alt_error, nav_controller_output->aspd_error, nav_controller_output->xtrack_error); } +/** + * @brief Encode a nav_controller_output struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param nav_controller_output C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_nav_controller_output_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_nav_controller_output_t* nav_controller_output) +{ + return mavlink_msg_nav_controller_output_pack_status(system_id, component_id, _status, msg, nav_controller_output->nav_roll, nav_controller_output->nav_pitch, nav_controller_output->nav_bearing, nav_controller_output->target_bearing, nav_controller_output->wp_dist, nav_controller_output->alt_error, nav_controller_output->aspd_error, nav_controller_output->xtrack_error); +} + /** * @brief Send a nav_controller_output message * @param chan MAVLink channel to send the message @@ -242,7 +310,7 @@ static inline void mavlink_msg_nav_controller_output_send_struct(mavlink_channel #if MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_obstacle_distance.h b/lib/main/MAVLink/common/mavlink_msg_obstacle_distance.h index 641f2627fe5..df3f31e864a 100644 --- a/lib/main/MAVLink/common/mavlink_msg_obstacle_distance.h +++ b/lib/main/MAVLink/common/mavlink_msg_obstacle_distance.h @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_obstacle_distance_pack(uint8_t system_id, uin packet.increment_f = increment_f; packet.angle_offset = angle_offset; packet.frame = frame; - mav_array_memcpy(packet.distances, distances, sizeof(uint16_t)*72); + mav_array_assign_uint16_t(packet.distances, distances, 72); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN); #endif @@ -109,6 +109,61 @@ static inline uint16_t mavlink_msg_obstacle_distance_pack(uint8_t system_id, uin return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC); } +/** + * @brief Pack a obstacle_distance message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param sensor_type Class id of the distance sensor type. + * @param distances [cm] Distance of obstacles around the vehicle with index 0 corresponding to north + angle_offset, unless otherwise specified in the frame. A value of 0 is valid and means that the obstacle is practically touching the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm. + * @param increment [deg] Angular width in degrees of each array element. Increment direction is clockwise. This field is ignored if increment_f is non-zero. + * @param min_distance [cm] Minimum distance the sensor can measure. + * @param max_distance [cm] Maximum distance the sensor can measure. + * @param increment_f [deg] Angular width in degrees of each array element as a float. If non-zero then this value is used instead of the uint8_t increment field. Positive is clockwise direction, negative is counter-clockwise. + * @param angle_offset [deg] Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise. + * @param frame Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is north aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_obstacle_distance_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint8_t sensor_type, const uint16_t *distances, uint8_t increment, uint16_t min_distance, uint16_t max_distance, float increment_f, float angle_offset, uint8_t frame) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 152, min_distance); + _mav_put_uint16_t(buf, 154, max_distance); + _mav_put_uint8_t(buf, 156, sensor_type); + _mav_put_uint8_t(buf, 157, increment); + _mav_put_float(buf, 158, increment_f); + _mav_put_float(buf, 162, angle_offset); + _mav_put_uint8_t(buf, 166, frame); + _mav_put_uint16_t_array(buf, 8, distances, 72); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN); +#else + mavlink_obstacle_distance_t packet; + packet.time_usec = time_usec; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.sensor_type = sensor_type; + packet.increment = increment; + packet.increment_f = increment_f; + packet.angle_offset = angle_offset; + packet.frame = frame; + mav_array_memcpy(packet.distances, distances, sizeof(uint16_t)*72); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OBSTACLE_DISTANCE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN); +#endif +} + /** * @brief Pack a obstacle_distance message on a channel * @param system_id ID of this system @@ -152,7 +207,7 @@ static inline uint16_t mavlink_msg_obstacle_distance_pack_chan(uint8_t system_id packet.increment_f = increment_f; packet.angle_offset = angle_offset; packet.frame = frame; - mav_array_memcpy(packet.distances, distances, sizeof(uint16_t)*72); + mav_array_assign_uint16_t(packet.distances, distances, 72); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN); #endif @@ -187,6 +242,20 @@ static inline uint16_t mavlink_msg_obstacle_distance_encode_chan(uint8_t system_ return mavlink_msg_obstacle_distance_pack_chan(system_id, component_id, chan, msg, obstacle_distance->time_usec, obstacle_distance->sensor_type, obstacle_distance->distances, obstacle_distance->increment, obstacle_distance->min_distance, obstacle_distance->max_distance, obstacle_distance->increment_f, obstacle_distance->angle_offset, obstacle_distance->frame); } +/** + * @brief Encode a obstacle_distance struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param obstacle_distance C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obstacle_distance_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_obstacle_distance_t* obstacle_distance) +{ + return mavlink_msg_obstacle_distance_pack_status(system_id, component_id, _status, msg, obstacle_distance->time_usec, obstacle_distance->sensor_type, obstacle_distance->distances, obstacle_distance->increment, obstacle_distance->min_distance, obstacle_distance->max_distance, obstacle_distance->increment_f, obstacle_distance->angle_offset, obstacle_distance->frame); +} + /** * @brief Send a obstacle_distance message * @param chan MAVLink channel to send the message @@ -227,7 +296,7 @@ static inline void mavlink_msg_obstacle_distance_send(mavlink_channel_t chan, ui packet.increment_f = increment_f; packet.angle_offset = angle_offset; packet.frame = frame; - mav_array_memcpy(packet.distances, distances, sizeof(uint16_t)*72); + mav_array_assign_uint16_t(packet.distances, distances, 72); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE, (const char *)&packet, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC); #endif } @@ -248,7 +317,7 @@ static inline void mavlink_msg_obstacle_distance_send_struct(mavlink_channel_t c #if MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -278,7 +347,7 @@ static inline void mavlink_msg_obstacle_distance_send_buf(mavlink_message_t *msg packet->increment_f = increment_f; packet->angle_offset = angle_offset; packet->frame = frame; - mav_array_memcpy(packet->distances, distances, sizeof(uint16_t)*72); + mav_array_assign_uint16_t(packet->distances, distances, 72); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE, (const char *)packet, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_odometry.h b/lib/main/MAVLink/common/mavlink_msg_odometry.h index 93869b7916b..ff4de1dca0f 100644 --- a/lib/main/MAVLink/common/mavlink_msg_odometry.h +++ b/lib/main/MAVLink/common/mavlink_msg_odometry.h @@ -22,11 +22,12 @@ typedef struct __mavlink_odometry_t { uint8_t child_frame_id; /*< Coordinate frame of reference for the velocity in free space (twist) data.*/ uint8_t reset_counter; /*< Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps.*/ uint8_t estimator_type; /*< Type of estimator that is providing the odometry.*/ + int8_t quality; /*< [%] Optional odometry quality metric as a percentage. -1 = odometry has failed, 0 = unknown/unset quality, 1 = worst quality, 100 = best quality*/ } mavlink_odometry_t; -#define MAVLINK_MSG_ID_ODOMETRY_LEN 232 +#define MAVLINK_MSG_ID_ODOMETRY_LEN 233 #define MAVLINK_MSG_ID_ODOMETRY_MIN_LEN 230 -#define MAVLINK_MSG_ID_331_LEN 232 +#define MAVLINK_MSG_ID_331_LEN 233 #define MAVLINK_MSG_ID_331_MIN_LEN 230 #define MAVLINK_MSG_ID_ODOMETRY_CRC 91 @@ -40,7 +41,7 @@ typedef struct __mavlink_odometry_t { #define MAVLINK_MESSAGE_INFO_ODOMETRY { \ 331, \ "ODOMETRY", \ - 17, \ + 18, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_odometry_t, time_usec) }, \ { "frame_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 228, offsetof(mavlink_odometry_t, frame_id) }, \ { "child_frame_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 229, offsetof(mavlink_odometry_t, child_frame_id) }, \ @@ -58,12 +59,13 @@ typedef struct __mavlink_odometry_t { { "velocity_covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 144, offsetof(mavlink_odometry_t, velocity_covariance) }, \ { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 230, offsetof(mavlink_odometry_t, reset_counter) }, \ { "estimator_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 231, offsetof(mavlink_odometry_t, estimator_type) }, \ + { "quality", NULL, MAVLINK_TYPE_INT8_T, 0, 232, offsetof(mavlink_odometry_t, quality) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_ODOMETRY { \ "ODOMETRY", \ - 17, \ + 18, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_odometry_t, time_usec) }, \ { "frame_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 228, offsetof(mavlink_odometry_t, frame_id) }, \ { "child_frame_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 229, offsetof(mavlink_odometry_t, child_frame_id) }, \ @@ -81,6 +83,7 @@ typedef struct __mavlink_odometry_t { { "velocity_covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 144, offsetof(mavlink_odometry_t, velocity_covariance) }, \ { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 230, offsetof(mavlink_odometry_t, reset_counter) }, \ { "estimator_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 231, offsetof(mavlink_odometry_t, estimator_type) }, \ + { "quality", NULL, MAVLINK_TYPE_INT8_T, 0, 232, offsetof(mavlink_odometry_t, quality) }, \ } \ } #endif @@ -108,10 +111,11 @@ typedef struct __mavlink_odometry_t { * @param velocity_covariance Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. * @param estimator_type Type of estimator that is providing the odometry. + * @param quality [%] Optional odometry quality metric as a percentage. -1 = odometry has failed, 0 = unknown/unset quality, 1 = worst quality, 100 = best quality * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_odometry_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t frame_id, uint8_t child_frame_id, float x, float y, float z, const float *q, float vx, float vy, float vz, float rollspeed, float pitchspeed, float yawspeed, const float *pose_covariance, const float *velocity_covariance, uint8_t reset_counter, uint8_t estimator_type) + uint64_t time_usec, uint8_t frame_id, uint8_t child_frame_id, float x, float y, float z, const float *q, float vx, float vy, float vz, float rollspeed, float pitchspeed, float yawspeed, const float *pose_covariance, const float *velocity_covariance, uint8_t reset_counter, uint8_t estimator_type, int8_t quality) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ODOMETRY_LEN]; @@ -129,6 +133,7 @@ static inline uint16_t mavlink_msg_odometry_pack(uint8_t system_id, uint8_t comp _mav_put_uint8_t(buf, 229, child_frame_id); _mav_put_uint8_t(buf, 230, reset_counter); _mav_put_uint8_t(buf, 231, estimator_type); + _mav_put_int8_t(buf, 232, quality); _mav_put_float_array(buf, 20, q, 4); _mav_put_float_array(buf, 60, pose_covariance, 21); _mav_put_float_array(buf, 144, velocity_covariance, 21); @@ -149,6 +154,85 @@ static inline uint16_t mavlink_msg_odometry_pack(uint8_t system_id, uint8_t comp packet.child_frame_id = child_frame_id; packet.reset_counter = reset_counter; packet.estimator_type = estimator_type; + packet.quality = quality; + mav_array_assign_float(packet.q, q, 4); + mav_array_assign_float(packet.pose_covariance, pose_covariance, 21); + mav_array_assign_float(packet.velocity_covariance, velocity_covariance, 21); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ODOMETRY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ODOMETRY; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); +} + +/** + * @brief Pack a odometry message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param frame_id Coordinate frame of reference for the pose data. + * @param child_frame_id Coordinate frame of reference for the velocity in free space (twist) data. + * @param x [m] X Position + * @param y [m] Y Position + * @param z [m] Z Position + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + * @param vx [m/s] X linear speed + * @param vy [m/s] Y linear speed + * @param vz [m/s] Z linear speed + * @param rollspeed [rad/s] Roll angular speed + * @param pitchspeed [rad/s] Pitch angular speed + * @param yawspeed [rad/s] Yaw angular speed + * @param pose_covariance Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param velocity_covariance Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + * @param estimator_type Type of estimator that is providing the odometry. + * @param quality [%] Optional odometry quality metric as a percentage. -1 = odometry has failed, 0 = unknown/unset quality, 1 = worst quality, 100 = best quality + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_odometry_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint8_t frame_id, uint8_t child_frame_id, float x, float y, float z, const float *q, float vx, float vy, float vz, float rollspeed, float pitchspeed, float yawspeed, const float *pose_covariance, const float *velocity_covariance, uint8_t reset_counter, uint8_t estimator_type, int8_t quality) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ODOMETRY_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 36, vx); + _mav_put_float(buf, 40, vy); + _mav_put_float(buf, 44, vz); + _mav_put_float(buf, 48, rollspeed); + _mav_put_float(buf, 52, pitchspeed); + _mav_put_float(buf, 56, yawspeed); + _mav_put_uint8_t(buf, 228, frame_id); + _mav_put_uint8_t(buf, 229, child_frame_id); + _mav_put_uint8_t(buf, 230, reset_counter); + _mav_put_uint8_t(buf, 231, estimator_type); + _mav_put_int8_t(buf, 232, quality); + _mav_put_float_array(buf, 20, q, 4); + _mav_put_float_array(buf, 60, pose_covariance, 21); + _mav_put_float_array(buf, 144, velocity_covariance, 21); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ODOMETRY_LEN); +#else + mavlink_odometry_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.frame_id = frame_id; + packet.child_frame_id = child_frame_id; + packet.reset_counter = reset_counter; + packet.estimator_type = estimator_type; + packet.quality = quality; mav_array_memcpy(packet.q, q, sizeof(float)*4); mav_array_memcpy(packet.pose_covariance, pose_covariance, sizeof(float)*21); mav_array_memcpy(packet.velocity_covariance, velocity_covariance, sizeof(float)*21); @@ -156,7 +240,11 @@ static inline uint16_t mavlink_msg_odometry_pack(uint8_t system_id, uint8_t comp #endif msg->msgid = MAVLINK_MSG_ID_ODOMETRY; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN); +#endif } /** @@ -182,11 +270,12 @@ static inline uint16_t mavlink_msg_odometry_pack(uint8_t system_id, uint8_t comp * @param velocity_covariance Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. * @param estimator_type Type of estimator that is providing the odometry. + * @param quality [%] Optional odometry quality metric as a percentage. -1 = odometry has failed, 0 = unknown/unset quality, 1 = worst quality, 100 = best quality * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_odometry_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,uint8_t frame_id,uint8_t child_frame_id,float x,float y,float z,const float *q,float vx,float vy,float vz,float rollspeed,float pitchspeed,float yawspeed,const float *pose_covariance,const float *velocity_covariance,uint8_t reset_counter,uint8_t estimator_type) + uint64_t time_usec,uint8_t frame_id,uint8_t child_frame_id,float x,float y,float z,const float *q,float vx,float vy,float vz,float rollspeed,float pitchspeed,float yawspeed,const float *pose_covariance,const float *velocity_covariance,uint8_t reset_counter,uint8_t estimator_type,int8_t quality) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ODOMETRY_LEN]; @@ -204,6 +293,7 @@ static inline uint16_t mavlink_msg_odometry_pack_chan(uint8_t system_id, uint8_t _mav_put_uint8_t(buf, 229, child_frame_id); _mav_put_uint8_t(buf, 230, reset_counter); _mav_put_uint8_t(buf, 231, estimator_type); + _mav_put_int8_t(buf, 232, quality); _mav_put_float_array(buf, 20, q, 4); _mav_put_float_array(buf, 60, pose_covariance, 21); _mav_put_float_array(buf, 144, velocity_covariance, 21); @@ -224,9 +314,10 @@ static inline uint16_t mavlink_msg_odometry_pack_chan(uint8_t system_id, uint8_t packet.child_frame_id = child_frame_id; packet.reset_counter = reset_counter; packet.estimator_type = estimator_type; - mav_array_memcpy(packet.q, q, sizeof(float)*4); - mav_array_memcpy(packet.pose_covariance, pose_covariance, sizeof(float)*21); - mav_array_memcpy(packet.velocity_covariance, velocity_covariance, sizeof(float)*21); + packet.quality = quality; + mav_array_assign_float(packet.q, q, 4); + mav_array_assign_float(packet.pose_covariance, pose_covariance, 21); + mav_array_assign_float(packet.velocity_covariance, velocity_covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ODOMETRY_LEN); #endif @@ -244,7 +335,7 @@ static inline uint16_t mavlink_msg_odometry_pack_chan(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_odometry_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_odometry_t* odometry) { - return mavlink_msg_odometry_pack(system_id, component_id, msg, odometry->time_usec, odometry->frame_id, odometry->child_frame_id, odometry->x, odometry->y, odometry->z, odometry->q, odometry->vx, odometry->vy, odometry->vz, odometry->rollspeed, odometry->pitchspeed, odometry->yawspeed, odometry->pose_covariance, odometry->velocity_covariance, odometry->reset_counter, odometry->estimator_type); + return mavlink_msg_odometry_pack(system_id, component_id, msg, odometry->time_usec, odometry->frame_id, odometry->child_frame_id, odometry->x, odometry->y, odometry->z, odometry->q, odometry->vx, odometry->vy, odometry->vz, odometry->rollspeed, odometry->pitchspeed, odometry->yawspeed, odometry->pose_covariance, odometry->velocity_covariance, odometry->reset_counter, odometry->estimator_type, odometry->quality); } /** @@ -258,7 +349,21 @@ static inline uint16_t mavlink_msg_odometry_encode(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_odometry_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_odometry_t* odometry) { - return mavlink_msg_odometry_pack_chan(system_id, component_id, chan, msg, odometry->time_usec, odometry->frame_id, odometry->child_frame_id, odometry->x, odometry->y, odometry->z, odometry->q, odometry->vx, odometry->vy, odometry->vz, odometry->rollspeed, odometry->pitchspeed, odometry->yawspeed, odometry->pose_covariance, odometry->velocity_covariance, odometry->reset_counter, odometry->estimator_type); + return mavlink_msg_odometry_pack_chan(system_id, component_id, chan, msg, odometry->time_usec, odometry->frame_id, odometry->child_frame_id, odometry->x, odometry->y, odometry->z, odometry->q, odometry->vx, odometry->vy, odometry->vz, odometry->rollspeed, odometry->pitchspeed, odometry->yawspeed, odometry->pose_covariance, odometry->velocity_covariance, odometry->reset_counter, odometry->estimator_type, odometry->quality); +} + +/** + * @brief Encode a odometry struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param odometry C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_odometry_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_odometry_t* odometry) +{ + return mavlink_msg_odometry_pack_status(system_id, component_id, _status, msg, odometry->time_usec, odometry->frame_id, odometry->child_frame_id, odometry->x, odometry->y, odometry->z, odometry->q, odometry->vx, odometry->vy, odometry->vz, odometry->rollspeed, odometry->pitchspeed, odometry->yawspeed, odometry->pose_covariance, odometry->velocity_covariance, odometry->reset_counter, odometry->estimator_type, odometry->quality); } /** @@ -282,10 +387,11 @@ static inline uint16_t mavlink_msg_odometry_encode_chan(uint8_t system_id, uint8 * @param velocity_covariance Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. * @param estimator_type Type of estimator that is providing the odometry. + * @param quality [%] Optional odometry quality metric as a percentage. -1 = odometry has failed, 0 = unknown/unset quality, 1 = worst quality, 100 = best quality */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_odometry_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t frame_id, uint8_t child_frame_id, float x, float y, float z, const float *q, float vx, float vy, float vz, float rollspeed, float pitchspeed, float yawspeed, const float *pose_covariance, const float *velocity_covariance, uint8_t reset_counter, uint8_t estimator_type) +static inline void mavlink_msg_odometry_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t frame_id, uint8_t child_frame_id, float x, float y, float z, const float *q, float vx, float vy, float vz, float rollspeed, float pitchspeed, float yawspeed, const float *pose_covariance, const float *velocity_covariance, uint8_t reset_counter, uint8_t estimator_type, int8_t quality) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ODOMETRY_LEN]; @@ -303,6 +409,7 @@ static inline void mavlink_msg_odometry_send(mavlink_channel_t chan, uint64_t ti _mav_put_uint8_t(buf, 229, child_frame_id); _mav_put_uint8_t(buf, 230, reset_counter); _mav_put_uint8_t(buf, 231, estimator_type); + _mav_put_int8_t(buf, 232, quality); _mav_put_float_array(buf, 20, q, 4); _mav_put_float_array(buf, 60, pose_covariance, 21); _mav_put_float_array(buf, 144, velocity_covariance, 21); @@ -323,9 +430,10 @@ static inline void mavlink_msg_odometry_send(mavlink_channel_t chan, uint64_t ti packet.child_frame_id = child_frame_id; packet.reset_counter = reset_counter; packet.estimator_type = estimator_type; - mav_array_memcpy(packet.q, q, sizeof(float)*4); - mav_array_memcpy(packet.pose_covariance, pose_covariance, sizeof(float)*21); - mav_array_memcpy(packet.velocity_covariance, velocity_covariance, sizeof(float)*21); + packet.quality = quality; + mav_array_assign_float(packet.q, q, 4); + mav_array_assign_float(packet.pose_covariance, pose_covariance, 21); + mav_array_assign_float(packet.velocity_covariance, velocity_covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, (const char *)&packet, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); #endif } @@ -338,7 +446,7 @@ static inline void mavlink_msg_odometry_send(mavlink_channel_t chan, uint64_t ti static inline void mavlink_msg_odometry_send_struct(mavlink_channel_t chan, const mavlink_odometry_t* odometry) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_odometry_send(chan, odometry->time_usec, odometry->frame_id, odometry->child_frame_id, odometry->x, odometry->y, odometry->z, odometry->q, odometry->vx, odometry->vy, odometry->vz, odometry->rollspeed, odometry->pitchspeed, odometry->yawspeed, odometry->pose_covariance, odometry->velocity_covariance, odometry->reset_counter, odometry->estimator_type); + mavlink_msg_odometry_send(chan, odometry->time_usec, odometry->frame_id, odometry->child_frame_id, odometry->x, odometry->y, odometry->z, odometry->q, odometry->vx, odometry->vy, odometry->vz, odometry->rollspeed, odometry->pitchspeed, odometry->yawspeed, odometry->pose_covariance, odometry->velocity_covariance, odometry->reset_counter, odometry->estimator_type, odometry->quality); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, (const char *)odometry, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); #endif @@ -346,13 +454,13 @@ static inline void mavlink_msg_odometry_send_struct(mavlink_channel_t chan, cons #if MAVLINK_MSG_ID_ODOMETRY_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_odometry_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t frame_id, uint8_t child_frame_id, float x, float y, float z, const float *q, float vx, float vy, float vz, float rollspeed, float pitchspeed, float yawspeed, const float *pose_covariance, const float *velocity_covariance, uint8_t reset_counter, uint8_t estimator_type) +static inline void mavlink_msg_odometry_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t frame_id, uint8_t child_frame_id, float x, float y, float z, const float *q, float vx, float vy, float vz, float rollspeed, float pitchspeed, float yawspeed, const float *pose_covariance, const float *velocity_covariance, uint8_t reset_counter, uint8_t estimator_type, int8_t quality) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -370,6 +478,7 @@ static inline void mavlink_msg_odometry_send_buf(mavlink_message_t *msgbuf, mavl _mav_put_uint8_t(buf, 229, child_frame_id); _mav_put_uint8_t(buf, 230, reset_counter); _mav_put_uint8_t(buf, 231, estimator_type); + _mav_put_int8_t(buf, 232, quality); _mav_put_float_array(buf, 20, q, 4); _mav_put_float_array(buf, 60, pose_covariance, 21); _mav_put_float_array(buf, 144, velocity_covariance, 21); @@ -390,9 +499,10 @@ static inline void mavlink_msg_odometry_send_buf(mavlink_message_t *msgbuf, mavl packet->child_frame_id = child_frame_id; packet->reset_counter = reset_counter; packet->estimator_type = estimator_type; - mav_array_memcpy(packet->q, q, sizeof(float)*4); - mav_array_memcpy(packet->pose_covariance, pose_covariance, sizeof(float)*21); - mav_array_memcpy(packet->velocity_covariance, velocity_covariance, sizeof(float)*21); + packet->quality = quality; + mav_array_assign_float(packet->q, q, 4); + mav_array_assign_float(packet->pose_covariance, pose_covariance, 21); + mav_array_assign_float(packet->velocity_covariance, velocity_covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, (const char *)packet, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); #endif } @@ -573,6 +683,16 @@ static inline uint8_t mavlink_msg_odometry_get_estimator_type(const mavlink_mess return _MAV_RETURN_uint8_t(msg, 231); } +/** + * @brief Get field quality from odometry message + * + * @return [%] Optional odometry quality metric as a percentage. -1 = odometry has failed, 0 = unknown/unset quality, 1 = worst quality, 100 = best quality + */ +static inline int8_t mavlink_msg_odometry_get_quality(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int8_t(msg, 232); +} + /** * @brief Decode a odometry message into a struct * @@ -599,6 +719,7 @@ static inline void mavlink_msg_odometry_decode(const mavlink_message_t* msg, mav odometry->child_frame_id = mavlink_msg_odometry_get_child_frame_id(msg); odometry->reset_counter = mavlink_msg_odometry_get_reset_counter(msg); odometry->estimator_type = mavlink_msg_odometry_get_estimator_type(msg); + odometry->quality = mavlink_msg_odometry_get_quality(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_ODOMETRY_LEN? msg->len : MAVLINK_MSG_ID_ODOMETRY_LEN; memset(odometry, 0, MAVLINK_MSG_ID_ODOMETRY_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_onboard_computer_status.h b/lib/main/MAVLink/common/mavlink_msg_onboard_computer_status.h index 6da2581423e..bb191e5a356 100644 --- a/lib/main/MAVLink/common/mavlink_msg_onboard_computer_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_onboard_computer_status.h @@ -136,6 +136,90 @@ typedef struct __mavlink_onboard_computer_status_t { static inline uint16_t mavlink_msg_onboard_computer_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_usec, uint32_t uptime, uint8_t type, const uint8_t *cpu_cores, const uint8_t *cpu_combined, const uint8_t *gpu_cores, const uint8_t *gpu_combined, int8_t temperature_board, const int8_t *temperature_core, const int16_t *fan_speed, uint32_t ram_usage, uint32_t ram_total, const uint32_t *storage_type, const uint32_t *storage_usage, const uint32_t *storage_total, const uint32_t *link_type, const uint32_t *link_tx_rate, const uint32_t *link_rx_rate, const uint32_t *link_tx_max, const uint32_t *link_rx_max) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime); + _mav_put_uint32_t(buf, 12, ram_usage); + _mav_put_uint32_t(buf, 16, ram_total); + _mav_put_uint8_t(buf, 196, type); + _mav_put_int8_t(buf, 229, temperature_board); + _mav_put_uint32_t_array(buf, 20, storage_type, 4); + _mav_put_uint32_t_array(buf, 36, storage_usage, 4); + _mav_put_uint32_t_array(buf, 52, storage_total, 4); + _mav_put_uint32_t_array(buf, 68, link_type, 6); + _mav_put_uint32_t_array(buf, 92, link_tx_rate, 6); + _mav_put_uint32_t_array(buf, 116, link_rx_rate, 6); + _mav_put_uint32_t_array(buf, 140, link_tx_max, 6); + _mav_put_uint32_t_array(buf, 164, link_rx_max, 6); + _mav_put_int16_t_array(buf, 188, fan_speed, 4); + _mav_put_uint8_t_array(buf, 197, cpu_cores, 8); + _mav_put_uint8_t_array(buf, 205, cpu_combined, 10); + _mav_put_uint8_t_array(buf, 215, gpu_cores, 4); + _mav_put_uint8_t_array(buf, 219, gpu_combined, 10); + _mav_put_int8_t_array(buf, 230, temperature_core, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN); +#else + mavlink_onboard_computer_status_t packet; + packet.time_usec = time_usec; + packet.uptime = uptime; + packet.ram_usage = ram_usage; + packet.ram_total = ram_total; + packet.type = type; + packet.temperature_board = temperature_board; + mav_array_assign_uint32_t(packet.storage_type, storage_type, 4); + mav_array_assign_uint32_t(packet.storage_usage, storage_usage, 4); + mav_array_assign_uint32_t(packet.storage_total, storage_total, 4); + mav_array_assign_uint32_t(packet.link_type, link_type, 6); + mav_array_assign_uint32_t(packet.link_tx_rate, link_tx_rate, 6); + mav_array_assign_uint32_t(packet.link_rx_rate, link_rx_rate, 6); + mav_array_assign_uint32_t(packet.link_tx_max, link_tx_max, 6); + mav_array_assign_uint32_t(packet.link_rx_max, link_rx_max, 6); + mav_array_assign_int16_t(packet.fan_speed, fan_speed, 4); + mav_array_assign_uint8_t(packet.cpu_cores, cpu_cores, 8); + mav_array_assign_uint8_t(packet.cpu_combined, cpu_combined, 10); + mav_array_assign_uint8_t(packet.gpu_cores, gpu_cores, 4); + mav_array_assign_uint8_t(packet.gpu_combined, gpu_combined, 10); + mav_array_assign_int8_t(packet.temperature_core, temperature_core, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); +} + +/** + * @brief Pack a onboard_computer_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime [ms] Time since system boot. + * @param type Type of the onboard computer: 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers. + * @param cpu_cores CPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + * @param cpu_combined Combined CPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + * @param gpu_cores GPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + * @param gpu_combined Combined GPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + * @param temperature_board [degC] Temperature of the board. A value of INT8_MAX implies the field is unused. + * @param temperature_core [degC] Temperature of the CPU core. A value of INT8_MAX implies the field is unused. + * @param fan_speed [rpm] Fan speeds. A value of INT16_MAX implies the field is unused. + * @param ram_usage [MiB] Amount of used RAM on the component system. A value of UINT32_MAX implies the field is unused. + * @param ram_total [MiB] Total amount of RAM on the component system. A value of UINT32_MAX implies the field is unused. + * @param storage_type Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable). A value of UINT32_MAX implies the field is unused. + * @param storage_usage [MiB] Amount of used storage space on the component system. A value of UINT32_MAX implies the field is unused. + * @param storage_total [MiB] Total amount of storage space on the component system. A value of UINT32_MAX implies the field is unused. + * @param link_type Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary + * @param link_tx_rate [KiB/s] Network traffic from the component system. A value of UINT32_MAX implies the field is unused. + * @param link_rx_rate [KiB/s] Network traffic to the component system. A value of UINT32_MAX implies the field is unused. + * @param link_tx_max [KiB/s] Network capacity from the component system. A value of UINT32_MAX implies the field is unused. + * @param link_rx_max [KiB/s] Network capacity to the component system. A value of UINT32_MAX implies the field is unused. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_onboard_computer_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint32_t uptime, uint8_t type, const uint8_t *cpu_cores, const uint8_t *cpu_combined, const uint8_t *gpu_cores, const uint8_t *gpu_combined, int8_t temperature_board, const int8_t *temperature_core, const int16_t *fan_speed, uint32_t ram_usage, uint32_t ram_total, const uint32_t *storage_type, const uint32_t *storage_usage, const uint32_t *storage_total, const uint32_t *link_type, const uint32_t *link_tx_rate, const uint32_t *link_rx_rate, const uint32_t *link_tx_max, const uint32_t *link_rx_max) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN]; _mav_put_uint64_t(buf, 0, time_usec); @@ -185,7 +269,11 @@ static inline uint16_t mavlink_msg_onboard_computer_status_pack(uint8_t system_i #endif msg->msgid = MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN); +#endif } /** @@ -251,20 +339,20 @@ static inline uint16_t mavlink_msg_onboard_computer_status_pack_chan(uint8_t sys packet.ram_total = ram_total; packet.type = type; packet.temperature_board = temperature_board; - mav_array_memcpy(packet.storage_type, storage_type, sizeof(uint32_t)*4); - mav_array_memcpy(packet.storage_usage, storage_usage, sizeof(uint32_t)*4); - mav_array_memcpy(packet.storage_total, storage_total, sizeof(uint32_t)*4); - mav_array_memcpy(packet.link_type, link_type, sizeof(uint32_t)*6); - mav_array_memcpy(packet.link_tx_rate, link_tx_rate, sizeof(uint32_t)*6); - mav_array_memcpy(packet.link_rx_rate, link_rx_rate, sizeof(uint32_t)*6); - mav_array_memcpy(packet.link_tx_max, link_tx_max, sizeof(uint32_t)*6); - mav_array_memcpy(packet.link_rx_max, link_rx_max, sizeof(uint32_t)*6); - mav_array_memcpy(packet.fan_speed, fan_speed, sizeof(int16_t)*4); - mav_array_memcpy(packet.cpu_cores, cpu_cores, sizeof(uint8_t)*8); - mav_array_memcpy(packet.cpu_combined, cpu_combined, sizeof(uint8_t)*10); - mav_array_memcpy(packet.gpu_cores, gpu_cores, sizeof(uint8_t)*4); - mav_array_memcpy(packet.gpu_combined, gpu_combined, sizeof(uint8_t)*10); - mav_array_memcpy(packet.temperature_core, temperature_core, sizeof(int8_t)*8); + mav_array_assign_uint32_t(packet.storage_type, storage_type, 4); + mav_array_assign_uint32_t(packet.storage_usage, storage_usage, 4); + mav_array_assign_uint32_t(packet.storage_total, storage_total, 4); + mav_array_assign_uint32_t(packet.link_type, link_type, 6); + mav_array_assign_uint32_t(packet.link_tx_rate, link_tx_rate, 6); + mav_array_assign_uint32_t(packet.link_rx_rate, link_rx_rate, 6); + mav_array_assign_uint32_t(packet.link_tx_max, link_tx_max, 6); + mav_array_assign_uint32_t(packet.link_rx_max, link_rx_max, 6); + mav_array_assign_int16_t(packet.fan_speed, fan_speed, 4); + mav_array_assign_uint8_t(packet.cpu_cores, cpu_cores, 8); + mav_array_assign_uint8_t(packet.cpu_combined, cpu_combined, 10); + mav_array_assign_uint8_t(packet.gpu_cores, gpu_cores, 4); + mav_array_assign_uint8_t(packet.gpu_combined, gpu_combined, 10); + mav_array_assign_int8_t(packet.temperature_core, temperature_core, 8); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN); #endif @@ -299,6 +387,20 @@ static inline uint16_t mavlink_msg_onboard_computer_status_encode_chan(uint8_t s return mavlink_msg_onboard_computer_status_pack_chan(system_id, component_id, chan, msg, onboard_computer_status->time_usec, onboard_computer_status->uptime, onboard_computer_status->type, onboard_computer_status->cpu_cores, onboard_computer_status->cpu_combined, onboard_computer_status->gpu_cores, onboard_computer_status->gpu_combined, onboard_computer_status->temperature_board, onboard_computer_status->temperature_core, onboard_computer_status->fan_speed, onboard_computer_status->ram_usage, onboard_computer_status->ram_total, onboard_computer_status->storage_type, onboard_computer_status->storage_usage, onboard_computer_status->storage_total, onboard_computer_status->link_type, onboard_computer_status->link_tx_rate, onboard_computer_status->link_rx_rate, onboard_computer_status->link_tx_max, onboard_computer_status->link_rx_max); } +/** + * @brief Encode a onboard_computer_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param onboard_computer_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_onboard_computer_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_onboard_computer_status_t* onboard_computer_status) +{ + return mavlink_msg_onboard_computer_status_pack_status(system_id, component_id, _status, msg, onboard_computer_status->time_usec, onboard_computer_status->uptime, onboard_computer_status->type, onboard_computer_status->cpu_cores, onboard_computer_status->cpu_combined, onboard_computer_status->gpu_cores, onboard_computer_status->gpu_combined, onboard_computer_status->temperature_board, onboard_computer_status->temperature_core, onboard_computer_status->fan_speed, onboard_computer_status->ram_usage, onboard_computer_status->ram_total, onboard_computer_status->storage_type, onboard_computer_status->storage_usage, onboard_computer_status->storage_total, onboard_computer_status->link_type, onboard_computer_status->link_tx_rate, onboard_computer_status->link_rx_rate, onboard_computer_status->link_tx_max, onboard_computer_status->link_rx_max); +} + /** * @brief Send a onboard_computer_status message * @param chan MAVLink channel to send the message @@ -359,20 +461,20 @@ static inline void mavlink_msg_onboard_computer_status_send(mavlink_channel_t ch packet.ram_total = ram_total; packet.type = type; packet.temperature_board = temperature_board; - mav_array_memcpy(packet.storage_type, storage_type, sizeof(uint32_t)*4); - mav_array_memcpy(packet.storage_usage, storage_usage, sizeof(uint32_t)*4); - mav_array_memcpy(packet.storage_total, storage_total, sizeof(uint32_t)*4); - mav_array_memcpy(packet.link_type, link_type, sizeof(uint32_t)*6); - mav_array_memcpy(packet.link_tx_rate, link_tx_rate, sizeof(uint32_t)*6); - mav_array_memcpy(packet.link_rx_rate, link_rx_rate, sizeof(uint32_t)*6); - mav_array_memcpy(packet.link_tx_max, link_tx_max, sizeof(uint32_t)*6); - mav_array_memcpy(packet.link_rx_max, link_rx_max, sizeof(uint32_t)*6); - mav_array_memcpy(packet.fan_speed, fan_speed, sizeof(int16_t)*4); - mav_array_memcpy(packet.cpu_cores, cpu_cores, sizeof(uint8_t)*8); - mav_array_memcpy(packet.cpu_combined, cpu_combined, sizeof(uint8_t)*10); - mav_array_memcpy(packet.gpu_cores, gpu_cores, sizeof(uint8_t)*4); - mav_array_memcpy(packet.gpu_combined, gpu_combined, sizeof(uint8_t)*10); - mav_array_memcpy(packet.temperature_core, temperature_core, sizeof(int8_t)*8); + mav_array_assign_uint32_t(packet.storage_type, storage_type, 4); + mav_array_assign_uint32_t(packet.storage_usage, storage_usage, 4); + mav_array_assign_uint32_t(packet.storage_total, storage_total, 4); + mav_array_assign_uint32_t(packet.link_type, link_type, 6); + mav_array_assign_uint32_t(packet.link_tx_rate, link_tx_rate, 6); + mav_array_assign_uint32_t(packet.link_rx_rate, link_rx_rate, 6); + mav_array_assign_uint32_t(packet.link_tx_max, link_tx_max, 6); + mav_array_assign_uint32_t(packet.link_rx_max, link_rx_max, 6); + mav_array_assign_int16_t(packet.fan_speed, fan_speed, 4); + mav_array_assign_uint8_t(packet.cpu_cores, cpu_cores, 8); + mav_array_assign_uint8_t(packet.cpu_combined, cpu_combined, 10); + mav_array_assign_uint8_t(packet.gpu_cores, gpu_cores, 4); + mav_array_assign_uint8_t(packet.gpu_combined, gpu_combined, 10); + mav_array_assign_int8_t(packet.temperature_core, temperature_core, 8); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); #endif } @@ -393,7 +495,7 @@ static inline void mavlink_msg_onboard_computer_status_send_struct(mavlink_chann #if MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -432,20 +534,20 @@ static inline void mavlink_msg_onboard_computer_status_send_buf(mavlink_message_ packet->ram_total = ram_total; packet->type = type; packet->temperature_board = temperature_board; - mav_array_memcpy(packet->storage_type, storage_type, sizeof(uint32_t)*4); - mav_array_memcpy(packet->storage_usage, storage_usage, sizeof(uint32_t)*4); - mav_array_memcpy(packet->storage_total, storage_total, sizeof(uint32_t)*4); - mav_array_memcpy(packet->link_type, link_type, sizeof(uint32_t)*6); - mav_array_memcpy(packet->link_tx_rate, link_tx_rate, sizeof(uint32_t)*6); - mav_array_memcpy(packet->link_rx_rate, link_rx_rate, sizeof(uint32_t)*6); - mav_array_memcpy(packet->link_tx_max, link_tx_max, sizeof(uint32_t)*6); - mav_array_memcpy(packet->link_rx_max, link_rx_max, sizeof(uint32_t)*6); - mav_array_memcpy(packet->fan_speed, fan_speed, sizeof(int16_t)*4); - mav_array_memcpy(packet->cpu_cores, cpu_cores, sizeof(uint8_t)*8); - mav_array_memcpy(packet->cpu_combined, cpu_combined, sizeof(uint8_t)*10); - mav_array_memcpy(packet->gpu_cores, gpu_cores, sizeof(uint8_t)*4); - mav_array_memcpy(packet->gpu_combined, gpu_combined, sizeof(uint8_t)*10); - mav_array_memcpy(packet->temperature_core, temperature_core, sizeof(int8_t)*8); + mav_array_assign_uint32_t(packet->storage_type, storage_type, 4); + mav_array_assign_uint32_t(packet->storage_usage, storage_usage, 4); + mav_array_assign_uint32_t(packet->storage_total, storage_total, 4); + mav_array_assign_uint32_t(packet->link_type, link_type, 6); + mav_array_assign_uint32_t(packet->link_tx_rate, link_tx_rate, 6); + mav_array_assign_uint32_t(packet->link_rx_rate, link_rx_rate, 6); + mav_array_assign_uint32_t(packet->link_tx_max, link_tx_max, 6); + mav_array_assign_uint32_t(packet->link_rx_max, link_rx_max, 6); + mav_array_assign_int16_t(packet->fan_speed, fan_speed, 4); + mav_array_assign_uint8_t(packet->cpu_cores, cpu_cores, 8); + mav_array_assign_uint8_t(packet->cpu_combined, cpu_combined, 10); + mav_array_assign_uint8_t(packet->gpu_cores, gpu_cores, 4); + mav_array_assign_uint8_t(packet->gpu_combined, gpu_combined, 10); + mav_array_assign_int8_t(packet->temperature_core, temperature_core, 8); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS, (const char *)packet, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_arm_status.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_arm_status.h new file mode 100644 index 00000000000..8b9f571007a --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_arm_status.h @@ -0,0 +1,278 @@ +#pragma once +// MESSAGE OPEN_DRONE_ID_ARM_STATUS PACKING + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS 12918 + + +typedef struct __mavlink_open_drone_id_arm_status_t { + uint8_t status; /*< Status level indicating if arming is allowed.*/ + char error[50]; /*< Text error message, should be empty if status is good to arm. Fill with nulls in unused portion.*/ +} mavlink_open_drone_id_arm_status_t; + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN 51 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_MIN_LEN 51 +#define MAVLINK_MSG_ID_12918_LEN 51 +#define MAVLINK_MSG_ID_12918_MIN_LEN 51 + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_CRC 139 +#define MAVLINK_MSG_ID_12918_CRC 139 + +#define MAVLINK_MSG_OPEN_DRONE_ID_ARM_STATUS_FIELD_ERROR_LEN 50 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_ARM_STATUS { \ + 12918, \ + "OPEN_DRONE_ID_ARM_STATUS", \ + 2, \ + { { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_arm_status_t, status) }, \ + { "error", NULL, MAVLINK_TYPE_CHAR, 50, 1, offsetof(mavlink_open_drone_id_arm_status_t, error) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_ARM_STATUS { \ + "OPEN_DRONE_ID_ARM_STATUS", \ + 2, \ + { { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_arm_status_t, status) }, \ + { "error", NULL, MAVLINK_TYPE_CHAR, 50, 1, offsetof(mavlink_open_drone_id_arm_status_t, error) }, \ + } \ +} +#endif + +/** + * @brief Pack a open_drone_id_arm_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param status Status level indicating if arming is allowed. + * @param error Text error message, should be empty if status is good to arm. Fill with nulls in unused portion. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_arm_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t status, const char *error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN]; + _mav_put_uint8_t(buf, 0, status); + _mav_put_char_array(buf, 1, error, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN); +#else + mavlink_open_drone_id_arm_status_t packet; + packet.status = status; + mav_array_assign_char(packet.error, error, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_CRC); +} + +/** + * @brief Pack a open_drone_id_arm_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param status Status level indicating if arming is allowed. + * @param error Text error message, should be empty if status is good to arm. Fill with nulls in unused portion. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_arm_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t status, const char *error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN]; + _mav_put_uint8_t(buf, 0, status); + _mav_put_char_array(buf, 1, error, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN); +#else + mavlink_open_drone_id_arm_status_t packet; + packet.status = status; + mav_array_memcpy(packet.error, error, sizeof(char)*50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN); +#endif +} + +/** + * @brief Pack a open_drone_id_arm_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param status Status level indicating if arming is allowed. + * @param error Text error message, should be empty if status is good to arm. Fill with nulls in unused portion. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_arm_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t status,const char *error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN]; + _mav_put_uint8_t(buf, 0, status); + _mav_put_char_array(buf, 1, error, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN); +#else + mavlink_open_drone_id_arm_status_t packet; + packet.status = status; + mav_array_assign_char(packet.error, error, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_CRC); +} + +/** + * @brief Encode a open_drone_id_arm_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_arm_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_arm_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_arm_status_t* open_drone_id_arm_status) +{ + return mavlink_msg_open_drone_id_arm_status_pack(system_id, component_id, msg, open_drone_id_arm_status->status, open_drone_id_arm_status->error); +} + +/** + * @brief Encode a open_drone_id_arm_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_arm_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_arm_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_arm_status_t* open_drone_id_arm_status) +{ + return mavlink_msg_open_drone_id_arm_status_pack_chan(system_id, component_id, chan, msg, open_drone_id_arm_status->status, open_drone_id_arm_status->error); +} + +/** + * @brief Encode a open_drone_id_arm_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_arm_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_arm_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_open_drone_id_arm_status_t* open_drone_id_arm_status) +{ + return mavlink_msg_open_drone_id_arm_status_pack_status(system_id, component_id, _status, msg, open_drone_id_arm_status->status, open_drone_id_arm_status->error); +} + +/** + * @brief Send a open_drone_id_arm_status message + * @param chan MAVLink channel to send the message + * + * @param status Status level indicating if arming is allowed. + * @param error Text error message, should be empty if status is good to arm. Fill with nulls in unused portion. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_open_drone_id_arm_status_send(mavlink_channel_t chan, uint8_t status, const char *error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN]; + _mav_put_uint8_t(buf, 0, status); + _mav_put_char_array(buf, 1, error, 50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_CRC); +#else + mavlink_open_drone_id_arm_status_t packet; + packet.status = status; + mav_array_assign_char(packet.error, error, 50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_CRC); +#endif +} + +/** + * @brief Send a open_drone_id_arm_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_open_drone_id_arm_status_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_arm_status_t* open_drone_id_arm_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_open_drone_id_arm_status_send(chan, open_drone_id_arm_status->status, open_drone_id_arm_status->error); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS, (const char *)open_drone_id_arm_status, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_open_drone_id_arm_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t status, const char *error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, status); + _mav_put_char_array(buf, 1, error, 50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_CRC); +#else + mavlink_open_drone_id_arm_status_t *packet = (mavlink_open_drone_id_arm_status_t *)msgbuf; + packet->status = status; + mav_array_assign_char(packet->error, error, 50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE OPEN_DRONE_ID_ARM_STATUS UNPACKING + + +/** + * @brief Get field status from open_drone_id_arm_status message + * + * @return Status level indicating if arming is allowed. + */ +static inline uint8_t mavlink_msg_open_drone_id_arm_status_get_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field error from open_drone_id_arm_status message + * + * @return Text error message, should be empty if status is good to arm. Fill with nulls in unused portion. + */ +static inline uint16_t mavlink_msg_open_drone_id_arm_status_get_error(const mavlink_message_t* msg, char *error) +{ + return _MAV_RETURN_char_array(msg, error, 50, 1); +} + +/** + * @brief Decode a open_drone_id_arm_status message into a struct + * + * @param msg The message to decode + * @param open_drone_id_arm_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_open_drone_id_arm_status_decode(const mavlink_message_t* msg, mavlink_open_drone_id_arm_status_t* open_drone_id_arm_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + open_drone_id_arm_status->status = mavlink_msg_open_drone_id_arm_status_get_status(msg); + mavlink_msg_open_drone_id_arm_status_get_error(msg, open_drone_id_arm_status->error); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN? msg->len : MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN; + memset(open_drone_id_arm_status, 0, MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_LEN); + memcpy(open_drone_id_arm_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_authentication.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_authentication.h index cd7b68e0268..8f1bd42c380 100644 --- a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_authentication.h +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_authentication.h @@ -10,9 +10,9 @@ typedef struct __mavlink_open_drone_id_authentication_t { uint8_t target_component; /*< Component ID (0 for broadcast).*/ uint8_t id_or_mac[20]; /*< Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */ uint8_t authentication_type; /*< Indicates the type of authentication.*/ - uint8_t data_page; /*< Allowed range is 0 - 4.*/ - uint8_t page_count; /*< This field is only present for page 0. Allowed range is 0 - 5.*/ - uint8_t length; /*< [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. Allowed range is 0 - 109 (17 + 23*4).*/ + uint8_t data_page; /*< Allowed range is 0 - 15.*/ + uint8_t last_page_index; /*< This field is only present for page 0. Allowed range is 0 - 15. See the description of struct ODID_Auth_data at https://github.com/opendroneid/opendroneid-core-c/blob/master/libopendroneid/opendroneid.h.*/ + uint8_t length; /*< [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. See the description of struct ODID_Auth_data at https://github.com/opendroneid/opendroneid-core-c/blob/master/libopendroneid/opendroneid.h.*/ uint8_t authentication_data[23]; /*< Opaque authentication data. For page 0, the size is only 17 bytes. For other pages, the size is 23 bytes. Shall be filled with nulls in the unused portion of the field.*/ } mavlink_open_drone_id_authentication_t; @@ -21,8 +21,8 @@ typedef struct __mavlink_open_drone_id_authentication_t { #define MAVLINK_MSG_ID_12902_LEN 53 #define MAVLINK_MSG_ID_12902_MIN_LEN 53 -#define MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC 49 -#define MAVLINK_MSG_ID_12902_CRC 49 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC 140 +#define MAVLINK_MSG_ID_12902_CRC 140 #define MAVLINK_MSG_OPEN_DRONE_ID_AUTHENTICATION_FIELD_ID_OR_MAC_LEN 20 #define MAVLINK_MSG_OPEN_DRONE_ID_AUTHENTICATION_FIELD_AUTHENTICATION_DATA_LEN 23 @@ -37,7 +37,7 @@ typedef struct __mavlink_open_drone_id_authentication_t { { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 6, offsetof(mavlink_open_drone_id_authentication_t, id_or_mac) }, \ { "authentication_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_open_drone_id_authentication_t, authentication_type) }, \ { "data_page", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_open_drone_id_authentication_t, data_page) }, \ - { "page_count", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_open_drone_id_authentication_t, page_count) }, \ + { "last_page_index", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_open_drone_id_authentication_t, last_page_index) }, \ { "length", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_open_drone_id_authentication_t, length) }, \ { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_open_drone_id_authentication_t, timestamp) }, \ { "authentication_data", NULL, MAVLINK_TYPE_UINT8_T, 23, 30, offsetof(mavlink_open_drone_id_authentication_t, authentication_data) }, \ @@ -52,7 +52,7 @@ typedef struct __mavlink_open_drone_id_authentication_t { { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 6, offsetof(mavlink_open_drone_id_authentication_t, id_or_mac) }, \ { "authentication_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_open_drone_id_authentication_t, authentication_type) }, \ { "data_page", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_open_drone_id_authentication_t, data_page) }, \ - { "page_count", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_open_drone_id_authentication_t, page_count) }, \ + { "last_page_index", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_open_drone_id_authentication_t, last_page_index) }, \ { "length", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_open_drone_id_authentication_t, length) }, \ { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_open_drone_id_authentication_t, timestamp) }, \ { "authentication_data", NULL, MAVLINK_TYPE_UINT8_T, 23, 30, offsetof(mavlink_open_drone_id_authentication_t, authentication_data) }, \ @@ -70,15 +70,15 @@ typedef struct __mavlink_open_drone_id_authentication_t { * @param target_component Component ID (0 for broadcast). * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. * @param authentication_type Indicates the type of authentication. - * @param data_page Allowed range is 0 - 4. - * @param page_count This field is only present for page 0. Allowed range is 0 - 5. - * @param length [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. Allowed range is 0 - 109 (17 + 23*4). + * @param data_page Allowed range is 0 - 15. + * @param last_page_index This field is only present for page 0. Allowed range is 0 - 15. See the description of struct ODID_Auth_data at https://github.com/opendroneid/opendroneid-core-c/blob/master/libopendroneid/opendroneid.h. + * @param length [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. See the description of struct ODID_Auth_data at https://github.com/opendroneid/opendroneid-core-c/blob/master/libopendroneid/opendroneid.h. * @param timestamp [s] This field is only present for page 0. 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. * @param authentication_data Opaque authentication data. For page 0, the size is only 17 bytes. For other pages, the size is 23 bytes. Shall be filled with nulls in the unused portion of the field. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_open_drone_id_authentication_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t authentication_type, uint8_t data_page, uint8_t page_count, uint8_t length, uint32_t timestamp, const uint8_t *authentication_data) + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t authentication_type, uint8_t data_page, uint8_t last_page_index, uint8_t length, uint32_t timestamp, const uint8_t *authentication_data) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN]; @@ -87,7 +87,7 @@ static inline uint16_t mavlink_msg_open_drone_id_authentication_pack(uint8_t sys _mav_put_uint8_t(buf, 5, target_component); _mav_put_uint8_t(buf, 26, authentication_type); _mav_put_uint8_t(buf, 27, data_page); - _mav_put_uint8_t(buf, 28, page_count); + _mav_put_uint8_t(buf, 28, last_page_index); _mav_put_uint8_t(buf, 29, length); _mav_put_uint8_t_array(buf, 6, id_or_mac, 20); _mav_put_uint8_t_array(buf, 30, authentication_data, 23); @@ -99,7 +99,58 @@ static inline uint16_t mavlink_msg_open_drone_id_authentication_pack(uint8_t sys packet.target_component = target_component; packet.authentication_type = authentication_type; packet.data_page = data_page; - packet.page_count = page_count; + packet.last_page_index = last_page_index; + packet.length = length; + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); + mav_array_assign_uint8_t(packet.authentication_data, authentication_data, 23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); +} + +/** + * @brief Pack a open_drone_id_authentication message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param authentication_type Indicates the type of authentication. + * @param data_page Allowed range is 0 - 15. + * @param last_page_index This field is only present for page 0. Allowed range is 0 - 15. See the description of struct ODID_Auth_data at https://github.com/opendroneid/opendroneid-core-c/blob/master/libopendroneid/opendroneid.h. + * @param length [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. See the description of struct ODID_Auth_data at https://github.com/opendroneid/opendroneid-core-c/blob/master/libopendroneid/opendroneid.h. + * @param timestamp [s] This field is only present for page 0. 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + * @param authentication_data Opaque authentication data. For page 0, the size is only 17 bytes. For other pages, the size is 23 bytes. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_authentication_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t authentication_type, uint8_t data_page, uint8_t last_page_index, uint8_t length, uint32_t timestamp, const uint8_t *authentication_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN]; + _mav_put_uint32_t(buf, 0, timestamp); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 26, authentication_type); + _mav_put_uint8_t(buf, 27, data_page); + _mav_put_uint8_t(buf, 28, last_page_index); + _mav_put_uint8_t(buf, 29, length); + _mav_put_uint8_t_array(buf, 6, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 30, authentication_data, 23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN); +#else + mavlink_open_drone_id_authentication_t packet; + packet.timestamp = timestamp; + packet.target_system = target_system; + packet.target_component = target_component; + packet.authentication_type = authentication_type; + packet.data_page = data_page; + packet.last_page_index = last_page_index; packet.length = length; mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); mav_array_memcpy(packet.authentication_data, authentication_data, sizeof(uint8_t)*23); @@ -107,7 +158,11 @@ static inline uint16_t mavlink_msg_open_drone_id_authentication_pack(uint8_t sys #endif msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN); +#endif } /** @@ -120,16 +175,16 @@ static inline uint16_t mavlink_msg_open_drone_id_authentication_pack(uint8_t sys * @param target_component Component ID (0 for broadcast). * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. * @param authentication_type Indicates the type of authentication. - * @param data_page Allowed range is 0 - 4. - * @param page_count This field is only present for page 0. Allowed range is 0 - 5. - * @param length [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. Allowed range is 0 - 109 (17 + 23*4). + * @param data_page Allowed range is 0 - 15. + * @param last_page_index This field is only present for page 0. Allowed range is 0 - 15. See the description of struct ODID_Auth_data at https://github.com/opendroneid/opendroneid-core-c/blob/master/libopendroneid/opendroneid.h. + * @param length [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. See the description of struct ODID_Auth_data at https://github.com/opendroneid/opendroneid-core-c/blob/master/libopendroneid/opendroneid.h. * @param timestamp [s] This field is only present for page 0. 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. * @param authentication_data Opaque authentication data. For page 0, the size is only 17 bytes. For other pages, the size is 23 bytes. Shall be filled with nulls in the unused portion of the field. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_open_drone_id_authentication_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,const uint8_t *id_or_mac,uint8_t authentication_type,uint8_t data_page,uint8_t page_count,uint8_t length,uint32_t timestamp,const uint8_t *authentication_data) + uint8_t target_system,uint8_t target_component,const uint8_t *id_or_mac,uint8_t authentication_type,uint8_t data_page,uint8_t last_page_index,uint8_t length,uint32_t timestamp,const uint8_t *authentication_data) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN]; @@ -138,7 +193,7 @@ static inline uint16_t mavlink_msg_open_drone_id_authentication_pack_chan(uint8_ _mav_put_uint8_t(buf, 5, target_component); _mav_put_uint8_t(buf, 26, authentication_type); _mav_put_uint8_t(buf, 27, data_page); - _mav_put_uint8_t(buf, 28, page_count); + _mav_put_uint8_t(buf, 28, last_page_index); _mav_put_uint8_t(buf, 29, length); _mav_put_uint8_t_array(buf, 6, id_or_mac, 20); _mav_put_uint8_t_array(buf, 30, authentication_data, 23); @@ -150,10 +205,10 @@ static inline uint16_t mavlink_msg_open_drone_id_authentication_pack_chan(uint8_ packet.target_component = target_component; packet.authentication_type = authentication_type; packet.data_page = data_page; - packet.page_count = page_count; + packet.last_page_index = last_page_index; packet.length = length; - mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); - mav_array_memcpy(packet.authentication_data, authentication_data, sizeof(uint8_t)*23); + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); + mav_array_assign_uint8_t(packet.authentication_data, authentication_data, 23); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN); #endif @@ -171,7 +226,7 @@ static inline uint16_t mavlink_msg_open_drone_id_authentication_pack_chan(uint8_ */ static inline uint16_t mavlink_msg_open_drone_id_authentication_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_authentication_t* open_drone_id_authentication) { - return mavlink_msg_open_drone_id_authentication_pack(system_id, component_id, msg, open_drone_id_authentication->target_system, open_drone_id_authentication->target_component, open_drone_id_authentication->id_or_mac, open_drone_id_authentication->authentication_type, open_drone_id_authentication->data_page, open_drone_id_authentication->page_count, open_drone_id_authentication->length, open_drone_id_authentication->timestamp, open_drone_id_authentication->authentication_data); + return mavlink_msg_open_drone_id_authentication_pack(system_id, component_id, msg, open_drone_id_authentication->target_system, open_drone_id_authentication->target_component, open_drone_id_authentication->id_or_mac, open_drone_id_authentication->authentication_type, open_drone_id_authentication->data_page, open_drone_id_authentication->last_page_index, open_drone_id_authentication->length, open_drone_id_authentication->timestamp, open_drone_id_authentication->authentication_data); } /** @@ -185,7 +240,21 @@ static inline uint16_t mavlink_msg_open_drone_id_authentication_encode(uint8_t s */ static inline uint16_t mavlink_msg_open_drone_id_authentication_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_authentication_t* open_drone_id_authentication) { - return mavlink_msg_open_drone_id_authentication_pack_chan(system_id, component_id, chan, msg, open_drone_id_authentication->target_system, open_drone_id_authentication->target_component, open_drone_id_authentication->id_or_mac, open_drone_id_authentication->authentication_type, open_drone_id_authentication->data_page, open_drone_id_authentication->page_count, open_drone_id_authentication->length, open_drone_id_authentication->timestamp, open_drone_id_authentication->authentication_data); + return mavlink_msg_open_drone_id_authentication_pack_chan(system_id, component_id, chan, msg, open_drone_id_authentication->target_system, open_drone_id_authentication->target_component, open_drone_id_authentication->id_or_mac, open_drone_id_authentication->authentication_type, open_drone_id_authentication->data_page, open_drone_id_authentication->last_page_index, open_drone_id_authentication->length, open_drone_id_authentication->timestamp, open_drone_id_authentication->authentication_data); +} + +/** + * @brief Encode a open_drone_id_authentication struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_authentication C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_authentication_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_open_drone_id_authentication_t* open_drone_id_authentication) +{ + return mavlink_msg_open_drone_id_authentication_pack_status(system_id, component_id, _status, msg, open_drone_id_authentication->target_system, open_drone_id_authentication->target_component, open_drone_id_authentication->id_or_mac, open_drone_id_authentication->authentication_type, open_drone_id_authentication->data_page, open_drone_id_authentication->last_page_index, open_drone_id_authentication->length, open_drone_id_authentication->timestamp, open_drone_id_authentication->authentication_data); } /** @@ -196,15 +265,15 @@ static inline uint16_t mavlink_msg_open_drone_id_authentication_encode_chan(uint * @param target_component Component ID (0 for broadcast). * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. * @param authentication_type Indicates the type of authentication. - * @param data_page Allowed range is 0 - 4. - * @param page_count This field is only present for page 0. Allowed range is 0 - 5. - * @param length [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. Allowed range is 0 - 109 (17 + 23*4). + * @param data_page Allowed range is 0 - 15. + * @param last_page_index This field is only present for page 0. Allowed range is 0 - 15. See the description of struct ODID_Auth_data at https://github.com/opendroneid/opendroneid-core-c/blob/master/libopendroneid/opendroneid.h. + * @param length [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. See the description of struct ODID_Auth_data at https://github.com/opendroneid/opendroneid-core-c/blob/master/libopendroneid/opendroneid.h. * @param timestamp [s] This field is only present for page 0. 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. * @param authentication_data Opaque authentication data. For page 0, the size is only 17 bytes. For other pages, the size is 23 bytes. Shall be filled with nulls in the unused portion of the field. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_open_drone_id_authentication_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t authentication_type, uint8_t data_page, uint8_t page_count, uint8_t length, uint32_t timestamp, const uint8_t *authentication_data) +static inline void mavlink_msg_open_drone_id_authentication_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t authentication_type, uint8_t data_page, uint8_t last_page_index, uint8_t length, uint32_t timestamp, const uint8_t *authentication_data) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN]; @@ -213,7 +282,7 @@ static inline void mavlink_msg_open_drone_id_authentication_send(mavlink_channel _mav_put_uint8_t(buf, 5, target_component); _mav_put_uint8_t(buf, 26, authentication_type); _mav_put_uint8_t(buf, 27, data_page); - _mav_put_uint8_t(buf, 28, page_count); + _mav_put_uint8_t(buf, 28, last_page_index); _mav_put_uint8_t(buf, 29, length); _mav_put_uint8_t_array(buf, 6, id_or_mac, 20); _mav_put_uint8_t_array(buf, 30, authentication_data, 23); @@ -225,10 +294,10 @@ static inline void mavlink_msg_open_drone_id_authentication_send(mavlink_channel packet.target_component = target_component; packet.authentication_type = authentication_type; packet.data_page = data_page; - packet.page_count = page_count; + packet.last_page_index = last_page_index; packet.length = length; - mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); - mav_array_memcpy(packet.authentication_data, authentication_data, sizeof(uint8_t)*23); + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); + mav_array_assign_uint8_t(packet.authentication_data, authentication_data, 23); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); #endif } @@ -241,7 +310,7 @@ static inline void mavlink_msg_open_drone_id_authentication_send(mavlink_channel static inline void mavlink_msg_open_drone_id_authentication_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_authentication_t* open_drone_id_authentication) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_open_drone_id_authentication_send(chan, open_drone_id_authentication->target_system, open_drone_id_authentication->target_component, open_drone_id_authentication->id_or_mac, open_drone_id_authentication->authentication_type, open_drone_id_authentication->data_page, open_drone_id_authentication->page_count, open_drone_id_authentication->length, open_drone_id_authentication->timestamp, open_drone_id_authentication->authentication_data); + mavlink_msg_open_drone_id_authentication_send(chan, open_drone_id_authentication->target_system, open_drone_id_authentication->target_component, open_drone_id_authentication->id_or_mac, open_drone_id_authentication->authentication_type, open_drone_id_authentication->data_page, open_drone_id_authentication->last_page_index, open_drone_id_authentication->length, open_drone_id_authentication->timestamp, open_drone_id_authentication->authentication_data); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION, (const char *)open_drone_id_authentication, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); #endif @@ -249,13 +318,13 @@ static inline void mavlink_msg_open_drone_id_authentication_send_struct(mavlink_ #if MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_open_drone_id_authentication_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t authentication_type, uint8_t data_page, uint8_t page_count, uint8_t length, uint32_t timestamp, const uint8_t *authentication_data) +static inline void mavlink_msg_open_drone_id_authentication_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t authentication_type, uint8_t data_page, uint8_t last_page_index, uint8_t length, uint32_t timestamp, const uint8_t *authentication_data) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -264,7 +333,7 @@ static inline void mavlink_msg_open_drone_id_authentication_send_buf(mavlink_mes _mav_put_uint8_t(buf, 5, target_component); _mav_put_uint8_t(buf, 26, authentication_type); _mav_put_uint8_t(buf, 27, data_page); - _mav_put_uint8_t(buf, 28, page_count); + _mav_put_uint8_t(buf, 28, last_page_index); _mav_put_uint8_t(buf, 29, length); _mav_put_uint8_t_array(buf, 6, id_or_mac, 20); _mav_put_uint8_t_array(buf, 30, authentication_data, 23); @@ -276,10 +345,10 @@ static inline void mavlink_msg_open_drone_id_authentication_send_buf(mavlink_mes packet->target_component = target_component; packet->authentication_type = authentication_type; packet->data_page = data_page; - packet->page_count = page_count; + packet->last_page_index = last_page_index; packet->length = length; - mav_array_memcpy(packet->id_or_mac, id_or_mac, sizeof(uint8_t)*20); - mav_array_memcpy(packet->authentication_data, authentication_data, sizeof(uint8_t)*23); + mav_array_assign_uint8_t(packet->id_or_mac, id_or_mac, 20); + mav_array_assign_uint8_t(packet->authentication_data, authentication_data, 23); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); #endif } @@ -333,7 +402,7 @@ static inline uint8_t mavlink_msg_open_drone_id_authentication_get_authenticatio /** * @brief Get field data_page from open_drone_id_authentication message * - * @return Allowed range is 0 - 4. + * @return Allowed range is 0 - 15. */ static inline uint8_t mavlink_msg_open_drone_id_authentication_get_data_page(const mavlink_message_t* msg) { @@ -341,11 +410,11 @@ static inline uint8_t mavlink_msg_open_drone_id_authentication_get_data_page(con } /** - * @brief Get field page_count from open_drone_id_authentication message + * @brief Get field last_page_index from open_drone_id_authentication message * - * @return This field is only present for page 0. Allowed range is 0 - 5. + * @return This field is only present for page 0. Allowed range is 0 - 15. See the description of struct ODID_Auth_data at https://github.com/opendroneid/opendroneid-core-c/blob/master/libopendroneid/opendroneid.h. */ -static inline uint8_t mavlink_msg_open_drone_id_authentication_get_page_count(const mavlink_message_t* msg) +static inline uint8_t mavlink_msg_open_drone_id_authentication_get_last_page_index(const mavlink_message_t* msg) { return _MAV_RETURN_uint8_t(msg, 28); } @@ -353,7 +422,7 @@ static inline uint8_t mavlink_msg_open_drone_id_authentication_get_page_count(co /** * @brief Get field length from open_drone_id_authentication message * - * @return [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. Allowed range is 0 - 109 (17 + 23*4). + * @return [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. See the description of struct ODID_Auth_data at https://github.com/opendroneid/opendroneid-core-c/blob/master/libopendroneid/opendroneid.h. */ static inline uint8_t mavlink_msg_open_drone_id_authentication_get_length(const mavlink_message_t* msg) { @@ -395,7 +464,7 @@ static inline void mavlink_msg_open_drone_id_authentication_decode(const mavlink mavlink_msg_open_drone_id_authentication_get_id_or_mac(msg, open_drone_id_authentication->id_or_mac); open_drone_id_authentication->authentication_type = mavlink_msg_open_drone_id_authentication_get_authentication_type(msg); open_drone_id_authentication->data_page = mavlink_msg_open_drone_id_authentication_get_data_page(msg); - open_drone_id_authentication->page_count = mavlink_msg_open_drone_id_authentication_get_page_count(msg); + open_drone_id_authentication->last_page_index = mavlink_msg_open_drone_id_authentication_get_last_page_index(msg); open_drone_id_authentication->length = mavlink_msg_open_drone_id_authentication_get_length(msg); mavlink_msg_open_drone_id_authentication_get_authentication_data(msg, open_drone_id_authentication->authentication_data); #else diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_basic_id.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_basic_id.h index 1102960b710..6ec8c3feec2 100644 --- a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_basic_id.h +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_basic_id.h @@ -68,6 +68,48 @@ typedef struct __mavlink_open_drone_id_basic_id_t { static inline uint16_t mavlink_msg_open_drone_id_basic_id_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t id_type, uint8_t ua_type, const uint8_t *uas_id) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, id_type); + _mav_put_uint8_t(buf, 23, ua_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 24, uas_id, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN); +#else + mavlink_open_drone_id_basic_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.id_type = id_type; + packet.ua_type = ua_type; + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); + mav_array_assign_uint8_t(packet.uas_id, uas_id, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); +} + +/** + * @brief Pack a open_drone_id_basic_id message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param id_type Indicates the format for the uas_id field of this message. + * @param ua_type Indicates the type of UA (Unmanned Aircraft). + * @param uas_id UAS (Unmanned Aircraft System) ID following the format specified by id_type. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_basic_id_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t id_type, uint8_t ua_type, const uint8_t *uas_id) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN]; _mav_put_uint8_t(buf, 0, target_system); @@ -89,7 +131,11 @@ static inline uint16_t mavlink_msg_open_drone_id_basic_id_pack(uint8_t system_id #endif msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN); +#endif } /** @@ -125,8 +171,8 @@ static inline uint16_t mavlink_msg_open_drone_id_basic_id_pack_chan(uint8_t syst packet.target_component = target_component; packet.id_type = id_type; packet.ua_type = ua_type; - mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); - mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*20); + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); + mav_array_assign_uint8_t(packet.uas_id, uas_id, 20); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN); #endif @@ -161,6 +207,20 @@ static inline uint16_t mavlink_msg_open_drone_id_basic_id_encode_chan(uint8_t sy return mavlink_msg_open_drone_id_basic_id_pack_chan(system_id, component_id, chan, msg, open_drone_id_basic_id->target_system, open_drone_id_basic_id->target_component, open_drone_id_basic_id->id_or_mac, open_drone_id_basic_id->id_type, open_drone_id_basic_id->ua_type, open_drone_id_basic_id->uas_id); } +/** + * @brief Encode a open_drone_id_basic_id struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_basic_id C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_basic_id_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_open_drone_id_basic_id_t* open_drone_id_basic_id) +{ + return mavlink_msg_open_drone_id_basic_id_pack_status(system_id, component_id, _status, msg, open_drone_id_basic_id->target_system, open_drone_id_basic_id->target_component, open_drone_id_basic_id->id_or_mac, open_drone_id_basic_id->id_type, open_drone_id_basic_id->ua_type, open_drone_id_basic_id->uas_id); +} + /** * @brief Send a open_drone_id_basic_id message * @param chan MAVLink channel to send the message @@ -191,8 +251,8 @@ static inline void mavlink_msg_open_drone_id_basic_id_send(mavlink_channel_t cha packet.target_component = target_component; packet.id_type = id_type; packet.ua_type = ua_type; - mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); - mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*20); + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); + mav_array_assign_uint8_t(packet.uas_id, uas_id, 20); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); #endif } @@ -213,7 +273,7 @@ static inline void mavlink_msg_open_drone_id_basic_id_send_struct(mavlink_channe #if MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -236,8 +296,8 @@ static inline void mavlink_msg_open_drone_id_basic_id_send_buf(mavlink_message_t packet->target_component = target_component; packet->id_type = id_type; packet->ua_type = ua_type; - mav_array_memcpy(packet->id_or_mac, id_or_mac, sizeof(uint8_t)*20); - mav_array_memcpy(packet->uas_id, uas_id, sizeof(uint8_t)*20); + mav_array_assign_uint8_t(packet->id_or_mac, id_or_mac, 20); + mav_array_assign_uint8_t(packet->uas_id, uas_id, 20); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_location.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_location.h index 0c283439336..730b96a5c03 100644 --- a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_location.h +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_location.h @@ -7,10 +7,10 @@ typedef struct __mavlink_open_drone_id_location_t { int32_t latitude; /*< [degE7] Current latitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon).*/ int32_t longitude; /*< [degE7] Current longitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon).*/ - float altitude_barometric; /*< [m] The altitude calculated from the barometric pressue. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m.*/ + float altitude_barometric; /*< [m] The altitude calculated from the barometric pressure. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m.*/ float altitude_geodetic; /*< [m] The geodetic altitude as defined by WGS84. If unknown: -1000 m.*/ float height; /*< [m] The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference. If unknown: -1000 m.*/ - float timestamp; /*< [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000.*/ + float timestamp; /*< [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. If unknown: 0xFFFF.*/ uint16_t direction; /*< [cdeg] Direction over ground (not heading, but direction of movement) measured clockwise from true North: 0 - 35999 centi-degrees. If unknown: 36100 centi-degrees.*/ uint16_t speed_horizontal; /*< [cm/s] Ground speed. Positive only. If unknown: 25500 cm/s. If speed is larger than 25425 cm/s, use 25425 cm/s.*/ int16_t speed_vertical; /*< [cm/s] The vertical speed. Up is positive. If unknown: 6300 cm/s. If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s.*/ @@ -104,7 +104,7 @@ typedef struct __mavlink_open_drone_id_location_t { * @param speed_vertical [cm/s] The vertical speed. Up is positive. If unknown: 6300 cm/s. If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s. * @param latitude [degE7] Current latitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). * @param longitude [degE7] Current longitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). - * @param altitude_barometric [m] The altitude calculated from the barometric pressue. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. + * @param altitude_barometric [m] The altitude calculated from the barometric pressure. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. * @param altitude_geodetic [m] The geodetic altitude as defined by WGS84. If unknown: -1000 m. * @param height_reference Indicates the reference point for the height field. * @param height [m] The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference. If unknown: -1000 m. @@ -112,7 +112,7 @@ typedef struct __mavlink_open_drone_id_location_t { * @param vertical_accuracy The accuracy of the vertical position. * @param barometer_accuracy The accuracy of the barometric altitude. * @param speed_accuracy The accuracy of the horizontal and vertical speed. - * @param timestamp [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. + * @param timestamp [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. If unknown: 0xFFFF. * @param timestamp_accuracy The accuracy of the timestamps. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_open_drone_id_location_pack(uint8_t system_id packet.barometer_accuracy = barometer_accuracy; packet.speed_accuracy = speed_accuracy; packet.timestamp_accuracy = timestamp_accuracy; - mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN); #endif @@ -169,6 +169,91 @@ static inline uint16_t mavlink_msg_open_drone_id_location_pack(uint8_t system_id return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC); } +/** + * @brief Pack a open_drone_id_location message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param status Indicates whether the unmanned aircraft is on the ground or in the air. + * @param direction [cdeg] Direction over ground (not heading, but direction of movement) measured clockwise from true North: 0 - 35999 centi-degrees. If unknown: 36100 centi-degrees. + * @param speed_horizontal [cm/s] Ground speed. Positive only. If unknown: 25500 cm/s. If speed is larger than 25425 cm/s, use 25425 cm/s. + * @param speed_vertical [cm/s] The vertical speed. Up is positive. If unknown: 6300 cm/s. If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s. + * @param latitude [degE7] Current latitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + * @param longitude [degE7] Current longitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + * @param altitude_barometric [m] The altitude calculated from the barometric pressure. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. + * @param altitude_geodetic [m] The geodetic altitude as defined by WGS84. If unknown: -1000 m. + * @param height_reference Indicates the reference point for the height field. + * @param height [m] The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference. If unknown: -1000 m. + * @param horizontal_accuracy The accuracy of the horizontal position. + * @param vertical_accuracy The accuracy of the vertical position. + * @param barometer_accuracy The accuracy of the barometric altitude. + * @param speed_accuracy The accuracy of the horizontal and vertical speed. + * @param timestamp [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. If unknown: 0xFFFF. + * @param timestamp_accuracy The accuracy of the timestamps. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_location_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t status, uint16_t direction, uint16_t speed_horizontal, int16_t speed_vertical, int32_t latitude, int32_t longitude, float altitude_barometric, float altitude_geodetic, uint8_t height_reference, float height, uint8_t horizontal_accuracy, uint8_t vertical_accuracy, uint8_t barometer_accuracy, uint8_t speed_accuracy, float timestamp, uint8_t timestamp_accuracy) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_float(buf, 8, altitude_barometric); + _mav_put_float(buf, 12, altitude_geodetic); + _mav_put_float(buf, 16, height); + _mav_put_float(buf, 20, timestamp); + _mav_put_uint16_t(buf, 24, direction); + _mav_put_uint16_t(buf, 26, speed_horizontal); + _mav_put_int16_t(buf, 28, speed_vertical); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 52, status); + _mav_put_uint8_t(buf, 53, height_reference); + _mav_put_uint8_t(buf, 54, horizontal_accuracy); + _mav_put_uint8_t(buf, 55, vertical_accuracy); + _mav_put_uint8_t(buf, 56, barometer_accuracy); + _mav_put_uint8_t(buf, 57, speed_accuracy); + _mav_put_uint8_t(buf, 58, timestamp_accuracy); + _mav_put_uint8_t_array(buf, 32, id_or_mac, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN); +#else + mavlink_open_drone_id_location_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude_barometric = altitude_barometric; + packet.altitude_geodetic = altitude_geodetic; + packet.height = height; + packet.timestamp = timestamp; + packet.direction = direction; + packet.speed_horizontal = speed_horizontal; + packet.speed_vertical = speed_vertical; + packet.target_system = target_system; + packet.target_component = target_component; + packet.status = status; + packet.height_reference = height_reference; + packet.horizontal_accuracy = horizontal_accuracy; + packet.vertical_accuracy = vertical_accuracy; + packet.barometer_accuracy = barometer_accuracy; + packet.speed_accuracy = speed_accuracy; + packet.timestamp_accuracy = timestamp_accuracy; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN); +#endif +} + /** * @brief Pack a open_drone_id_location message on a channel * @param system_id ID of this system @@ -184,7 +269,7 @@ static inline uint16_t mavlink_msg_open_drone_id_location_pack(uint8_t system_id * @param speed_vertical [cm/s] The vertical speed. Up is positive. If unknown: 6300 cm/s. If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s. * @param latitude [degE7] Current latitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). * @param longitude [degE7] Current longitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). - * @param altitude_barometric [m] The altitude calculated from the barometric pressue. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. + * @param altitude_barometric [m] The altitude calculated from the barometric pressure. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. * @param altitude_geodetic [m] The geodetic altitude as defined by WGS84. If unknown: -1000 m. * @param height_reference Indicates the reference point for the height field. * @param height [m] The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference. If unknown: -1000 m. @@ -192,7 +277,7 @@ static inline uint16_t mavlink_msg_open_drone_id_location_pack(uint8_t system_id * @param vertical_accuracy The accuracy of the vertical position. * @param barometer_accuracy The accuracy of the barometric altitude. * @param speed_accuracy The accuracy of the horizontal and vertical speed. - * @param timestamp [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. + * @param timestamp [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. If unknown: 0xFFFF. * @param timestamp_accuracy The accuracy of the timestamps. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -242,7 +327,7 @@ static inline uint16_t mavlink_msg_open_drone_id_location_pack_chan(uint8_t syst packet.barometer_accuracy = barometer_accuracy; packet.speed_accuracy = speed_accuracy; packet.timestamp_accuracy = timestamp_accuracy; - mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN); #endif @@ -277,6 +362,20 @@ static inline uint16_t mavlink_msg_open_drone_id_location_encode_chan(uint8_t sy return mavlink_msg_open_drone_id_location_pack_chan(system_id, component_id, chan, msg, open_drone_id_location->target_system, open_drone_id_location->target_component, open_drone_id_location->id_or_mac, open_drone_id_location->status, open_drone_id_location->direction, open_drone_id_location->speed_horizontal, open_drone_id_location->speed_vertical, open_drone_id_location->latitude, open_drone_id_location->longitude, open_drone_id_location->altitude_barometric, open_drone_id_location->altitude_geodetic, open_drone_id_location->height_reference, open_drone_id_location->height, open_drone_id_location->horizontal_accuracy, open_drone_id_location->vertical_accuracy, open_drone_id_location->barometer_accuracy, open_drone_id_location->speed_accuracy, open_drone_id_location->timestamp, open_drone_id_location->timestamp_accuracy); } +/** + * @brief Encode a open_drone_id_location struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_location C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_location_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_open_drone_id_location_t* open_drone_id_location) +{ + return mavlink_msg_open_drone_id_location_pack_status(system_id, component_id, _status, msg, open_drone_id_location->target_system, open_drone_id_location->target_component, open_drone_id_location->id_or_mac, open_drone_id_location->status, open_drone_id_location->direction, open_drone_id_location->speed_horizontal, open_drone_id_location->speed_vertical, open_drone_id_location->latitude, open_drone_id_location->longitude, open_drone_id_location->altitude_barometric, open_drone_id_location->altitude_geodetic, open_drone_id_location->height_reference, open_drone_id_location->height, open_drone_id_location->horizontal_accuracy, open_drone_id_location->vertical_accuracy, open_drone_id_location->barometer_accuracy, open_drone_id_location->speed_accuracy, open_drone_id_location->timestamp, open_drone_id_location->timestamp_accuracy); +} + /** * @brief Send a open_drone_id_location message * @param chan MAVLink channel to send the message @@ -290,7 +389,7 @@ static inline uint16_t mavlink_msg_open_drone_id_location_encode_chan(uint8_t sy * @param speed_vertical [cm/s] The vertical speed. Up is positive. If unknown: 6300 cm/s. If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s. * @param latitude [degE7] Current latitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). * @param longitude [degE7] Current longitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). - * @param altitude_barometric [m] The altitude calculated from the barometric pressue. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. + * @param altitude_barometric [m] The altitude calculated from the barometric pressure. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. * @param altitude_geodetic [m] The geodetic altitude as defined by WGS84. If unknown: -1000 m. * @param height_reference Indicates the reference point for the height field. * @param height [m] The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference. If unknown: -1000 m. @@ -298,7 +397,7 @@ static inline uint16_t mavlink_msg_open_drone_id_location_encode_chan(uint8_t sy * @param vertical_accuracy The accuracy of the vertical position. * @param barometer_accuracy The accuracy of the barometric altitude. * @param speed_accuracy The accuracy of the horizontal and vertical speed. - * @param timestamp [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. + * @param timestamp [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. If unknown: 0xFFFF. * @param timestamp_accuracy The accuracy of the timestamps. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -347,7 +446,7 @@ static inline void mavlink_msg_open_drone_id_location_send(mavlink_channel_t cha packet.barometer_accuracy = barometer_accuracy; packet.speed_accuracy = speed_accuracy; packet.timestamp_accuracy = timestamp_accuracy; - mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC); #endif } @@ -368,7 +467,7 @@ static inline void mavlink_msg_open_drone_id_location_send_struct(mavlink_channe #if MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -418,7 +517,7 @@ static inline void mavlink_msg_open_drone_id_location_send_buf(mavlink_message_t packet->barometer_accuracy = barometer_accuracy; packet->speed_accuracy = speed_accuracy; packet->timestamp_accuracy = timestamp_accuracy; - mav_array_memcpy(packet->id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_assign_uint8_t(packet->id_or_mac, id_or_mac, 20); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC); #endif } @@ -522,7 +621,7 @@ static inline int32_t mavlink_msg_open_drone_id_location_get_longitude(const mav /** * @brief Get field altitude_barometric from open_drone_id_location message * - * @return [m] The altitude calculated from the barometric pressue. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. + * @return [m] The altitude calculated from the barometric pressure. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. */ static inline float mavlink_msg_open_drone_id_location_get_altitude_barometric(const mavlink_message_t* msg) { @@ -602,7 +701,7 @@ static inline uint8_t mavlink_msg_open_drone_id_location_get_speed_accuracy(cons /** * @brief Get field timestamp from open_drone_id_location message * - * @return [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. + * @return [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. If unknown: 0xFFFF. */ static inline float mavlink_msg_open_drone_id_location_get_timestamp(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_message_pack.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_message_pack.h index 16407cfb358..80e0962449a 100644 --- a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_message_pack.h +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_message_pack.h @@ -7,42 +7,46 @@ typedef struct __mavlink_open_drone_id_message_pack_t { uint8_t target_system; /*< System ID (0 for broadcast).*/ uint8_t target_component; /*< Component ID (0 for broadcast).*/ - uint8_t single_message_size; /*< [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specificed to have this length.*/ - uint8_t msg_pack_size; /*< Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 10.*/ - uint8_t messages[250]; /*< Concatenation of encoded OpenDroneID messages. Shall be filled with nulls in the unused portion of the field.*/ + uint8_t id_or_mac[20]; /*< Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */ + uint8_t single_message_size; /*< [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specified to have this length.*/ + uint8_t msg_pack_size; /*< Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 9.*/ + uint8_t messages[225]; /*< Concatenation of encoded OpenDroneID messages. Shall be filled with nulls in the unused portion of the field.*/ } mavlink_open_drone_id_message_pack_t; -#define MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN 254 -#define MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN 254 -#define MAVLINK_MSG_ID_12915_LEN 254 -#define MAVLINK_MSG_ID_12915_MIN_LEN 254 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN 249 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN 249 +#define MAVLINK_MSG_ID_12915_LEN 249 +#define MAVLINK_MSG_ID_12915_MIN_LEN 249 -#define MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC 62 -#define MAVLINK_MSG_ID_12915_CRC 62 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC 94 +#define MAVLINK_MSG_ID_12915_CRC 94 -#define MAVLINK_MSG_OPEN_DRONE_ID_MESSAGE_PACK_FIELD_MESSAGES_LEN 250 +#define MAVLINK_MSG_OPEN_DRONE_ID_MESSAGE_PACK_FIELD_ID_OR_MAC_LEN 20 +#define MAVLINK_MSG_OPEN_DRONE_ID_MESSAGE_PACK_FIELD_MESSAGES_LEN 225 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_MESSAGE_PACK { \ 12915, \ "OPEN_DRONE_ID_MESSAGE_PACK", \ - 5, \ + 6, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_message_pack_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_open_drone_id_message_pack_t, target_component) }, \ - { "single_message_size", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_open_drone_id_message_pack_t, single_message_size) }, \ - { "msg_pack_size", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_open_drone_id_message_pack_t, msg_pack_size) }, \ - { "messages", NULL, MAVLINK_TYPE_UINT8_T, 250, 4, offsetof(mavlink_open_drone_id_message_pack_t, messages) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 2, offsetof(mavlink_open_drone_id_message_pack_t, id_or_mac) }, \ + { "single_message_size", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_open_drone_id_message_pack_t, single_message_size) }, \ + { "msg_pack_size", NULL, MAVLINK_TYPE_UINT8_T, 0, 23, offsetof(mavlink_open_drone_id_message_pack_t, msg_pack_size) }, \ + { "messages", NULL, MAVLINK_TYPE_UINT8_T, 225, 24, offsetof(mavlink_open_drone_id_message_pack_t, messages) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_MESSAGE_PACK { \ "OPEN_DRONE_ID_MESSAGE_PACK", \ - 5, \ + 6, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_message_pack_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_open_drone_id_message_pack_t, target_component) }, \ - { "single_message_size", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_open_drone_id_message_pack_t, single_message_size) }, \ - { "msg_pack_size", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_open_drone_id_message_pack_t, msg_pack_size) }, \ - { "messages", NULL, MAVLINK_TYPE_UINT8_T, 250, 4, offsetof(mavlink_open_drone_id_message_pack_t, messages) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 2, offsetof(mavlink_open_drone_id_message_pack_t, id_or_mac) }, \ + { "single_message_size", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_open_drone_id_message_pack_t, single_message_size) }, \ + { "msg_pack_size", NULL, MAVLINK_TYPE_UINT8_T, 0, 23, offsetof(mavlink_open_drone_id_message_pack_t, msg_pack_size) }, \ + { "messages", NULL, MAVLINK_TYPE_UINT8_T, 225, 24, offsetof(mavlink_open_drone_id_message_pack_t, messages) }, \ } \ } #endif @@ -55,21 +59,23 @@ typedef struct __mavlink_open_drone_id_message_pack_t { * * @param target_system System ID (0 for broadcast). * @param target_component Component ID (0 for broadcast). - * @param single_message_size [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specificed to have this length. - * @param msg_pack_size Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 10. + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param single_message_size [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specified to have this length. + * @param msg_pack_size Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 9. * @param messages Concatenation of encoded OpenDroneID messages. Shall be filled with nulls in the unused portion of the field. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_open_drone_id_message_pack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t single_message_size, uint8_t msg_pack_size, const uint8_t *messages) + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t single_message_size, uint8_t msg_pack_size, const uint8_t *messages) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, single_message_size); - _mav_put_uint8_t(buf, 3, msg_pack_size); - _mav_put_uint8_t_array(buf, 4, messages, 250); + _mav_put_uint8_t(buf, 22, single_message_size); + _mav_put_uint8_t(buf, 23, msg_pack_size); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 24, messages, 225); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN); #else mavlink_open_drone_id_message_pack_t packet; @@ -77,7 +83,8 @@ static inline uint16_t mavlink_msg_open_drone_id_message_pack_pack(uint8_t syste packet.target_component = target_component; packet.single_message_size = single_message_size; packet.msg_pack_size = msg_pack_size; - mav_array_memcpy(packet.messages, messages, sizeof(uint8_t)*250); + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); + mav_array_assign_uint8_t(packet.messages, messages, 225); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN); #endif @@ -85,6 +92,52 @@ static inline uint16_t mavlink_msg_open_drone_id_message_pack_pack(uint8_t syste return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); } +/** + * @brief Pack a open_drone_id_message_pack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param single_message_size [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specified to have this length. + * @param msg_pack_size Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 9. + * @param messages Concatenation of encoded OpenDroneID messages. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_message_pack_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t single_message_size, uint8_t msg_pack_size, const uint8_t *messages) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, single_message_size); + _mav_put_uint8_t(buf, 23, msg_pack_size); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 24, messages, 225); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN); +#else + mavlink_open_drone_id_message_pack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.single_message_size = single_message_size; + packet.msg_pack_size = msg_pack_size; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.messages, messages, sizeof(uint8_t)*225); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN); +#endif +} + /** * @brief Pack a open_drone_id_message_pack message on a channel * @param system_id ID of this system @@ -93,22 +146,24 @@ static inline uint16_t mavlink_msg_open_drone_id_message_pack_pack(uint8_t syste * @param msg The MAVLink message to compress the data into * @param target_system System ID (0 for broadcast). * @param target_component Component ID (0 for broadcast). - * @param single_message_size [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specificed to have this length. - * @param msg_pack_size Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 10. + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param single_message_size [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specified to have this length. + * @param msg_pack_size Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 9. * @param messages Concatenation of encoded OpenDroneID messages. Shall be filled with nulls in the unused portion of the field. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_open_drone_id_message_pack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t single_message_size,uint8_t msg_pack_size,const uint8_t *messages) + uint8_t target_system,uint8_t target_component,const uint8_t *id_or_mac,uint8_t single_message_size,uint8_t msg_pack_size,const uint8_t *messages) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, single_message_size); - _mav_put_uint8_t(buf, 3, msg_pack_size); - _mav_put_uint8_t_array(buf, 4, messages, 250); + _mav_put_uint8_t(buf, 22, single_message_size); + _mav_put_uint8_t(buf, 23, msg_pack_size); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 24, messages, 225); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN); #else mavlink_open_drone_id_message_pack_t packet; @@ -116,7 +171,8 @@ static inline uint16_t mavlink_msg_open_drone_id_message_pack_pack_chan(uint8_t packet.target_component = target_component; packet.single_message_size = single_message_size; packet.msg_pack_size = msg_pack_size; - mav_array_memcpy(packet.messages, messages, sizeof(uint8_t)*250); + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); + mav_array_assign_uint8_t(packet.messages, messages, 225); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN); #endif @@ -134,7 +190,7 @@ static inline uint16_t mavlink_msg_open_drone_id_message_pack_pack_chan(uint8_t */ static inline uint16_t mavlink_msg_open_drone_id_message_pack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_message_pack_t* open_drone_id_message_pack) { - return mavlink_msg_open_drone_id_message_pack_pack(system_id, component_id, msg, open_drone_id_message_pack->target_system, open_drone_id_message_pack->target_component, open_drone_id_message_pack->single_message_size, open_drone_id_message_pack->msg_pack_size, open_drone_id_message_pack->messages); + return mavlink_msg_open_drone_id_message_pack_pack(system_id, component_id, msg, open_drone_id_message_pack->target_system, open_drone_id_message_pack->target_component, open_drone_id_message_pack->id_or_mac, open_drone_id_message_pack->single_message_size, open_drone_id_message_pack->msg_pack_size, open_drone_id_message_pack->messages); } /** @@ -148,7 +204,21 @@ static inline uint16_t mavlink_msg_open_drone_id_message_pack_encode(uint8_t sys */ static inline uint16_t mavlink_msg_open_drone_id_message_pack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_message_pack_t* open_drone_id_message_pack) { - return mavlink_msg_open_drone_id_message_pack_pack_chan(system_id, component_id, chan, msg, open_drone_id_message_pack->target_system, open_drone_id_message_pack->target_component, open_drone_id_message_pack->single_message_size, open_drone_id_message_pack->msg_pack_size, open_drone_id_message_pack->messages); + return mavlink_msg_open_drone_id_message_pack_pack_chan(system_id, component_id, chan, msg, open_drone_id_message_pack->target_system, open_drone_id_message_pack->target_component, open_drone_id_message_pack->id_or_mac, open_drone_id_message_pack->single_message_size, open_drone_id_message_pack->msg_pack_size, open_drone_id_message_pack->messages); +} + +/** + * @brief Encode a open_drone_id_message_pack struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_message_pack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_message_pack_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_open_drone_id_message_pack_t* open_drone_id_message_pack) +{ + return mavlink_msg_open_drone_id_message_pack_pack_status(system_id, component_id, _status, msg, open_drone_id_message_pack->target_system, open_drone_id_message_pack->target_component, open_drone_id_message_pack->id_or_mac, open_drone_id_message_pack->single_message_size, open_drone_id_message_pack->msg_pack_size, open_drone_id_message_pack->messages); } /** @@ -157,21 +227,23 @@ static inline uint16_t mavlink_msg_open_drone_id_message_pack_encode_chan(uint8_ * * @param target_system System ID (0 for broadcast). * @param target_component Component ID (0 for broadcast). - * @param single_message_size [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specificed to have this length. - * @param msg_pack_size Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 10. + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param single_message_size [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specified to have this length. + * @param msg_pack_size Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 9. * @param messages Concatenation of encoded OpenDroneID messages. Shall be filled with nulls in the unused portion of the field. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_open_drone_id_message_pack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t single_message_size, uint8_t msg_pack_size, const uint8_t *messages) +static inline void mavlink_msg_open_drone_id_message_pack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t single_message_size, uint8_t msg_pack_size, const uint8_t *messages) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, single_message_size); - _mav_put_uint8_t(buf, 3, msg_pack_size); - _mav_put_uint8_t_array(buf, 4, messages, 250); + _mav_put_uint8_t(buf, 22, single_message_size); + _mav_put_uint8_t(buf, 23, msg_pack_size); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 24, messages, 225); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); #else mavlink_open_drone_id_message_pack_t packet; @@ -179,7 +251,8 @@ static inline void mavlink_msg_open_drone_id_message_pack_send(mavlink_channel_t packet.target_component = target_component; packet.single_message_size = single_message_size; packet.msg_pack_size = msg_pack_size; - mav_array_memcpy(packet.messages, messages, sizeof(uint8_t)*250); + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); + mav_array_assign_uint8_t(packet.messages, messages, 225); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); #endif } @@ -192,7 +265,7 @@ static inline void mavlink_msg_open_drone_id_message_pack_send(mavlink_channel_t static inline void mavlink_msg_open_drone_id_message_pack_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_message_pack_t* open_drone_id_message_pack) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_open_drone_id_message_pack_send(chan, open_drone_id_message_pack->target_system, open_drone_id_message_pack->target_component, open_drone_id_message_pack->single_message_size, open_drone_id_message_pack->msg_pack_size, open_drone_id_message_pack->messages); + mavlink_msg_open_drone_id_message_pack_send(chan, open_drone_id_message_pack->target_system, open_drone_id_message_pack->target_component, open_drone_id_message_pack->id_or_mac, open_drone_id_message_pack->single_message_size, open_drone_id_message_pack->msg_pack_size, open_drone_id_message_pack->messages); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK, (const char *)open_drone_id_message_pack, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); #endif @@ -200,21 +273,22 @@ static inline void mavlink_msg_open_drone_id_message_pack_send_struct(mavlink_ch #if MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_open_drone_id_message_pack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t single_message_size, uint8_t msg_pack_size, const uint8_t *messages) +static inline void mavlink_msg_open_drone_id_message_pack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t single_message_size, uint8_t msg_pack_size, const uint8_t *messages) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, single_message_size); - _mav_put_uint8_t(buf, 3, msg_pack_size); - _mav_put_uint8_t_array(buf, 4, messages, 250); + _mav_put_uint8_t(buf, 22, single_message_size); + _mav_put_uint8_t(buf, 23, msg_pack_size); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 24, messages, 225); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); #else mavlink_open_drone_id_message_pack_t *packet = (mavlink_open_drone_id_message_pack_t *)msgbuf; @@ -222,7 +296,8 @@ static inline void mavlink_msg_open_drone_id_message_pack_send_buf(mavlink_messa packet->target_component = target_component; packet->single_message_size = single_message_size; packet->msg_pack_size = msg_pack_size; - mav_array_memcpy(packet->messages, messages, sizeof(uint8_t)*250); + mav_array_assign_uint8_t(packet->id_or_mac, id_or_mac, 20); + mav_array_assign_uint8_t(packet->messages, messages, 225); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); #endif } @@ -253,24 +328,34 @@ static inline uint8_t mavlink_msg_open_drone_id_message_pack_get_target_componen return _MAV_RETURN_uint8_t(msg, 1); } +/** + * @brief Get field id_or_mac from open_drone_id_message_pack message + * + * @return Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + */ +static inline uint16_t mavlink_msg_open_drone_id_message_pack_get_id_or_mac(const mavlink_message_t* msg, uint8_t *id_or_mac) +{ + return _MAV_RETURN_uint8_t_array(msg, id_or_mac, 20, 2); +} + /** * @brief Get field single_message_size from open_drone_id_message_pack message * - * @return [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specificed to have this length. + * @return [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specified to have this length. */ static inline uint8_t mavlink_msg_open_drone_id_message_pack_get_single_message_size(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 22); } /** * @brief Get field msg_pack_size from open_drone_id_message_pack message * - * @return Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 10. + * @return Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 9. */ static inline uint8_t mavlink_msg_open_drone_id_message_pack_get_msg_pack_size(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 3); + return _MAV_RETURN_uint8_t(msg, 23); } /** @@ -280,7 +365,7 @@ static inline uint8_t mavlink_msg_open_drone_id_message_pack_get_msg_pack_size(c */ static inline uint16_t mavlink_msg_open_drone_id_message_pack_get_messages(const mavlink_message_t* msg, uint8_t *messages) { - return _MAV_RETURN_uint8_t_array(msg, messages, 250, 4); + return _MAV_RETURN_uint8_t_array(msg, messages, 225, 24); } /** @@ -294,6 +379,7 @@ static inline void mavlink_msg_open_drone_id_message_pack_decode(const mavlink_m #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS open_drone_id_message_pack->target_system = mavlink_msg_open_drone_id_message_pack_get_target_system(msg); open_drone_id_message_pack->target_component = mavlink_msg_open_drone_id_message_pack_get_target_component(msg); + mavlink_msg_open_drone_id_message_pack_get_id_or_mac(msg, open_drone_id_message_pack->id_or_mac); open_drone_id_message_pack->single_message_size = mavlink_msg_open_drone_id_message_pack_get_single_message_size(msg); open_drone_id_message_pack->msg_pack_size = mavlink_msg_open_drone_id_message_pack_get_msg_pack_size(msg); mavlink_msg_open_drone_id_message_pack_get_messages(msg, open_drone_id_message_pack->messages); diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_operator_id.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_operator_id.h index f3bf5aff296..2d9b3f779f8 100644 --- a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_operator_id.h +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_operator_id.h @@ -64,6 +64,45 @@ typedef struct __mavlink_open_drone_id_operator_id_t { static inline uint16_t mavlink_msg_open_drone_id_operator_id_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_id_type, const char *operator_id) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, operator_id_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_char_array(buf, 23, operator_id, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN); +#else + mavlink_open_drone_id_operator_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.operator_id_type = operator_id_type; + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); + mav_array_assign_char(packet.operator_id, operator_id, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); +} + +/** + * @brief Pack a open_drone_id_operator_id message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param operator_id_type Indicates the type of the operator_id field. + * @param operator_id Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_operator_id_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_id_type, const char *operator_id) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN]; _mav_put_uint8_t(buf, 0, target_system); @@ -83,7 +122,11 @@ static inline uint16_t mavlink_msg_open_drone_id_operator_id_pack(uint8_t system #endif msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN); +#endif } /** @@ -116,8 +159,8 @@ static inline uint16_t mavlink_msg_open_drone_id_operator_id_pack_chan(uint8_t s packet.target_system = target_system; packet.target_component = target_component; packet.operator_id_type = operator_id_type; - mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); - mav_array_memcpy(packet.operator_id, operator_id, sizeof(char)*20); + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); + mav_array_assign_char(packet.operator_id, operator_id, 20); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN); #endif @@ -152,6 +195,20 @@ static inline uint16_t mavlink_msg_open_drone_id_operator_id_encode_chan(uint8_t return mavlink_msg_open_drone_id_operator_id_pack_chan(system_id, component_id, chan, msg, open_drone_id_operator_id->target_system, open_drone_id_operator_id->target_component, open_drone_id_operator_id->id_or_mac, open_drone_id_operator_id->operator_id_type, open_drone_id_operator_id->operator_id); } +/** + * @brief Encode a open_drone_id_operator_id struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_operator_id C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_operator_id_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_open_drone_id_operator_id_t* open_drone_id_operator_id) +{ + return mavlink_msg_open_drone_id_operator_id_pack_status(system_id, component_id, _status, msg, open_drone_id_operator_id->target_system, open_drone_id_operator_id->target_component, open_drone_id_operator_id->id_or_mac, open_drone_id_operator_id->operator_id_type, open_drone_id_operator_id->operator_id); +} + /** * @brief Send a open_drone_id_operator_id message * @param chan MAVLink channel to send the message @@ -179,8 +236,8 @@ static inline void mavlink_msg_open_drone_id_operator_id_send(mavlink_channel_t packet.target_system = target_system; packet.target_component = target_component; packet.operator_id_type = operator_id_type; - mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); - mav_array_memcpy(packet.operator_id, operator_id, sizeof(char)*20); + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); + mav_array_assign_char(packet.operator_id, operator_id, 20); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); #endif } @@ -201,7 +258,7 @@ static inline void mavlink_msg_open_drone_id_operator_id_send_struct(mavlink_cha #if MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -222,8 +279,8 @@ static inline void mavlink_msg_open_drone_id_operator_id_send_buf(mavlink_messag packet->target_system = target_system; packet->target_component = target_component; packet->operator_id_type = operator_id_type; - mav_array_memcpy(packet->id_or_mac, id_or_mac, sizeof(uint8_t)*20); - mav_array_memcpy(packet->operator_id, operator_id, sizeof(char)*20); + mav_array_assign_uint8_t(packet->id_or_mac, id_or_mac, 20); + mav_array_assign_char(packet->operator_id, operator_id, 20); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_self_id.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_self_id.h index 0750472ace1..67ce4f5a904 100644 --- a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_self_id.h +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_self_id.h @@ -64,6 +64,45 @@ typedef struct __mavlink_open_drone_id_self_id_t { static inline uint16_t mavlink_msg_open_drone_id_self_id_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t description_type, const char *description) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, description_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_char_array(buf, 23, description, 23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN); +#else + mavlink_open_drone_id_self_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.description_type = description_type; + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); + mav_array_assign_char(packet.description, description, 23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); +} + +/** + * @brief Pack a open_drone_id_self_id message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param description_type Indicates the type of the description field. + * @param description Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_self_id_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t description_type, const char *description) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN]; _mav_put_uint8_t(buf, 0, target_system); @@ -83,7 +122,11 @@ static inline uint16_t mavlink_msg_open_drone_id_self_id_pack(uint8_t system_id, #endif msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN); +#endif } /** @@ -116,8 +159,8 @@ static inline uint16_t mavlink_msg_open_drone_id_self_id_pack_chan(uint8_t syste packet.target_system = target_system; packet.target_component = target_component; packet.description_type = description_type; - mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); - mav_array_memcpy(packet.description, description, sizeof(char)*23); + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); + mav_array_assign_char(packet.description, description, 23); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN); #endif @@ -152,6 +195,20 @@ static inline uint16_t mavlink_msg_open_drone_id_self_id_encode_chan(uint8_t sys return mavlink_msg_open_drone_id_self_id_pack_chan(system_id, component_id, chan, msg, open_drone_id_self_id->target_system, open_drone_id_self_id->target_component, open_drone_id_self_id->id_or_mac, open_drone_id_self_id->description_type, open_drone_id_self_id->description); } +/** + * @brief Encode a open_drone_id_self_id struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_self_id C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_self_id_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_open_drone_id_self_id_t* open_drone_id_self_id) +{ + return mavlink_msg_open_drone_id_self_id_pack_status(system_id, component_id, _status, msg, open_drone_id_self_id->target_system, open_drone_id_self_id->target_component, open_drone_id_self_id->id_or_mac, open_drone_id_self_id->description_type, open_drone_id_self_id->description); +} + /** * @brief Send a open_drone_id_self_id message * @param chan MAVLink channel to send the message @@ -179,8 +236,8 @@ static inline void mavlink_msg_open_drone_id_self_id_send(mavlink_channel_t chan packet.target_system = target_system; packet.target_component = target_component; packet.description_type = description_type; - mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); - mav_array_memcpy(packet.description, description, sizeof(char)*23); + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); + mav_array_assign_char(packet.description, description, 23); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); #endif } @@ -201,7 +258,7 @@ static inline void mavlink_msg_open_drone_id_self_id_send_struct(mavlink_channel #if MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -222,8 +279,8 @@ static inline void mavlink_msg_open_drone_id_self_id_send_buf(mavlink_message_t packet->target_system = target_system; packet->target_component = target_component; packet->description_type = description_type; - mav_array_memcpy(packet->id_or_mac, id_or_mac, sizeof(uint8_t)*20); - mav_array_memcpy(packet->description, description, sizeof(char)*23); + mav_array_assign_uint8_t(packet->id_or_mac, id_or_mac, 20); + mav_array_assign_char(packet->description, description, 23); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_system.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_system.h index 2af999649f7..f7ce4874177 100644 --- a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_system.h +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_system.h @@ -7,10 +7,12 @@ typedef struct __mavlink_open_drone_id_system_t { int32_t operator_latitude; /*< [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon).*/ int32_t operator_longitude; /*< [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon).*/ - float area_ceiling; /*< [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m.*/ - float area_floor; /*< [m] Area Operations Floor relative to WGS84. If unknown: -1000 m.*/ - uint16_t area_count; /*< Number of aircraft in the area, group or formation (default 1).*/ - uint16_t area_radius; /*< [m] Radius of the cylindrical area of the group or formation (default 0).*/ + float area_ceiling; /*< [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. Used only for swarms/multiple UA.*/ + float area_floor; /*< [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. Used only for swarms/multiple UA.*/ + float operator_altitude_geo; /*< [m] Geodetic altitude of the operator relative to WGS84. If unknown: -1000 m.*/ + uint32_t timestamp; /*< [s] 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019.*/ + uint16_t area_count; /*< Number of aircraft in the area, group or formation (default 1). Used only for swarms/multiple UA.*/ + uint16_t area_radius; /*< [m] Radius of the cylindrical area of the group or formation (default 0). Used only for swarms/multiple UA.*/ uint8_t target_system; /*< System ID (0 for broadcast).*/ uint8_t target_component; /*< Component ID (0 for broadcast).*/ uint8_t id_or_mac[20]; /*< Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */ @@ -20,13 +22,13 @@ typedef struct __mavlink_open_drone_id_system_t { uint8_t class_eu; /*< When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the class of the UA.*/ } mavlink_open_drone_id_system_t; -#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN 46 -#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN 46 -#define MAVLINK_MSG_ID_12904_LEN 46 -#define MAVLINK_MSG_ID_12904_MIN_LEN 46 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN 54 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN 54 +#define MAVLINK_MSG_ID_12904_LEN 54 +#define MAVLINK_MSG_ID_12904_MIN_LEN 54 -#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC 203 -#define MAVLINK_MSG_ID_12904_CRC 203 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC 77 +#define MAVLINK_MSG_ID_12904_CRC 77 #define MAVLINK_MSG_OPEN_DRONE_ID_SYSTEM_FIELD_ID_OR_MAC_LEN 20 @@ -34,39 +36,43 @@ typedef struct __mavlink_open_drone_id_system_t { #define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SYSTEM { \ 12904, \ "OPEN_DRONE_ID_SYSTEM", \ - 13, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_open_drone_id_system_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_open_drone_id_system_t, target_component) }, \ - { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 22, offsetof(mavlink_open_drone_id_system_t, id_or_mac) }, \ - { "operator_location_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_open_drone_id_system_t, operator_location_type) }, \ - { "classification_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_open_drone_id_system_t, classification_type) }, \ + 15, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_open_drone_id_system_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_open_drone_id_system_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 30, offsetof(mavlink_open_drone_id_system_t, id_or_mac) }, \ + { "operator_location_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 50, offsetof(mavlink_open_drone_id_system_t, operator_location_type) }, \ + { "classification_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 51, offsetof(mavlink_open_drone_id_system_t, classification_type) }, \ { "operator_latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_open_drone_id_system_t, operator_latitude) }, \ { "operator_longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_open_drone_id_system_t, operator_longitude) }, \ - { "area_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_open_drone_id_system_t, area_count) }, \ - { "area_radius", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_open_drone_id_system_t, area_radius) }, \ + { "area_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_open_drone_id_system_t, area_count) }, \ + { "area_radius", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_open_drone_id_system_t, area_radius) }, \ { "area_ceiling", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_open_drone_id_system_t, area_ceiling) }, \ { "area_floor", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_open_drone_id_system_t, area_floor) }, \ - { "category_eu", NULL, MAVLINK_TYPE_UINT8_T, 0, 44, offsetof(mavlink_open_drone_id_system_t, category_eu) }, \ - { "class_eu", NULL, MAVLINK_TYPE_UINT8_T, 0, 45, offsetof(mavlink_open_drone_id_system_t, class_eu) }, \ + { "category_eu", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_open_drone_id_system_t, category_eu) }, \ + { "class_eu", NULL, MAVLINK_TYPE_UINT8_T, 0, 53, offsetof(mavlink_open_drone_id_system_t, class_eu) }, \ + { "operator_altitude_geo", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_open_drone_id_system_t, operator_altitude_geo) }, \ + { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_open_drone_id_system_t, timestamp) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SYSTEM { \ "OPEN_DRONE_ID_SYSTEM", \ - 13, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_open_drone_id_system_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_open_drone_id_system_t, target_component) }, \ - { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 22, offsetof(mavlink_open_drone_id_system_t, id_or_mac) }, \ - { "operator_location_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_open_drone_id_system_t, operator_location_type) }, \ - { "classification_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_open_drone_id_system_t, classification_type) }, \ + 15, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_open_drone_id_system_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_open_drone_id_system_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 30, offsetof(mavlink_open_drone_id_system_t, id_or_mac) }, \ + { "operator_location_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 50, offsetof(mavlink_open_drone_id_system_t, operator_location_type) }, \ + { "classification_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 51, offsetof(mavlink_open_drone_id_system_t, classification_type) }, \ { "operator_latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_open_drone_id_system_t, operator_latitude) }, \ { "operator_longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_open_drone_id_system_t, operator_longitude) }, \ - { "area_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_open_drone_id_system_t, area_count) }, \ - { "area_radius", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_open_drone_id_system_t, area_radius) }, \ + { "area_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_open_drone_id_system_t, area_count) }, \ + { "area_radius", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_open_drone_id_system_t, area_radius) }, \ { "area_ceiling", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_open_drone_id_system_t, area_ceiling) }, \ { "area_floor", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_open_drone_id_system_t, area_floor) }, \ - { "category_eu", NULL, MAVLINK_TYPE_UINT8_T, 0, 44, offsetof(mavlink_open_drone_id_system_t, category_eu) }, \ - { "class_eu", NULL, MAVLINK_TYPE_UINT8_T, 0, 45, offsetof(mavlink_open_drone_id_system_t, class_eu) }, \ + { "category_eu", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_open_drone_id_system_t, category_eu) }, \ + { "class_eu", NULL, MAVLINK_TYPE_UINT8_T, 0, 53, offsetof(mavlink_open_drone_id_system_t, class_eu) }, \ + { "operator_altitude_geo", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_open_drone_id_system_t, operator_altitude_geo) }, \ + { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_open_drone_id_system_t, timestamp) }, \ } \ } #endif @@ -84,16 +90,18 @@ typedef struct __mavlink_open_drone_id_system_t { * @param classification_type Specifies the classification type of the UA. * @param operator_latitude [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon). * @param operator_longitude [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon). - * @param area_count Number of aircraft in the area, group or formation (default 1). - * @param area_radius [m] Radius of the cylindrical area of the group or formation (default 0). - * @param area_ceiling [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. - * @param area_floor [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. + * @param area_count Number of aircraft in the area, group or formation (default 1). Used only for swarms/multiple UA. + * @param area_radius [m] Radius of the cylindrical area of the group or formation (default 0). Used only for swarms/multiple UA. + * @param area_ceiling [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. Used only for swarms/multiple UA. + * @param area_floor [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. Used only for swarms/multiple UA. * @param category_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the category of the UA. * @param class_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the class of the UA. + * @param operator_altitude_geo [m] Geodetic altitude of the operator relative to WGS84. If unknown: -1000 m. + * @param timestamp [s] 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_open_drone_id_system_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_location_type, uint8_t classification_type, int32_t operator_latitude, int32_t operator_longitude, uint16_t area_count, uint16_t area_radius, float area_ceiling, float area_floor, uint8_t category_eu, uint8_t class_eu) + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_location_type, uint8_t classification_type, int32_t operator_latitude, int32_t operator_longitude, uint16_t area_count, uint16_t area_radius, float area_ceiling, float area_floor, uint8_t category_eu, uint8_t class_eu, float operator_altitude_geo, uint32_t timestamp) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN]; @@ -101,15 +109,17 @@ static inline uint16_t mavlink_msg_open_drone_id_system_pack(uint8_t system_id, _mav_put_int32_t(buf, 4, operator_longitude); _mav_put_float(buf, 8, area_ceiling); _mav_put_float(buf, 12, area_floor); - _mav_put_uint16_t(buf, 16, area_count); - _mav_put_uint16_t(buf, 18, area_radius); - _mav_put_uint8_t(buf, 20, target_system); - _mav_put_uint8_t(buf, 21, target_component); - _mav_put_uint8_t(buf, 42, operator_location_type); - _mav_put_uint8_t(buf, 43, classification_type); - _mav_put_uint8_t(buf, 44, category_eu); - _mav_put_uint8_t(buf, 45, class_eu); - _mav_put_uint8_t_array(buf, 22, id_or_mac, 20); + _mav_put_float(buf, 16, operator_altitude_geo); + _mav_put_uint32_t(buf, 20, timestamp); + _mav_put_uint16_t(buf, 24, area_count); + _mav_put_uint16_t(buf, 26, area_radius); + _mav_put_uint8_t(buf, 28, target_system); + _mav_put_uint8_t(buf, 29, target_component); + _mav_put_uint8_t(buf, 50, operator_location_type); + _mav_put_uint8_t(buf, 51, classification_type); + _mav_put_uint8_t(buf, 52, category_eu); + _mav_put_uint8_t(buf, 53, class_eu); + _mav_put_uint8_t_array(buf, 30, id_or_mac, 20); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN); #else mavlink_open_drone_id_system_t packet; @@ -117,6 +127,8 @@ static inline uint16_t mavlink_msg_open_drone_id_system_pack(uint8_t system_id, packet.operator_longitude = operator_longitude; packet.area_ceiling = area_ceiling; packet.area_floor = area_floor; + packet.operator_altitude_geo = operator_altitude_geo; + packet.timestamp = timestamp; packet.area_count = area_count; packet.area_radius = area_radius; packet.target_system = target_system; @@ -125,7 +137,7 @@ static inline uint16_t mavlink_msg_open_drone_id_system_pack(uint8_t system_id, packet.classification_type = classification_type; packet.category_eu = category_eu; packet.class_eu = class_eu; - mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN); #endif @@ -133,6 +145,79 @@ static inline uint16_t mavlink_msg_open_drone_id_system_pack(uint8_t system_id, return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); } +/** + * @brief Pack a open_drone_id_system message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param operator_location_type Specifies the operator location type. + * @param classification_type Specifies the classification type of the UA. + * @param operator_latitude [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon). + * @param operator_longitude [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon). + * @param area_count Number of aircraft in the area, group or formation (default 1). Used only for swarms/multiple UA. + * @param area_radius [m] Radius of the cylindrical area of the group or formation (default 0). Used only for swarms/multiple UA. + * @param area_ceiling [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. Used only for swarms/multiple UA. + * @param area_floor [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. Used only for swarms/multiple UA. + * @param category_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the category of the UA. + * @param class_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the class of the UA. + * @param operator_altitude_geo [m] Geodetic altitude of the operator relative to WGS84. If unknown: -1000 m. + * @param timestamp [s] 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_system_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_location_type, uint8_t classification_type, int32_t operator_latitude, int32_t operator_longitude, uint16_t area_count, uint16_t area_radius, float area_ceiling, float area_floor, uint8_t category_eu, uint8_t class_eu, float operator_altitude_geo, uint32_t timestamp) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN]; + _mav_put_int32_t(buf, 0, operator_latitude); + _mav_put_int32_t(buf, 4, operator_longitude); + _mav_put_float(buf, 8, area_ceiling); + _mav_put_float(buf, 12, area_floor); + _mav_put_float(buf, 16, operator_altitude_geo); + _mav_put_uint32_t(buf, 20, timestamp); + _mav_put_uint16_t(buf, 24, area_count); + _mav_put_uint16_t(buf, 26, area_radius); + _mav_put_uint8_t(buf, 28, target_system); + _mav_put_uint8_t(buf, 29, target_component); + _mav_put_uint8_t(buf, 50, operator_location_type); + _mav_put_uint8_t(buf, 51, classification_type); + _mav_put_uint8_t(buf, 52, category_eu); + _mav_put_uint8_t(buf, 53, class_eu); + _mav_put_uint8_t_array(buf, 30, id_or_mac, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN); +#else + mavlink_open_drone_id_system_t packet; + packet.operator_latitude = operator_latitude; + packet.operator_longitude = operator_longitude; + packet.area_ceiling = area_ceiling; + packet.area_floor = area_floor; + packet.operator_altitude_geo = operator_altitude_geo; + packet.timestamp = timestamp; + packet.area_count = area_count; + packet.area_radius = area_radius; + packet.target_system = target_system; + packet.target_component = target_component; + packet.operator_location_type = operator_location_type; + packet.classification_type = classification_type; + packet.category_eu = category_eu; + packet.class_eu = class_eu; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN); +#endif +} + /** * @brief Pack a open_drone_id_system message on a channel * @param system_id ID of this system @@ -146,17 +231,19 @@ static inline uint16_t mavlink_msg_open_drone_id_system_pack(uint8_t system_id, * @param classification_type Specifies the classification type of the UA. * @param operator_latitude [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon). * @param operator_longitude [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon). - * @param area_count Number of aircraft in the area, group or formation (default 1). - * @param area_radius [m] Radius of the cylindrical area of the group or formation (default 0). - * @param area_ceiling [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. - * @param area_floor [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. + * @param area_count Number of aircraft in the area, group or formation (default 1). Used only for swarms/multiple UA. + * @param area_radius [m] Radius of the cylindrical area of the group or formation (default 0). Used only for swarms/multiple UA. + * @param area_ceiling [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. Used only for swarms/multiple UA. + * @param area_floor [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. Used only for swarms/multiple UA. * @param category_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the category of the UA. * @param class_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the class of the UA. + * @param operator_altitude_geo [m] Geodetic altitude of the operator relative to WGS84. If unknown: -1000 m. + * @param timestamp [s] 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_open_drone_id_system_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,const uint8_t *id_or_mac,uint8_t operator_location_type,uint8_t classification_type,int32_t operator_latitude,int32_t operator_longitude,uint16_t area_count,uint16_t area_radius,float area_ceiling,float area_floor,uint8_t category_eu,uint8_t class_eu) + uint8_t target_system,uint8_t target_component,const uint8_t *id_or_mac,uint8_t operator_location_type,uint8_t classification_type,int32_t operator_latitude,int32_t operator_longitude,uint16_t area_count,uint16_t area_radius,float area_ceiling,float area_floor,uint8_t category_eu,uint8_t class_eu,float operator_altitude_geo,uint32_t timestamp) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN]; @@ -164,15 +251,17 @@ static inline uint16_t mavlink_msg_open_drone_id_system_pack_chan(uint8_t system _mav_put_int32_t(buf, 4, operator_longitude); _mav_put_float(buf, 8, area_ceiling); _mav_put_float(buf, 12, area_floor); - _mav_put_uint16_t(buf, 16, area_count); - _mav_put_uint16_t(buf, 18, area_radius); - _mav_put_uint8_t(buf, 20, target_system); - _mav_put_uint8_t(buf, 21, target_component); - _mav_put_uint8_t(buf, 42, operator_location_type); - _mav_put_uint8_t(buf, 43, classification_type); - _mav_put_uint8_t(buf, 44, category_eu); - _mav_put_uint8_t(buf, 45, class_eu); - _mav_put_uint8_t_array(buf, 22, id_or_mac, 20); + _mav_put_float(buf, 16, operator_altitude_geo); + _mav_put_uint32_t(buf, 20, timestamp); + _mav_put_uint16_t(buf, 24, area_count); + _mav_put_uint16_t(buf, 26, area_radius); + _mav_put_uint8_t(buf, 28, target_system); + _mav_put_uint8_t(buf, 29, target_component); + _mav_put_uint8_t(buf, 50, operator_location_type); + _mav_put_uint8_t(buf, 51, classification_type); + _mav_put_uint8_t(buf, 52, category_eu); + _mav_put_uint8_t(buf, 53, class_eu); + _mav_put_uint8_t_array(buf, 30, id_or_mac, 20); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN); #else mavlink_open_drone_id_system_t packet; @@ -180,6 +269,8 @@ static inline uint16_t mavlink_msg_open_drone_id_system_pack_chan(uint8_t system packet.operator_longitude = operator_longitude; packet.area_ceiling = area_ceiling; packet.area_floor = area_floor; + packet.operator_altitude_geo = operator_altitude_geo; + packet.timestamp = timestamp; packet.area_count = area_count; packet.area_radius = area_radius; packet.target_system = target_system; @@ -188,7 +279,7 @@ static inline uint16_t mavlink_msg_open_drone_id_system_pack_chan(uint8_t system packet.classification_type = classification_type; packet.category_eu = category_eu; packet.class_eu = class_eu; - mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN); #endif @@ -206,7 +297,7 @@ static inline uint16_t mavlink_msg_open_drone_id_system_pack_chan(uint8_t system */ static inline uint16_t mavlink_msg_open_drone_id_system_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_system_t* open_drone_id_system) { - return mavlink_msg_open_drone_id_system_pack(system_id, component_id, msg, open_drone_id_system->target_system, open_drone_id_system->target_component, open_drone_id_system->id_or_mac, open_drone_id_system->operator_location_type, open_drone_id_system->classification_type, open_drone_id_system->operator_latitude, open_drone_id_system->operator_longitude, open_drone_id_system->area_count, open_drone_id_system->area_radius, open_drone_id_system->area_ceiling, open_drone_id_system->area_floor, open_drone_id_system->category_eu, open_drone_id_system->class_eu); + return mavlink_msg_open_drone_id_system_pack(system_id, component_id, msg, open_drone_id_system->target_system, open_drone_id_system->target_component, open_drone_id_system->id_or_mac, open_drone_id_system->operator_location_type, open_drone_id_system->classification_type, open_drone_id_system->operator_latitude, open_drone_id_system->operator_longitude, open_drone_id_system->area_count, open_drone_id_system->area_radius, open_drone_id_system->area_ceiling, open_drone_id_system->area_floor, open_drone_id_system->category_eu, open_drone_id_system->class_eu, open_drone_id_system->operator_altitude_geo, open_drone_id_system->timestamp); } /** @@ -220,7 +311,21 @@ static inline uint16_t mavlink_msg_open_drone_id_system_encode(uint8_t system_id */ static inline uint16_t mavlink_msg_open_drone_id_system_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_system_t* open_drone_id_system) { - return mavlink_msg_open_drone_id_system_pack_chan(system_id, component_id, chan, msg, open_drone_id_system->target_system, open_drone_id_system->target_component, open_drone_id_system->id_or_mac, open_drone_id_system->operator_location_type, open_drone_id_system->classification_type, open_drone_id_system->operator_latitude, open_drone_id_system->operator_longitude, open_drone_id_system->area_count, open_drone_id_system->area_radius, open_drone_id_system->area_ceiling, open_drone_id_system->area_floor, open_drone_id_system->category_eu, open_drone_id_system->class_eu); + return mavlink_msg_open_drone_id_system_pack_chan(system_id, component_id, chan, msg, open_drone_id_system->target_system, open_drone_id_system->target_component, open_drone_id_system->id_or_mac, open_drone_id_system->operator_location_type, open_drone_id_system->classification_type, open_drone_id_system->operator_latitude, open_drone_id_system->operator_longitude, open_drone_id_system->area_count, open_drone_id_system->area_radius, open_drone_id_system->area_ceiling, open_drone_id_system->area_floor, open_drone_id_system->category_eu, open_drone_id_system->class_eu, open_drone_id_system->operator_altitude_geo, open_drone_id_system->timestamp); +} + +/** + * @brief Encode a open_drone_id_system struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_system C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_system_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_open_drone_id_system_t* open_drone_id_system) +{ + return mavlink_msg_open_drone_id_system_pack_status(system_id, component_id, _status, msg, open_drone_id_system->target_system, open_drone_id_system->target_component, open_drone_id_system->id_or_mac, open_drone_id_system->operator_location_type, open_drone_id_system->classification_type, open_drone_id_system->operator_latitude, open_drone_id_system->operator_longitude, open_drone_id_system->area_count, open_drone_id_system->area_radius, open_drone_id_system->area_ceiling, open_drone_id_system->area_floor, open_drone_id_system->category_eu, open_drone_id_system->class_eu, open_drone_id_system->operator_altitude_geo, open_drone_id_system->timestamp); } /** @@ -234,16 +339,18 @@ static inline uint16_t mavlink_msg_open_drone_id_system_encode_chan(uint8_t syst * @param classification_type Specifies the classification type of the UA. * @param operator_latitude [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon). * @param operator_longitude [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon). - * @param area_count Number of aircraft in the area, group or formation (default 1). - * @param area_radius [m] Radius of the cylindrical area of the group or formation (default 0). - * @param area_ceiling [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. - * @param area_floor [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. + * @param area_count Number of aircraft in the area, group or formation (default 1). Used only for swarms/multiple UA. + * @param area_radius [m] Radius of the cylindrical area of the group or formation (default 0). Used only for swarms/multiple UA. + * @param area_ceiling [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. Used only for swarms/multiple UA. + * @param area_floor [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. Used only for swarms/multiple UA. * @param category_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the category of the UA. * @param class_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the class of the UA. + * @param operator_altitude_geo [m] Geodetic altitude of the operator relative to WGS84. If unknown: -1000 m. + * @param timestamp [s] 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_open_drone_id_system_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_location_type, uint8_t classification_type, int32_t operator_latitude, int32_t operator_longitude, uint16_t area_count, uint16_t area_radius, float area_ceiling, float area_floor, uint8_t category_eu, uint8_t class_eu) +static inline void mavlink_msg_open_drone_id_system_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_location_type, uint8_t classification_type, int32_t operator_latitude, int32_t operator_longitude, uint16_t area_count, uint16_t area_radius, float area_ceiling, float area_floor, uint8_t category_eu, uint8_t class_eu, float operator_altitude_geo, uint32_t timestamp) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN]; @@ -251,15 +358,17 @@ static inline void mavlink_msg_open_drone_id_system_send(mavlink_channel_t chan, _mav_put_int32_t(buf, 4, operator_longitude); _mav_put_float(buf, 8, area_ceiling); _mav_put_float(buf, 12, area_floor); - _mav_put_uint16_t(buf, 16, area_count); - _mav_put_uint16_t(buf, 18, area_radius); - _mav_put_uint8_t(buf, 20, target_system); - _mav_put_uint8_t(buf, 21, target_component); - _mav_put_uint8_t(buf, 42, operator_location_type); - _mav_put_uint8_t(buf, 43, classification_type); - _mav_put_uint8_t(buf, 44, category_eu); - _mav_put_uint8_t(buf, 45, class_eu); - _mav_put_uint8_t_array(buf, 22, id_or_mac, 20); + _mav_put_float(buf, 16, operator_altitude_geo); + _mav_put_uint32_t(buf, 20, timestamp); + _mav_put_uint16_t(buf, 24, area_count); + _mav_put_uint16_t(buf, 26, area_radius); + _mav_put_uint8_t(buf, 28, target_system); + _mav_put_uint8_t(buf, 29, target_component); + _mav_put_uint8_t(buf, 50, operator_location_type); + _mav_put_uint8_t(buf, 51, classification_type); + _mav_put_uint8_t(buf, 52, category_eu); + _mav_put_uint8_t(buf, 53, class_eu); + _mav_put_uint8_t_array(buf, 30, id_or_mac, 20); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); #else mavlink_open_drone_id_system_t packet; @@ -267,6 +376,8 @@ static inline void mavlink_msg_open_drone_id_system_send(mavlink_channel_t chan, packet.operator_longitude = operator_longitude; packet.area_ceiling = area_ceiling; packet.area_floor = area_floor; + packet.operator_altitude_geo = operator_altitude_geo; + packet.timestamp = timestamp; packet.area_count = area_count; packet.area_radius = area_radius; packet.target_system = target_system; @@ -275,7 +386,7 @@ static inline void mavlink_msg_open_drone_id_system_send(mavlink_channel_t chan, packet.classification_type = classification_type; packet.category_eu = category_eu; packet.class_eu = class_eu; - mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_assign_uint8_t(packet.id_or_mac, id_or_mac, 20); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); #endif } @@ -288,7 +399,7 @@ static inline void mavlink_msg_open_drone_id_system_send(mavlink_channel_t chan, static inline void mavlink_msg_open_drone_id_system_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_system_t* open_drone_id_system) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_open_drone_id_system_send(chan, open_drone_id_system->target_system, open_drone_id_system->target_component, open_drone_id_system->id_or_mac, open_drone_id_system->operator_location_type, open_drone_id_system->classification_type, open_drone_id_system->operator_latitude, open_drone_id_system->operator_longitude, open_drone_id_system->area_count, open_drone_id_system->area_radius, open_drone_id_system->area_ceiling, open_drone_id_system->area_floor, open_drone_id_system->category_eu, open_drone_id_system->class_eu); + mavlink_msg_open_drone_id_system_send(chan, open_drone_id_system->target_system, open_drone_id_system->target_component, open_drone_id_system->id_or_mac, open_drone_id_system->operator_location_type, open_drone_id_system->classification_type, open_drone_id_system->operator_latitude, open_drone_id_system->operator_longitude, open_drone_id_system->area_count, open_drone_id_system->area_radius, open_drone_id_system->area_ceiling, open_drone_id_system->area_floor, open_drone_id_system->category_eu, open_drone_id_system->class_eu, open_drone_id_system->operator_altitude_geo, open_drone_id_system->timestamp); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM, (const char *)open_drone_id_system, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); #endif @@ -296,13 +407,13 @@ static inline void mavlink_msg_open_drone_id_system_send_struct(mavlink_channel_ #if MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_open_drone_id_system_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_location_type, uint8_t classification_type, int32_t operator_latitude, int32_t operator_longitude, uint16_t area_count, uint16_t area_radius, float area_ceiling, float area_floor, uint8_t category_eu, uint8_t class_eu) +static inline void mavlink_msg_open_drone_id_system_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_location_type, uint8_t classification_type, int32_t operator_latitude, int32_t operator_longitude, uint16_t area_count, uint16_t area_radius, float area_ceiling, float area_floor, uint8_t category_eu, uint8_t class_eu, float operator_altitude_geo, uint32_t timestamp) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -310,15 +421,17 @@ static inline void mavlink_msg_open_drone_id_system_send_buf(mavlink_message_t * _mav_put_int32_t(buf, 4, operator_longitude); _mav_put_float(buf, 8, area_ceiling); _mav_put_float(buf, 12, area_floor); - _mav_put_uint16_t(buf, 16, area_count); - _mav_put_uint16_t(buf, 18, area_radius); - _mav_put_uint8_t(buf, 20, target_system); - _mav_put_uint8_t(buf, 21, target_component); - _mav_put_uint8_t(buf, 42, operator_location_type); - _mav_put_uint8_t(buf, 43, classification_type); - _mav_put_uint8_t(buf, 44, category_eu); - _mav_put_uint8_t(buf, 45, class_eu); - _mav_put_uint8_t_array(buf, 22, id_or_mac, 20); + _mav_put_float(buf, 16, operator_altitude_geo); + _mav_put_uint32_t(buf, 20, timestamp); + _mav_put_uint16_t(buf, 24, area_count); + _mav_put_uint16_t(buf, 26, area_radius); + _mav_put_uint8_t(buf, 28, target_system); + _mav_put_uint8_t(buf, 29, target_component); + _mav_put_uint8_t(buf, 50, operator_location_type); + _mav_put_uint8_t(buf, 51, classification_type); + _mav_put_uint8_t(buf, 52, category_eu); + _mav_put_uint8_t(buf, 53, class_eu); + _mav_put_uint8_t_array(buf, 30, id_or_mac, 20); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); #else mavlink_open_drone_id_system_t *packet = (mavlink_open_drone_id_system_t *)msgbuf; @@ -326,6 +439,8 @@ static inline void mavlink_msg_open_drone_id_system_send_buf(mavlink_message_t * packet->operator_longitude = operator_longitude; packet->area_ceiling = area_ceiling; packet->area_floor = area_floor; + packet->operator_altitude_geo = operator_altitude_geo; + packet->timestamp = timestamp; packet->area_count = area_count; packet->area_radius = area_radius; packet->target_system = target_system; @@ -334,7 +449,7 @@ static inline void mavlink_msg_open_drone_id_system_send_buf(mavlink_message_t * packet->classification_type = classification_type; packet->category_eu = category_eu; packet->class_eu = class_eu; - mav_array_memcpy(packet->id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_assign_uint8_t(packet->id_or_mac, id_or_mac, 20); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); #endif } @@ -352,7 +467,7 @@ static inline void mavlink_msg_open_drone_id_system_send_buf(mavlink_message_t * */ static inline uint8_t mavlink_msg_open_drone_id_system_get_target_system(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 20); + return _MAV_RETURN_uint8_t(msg, 28); } /** @@ -362,7 +477,7 @@ static inline uint8_t mavlink_msg_open_drone_id_system_get_target_system(const m */ static inline uint8_t mavlink_msg_open_drone_id_system_get_target_component(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 21); + return _MAV_RETURN_uint8_t(msg, 29); } /** @@ -372,7 +487,7 @@ static inline uint8_t mavlink_msg_open_drone_id_system_get_target_component(cons */ static inline uint16_t mavlink_msg_open_drone_id_system_get_id_or_mac(const mavlink_message_t* msg, uint8_t *id_or_mac) { - return _MAV_RETURN_uint8_t_array(msg, id_or_mac, 20, 22); + return _MAV_RETURN_uint8_t_array(msg, id_or_mac, 20, 30); } /** @@ -382,7 +497,7 @@ static inline uint16_t mavlink_msg_open_drone_id_system_get_id_or_mac(const mavl */ static inline uint8_t mavlink_msg_open_drone_id_system_get_operator_location_type(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 42); + return _MAV_RETURN_uint8_t(msg, 50); } /** @@ -392,7 +507,7 @@ static inline uint8_t mavlink_msg_open_drone_id_system_get_operator_location_typ */ static inline uint8_t mavlink_msg_open_drone_id_system_get_classification_type(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 43); + return _MAV_RETURN_uint8_t(msg, 51); } /** @@ -418,27 +533,27 @@ static inline int32_t mavlink_msg_open_drone_id_system_get_operator_longitude(co /** * @brief Get field area_count from open_drone_id_system message * - * @return Number of aircraft in the area, group or formation (default 1). + * @return Number of aircraft in the area, group or formation (default 1). Used only for swarms/multiple UA. */ static inline uint16_t mavlink_msg_open_drone_id_system_get_area_count(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 16); + return _MAV_RETURN_uint16_t(msg, 24); } /** * @brief Get field area_radius from open_drone_id_system message * - * @return [m] Radius of the cylindrical area of the group or formation (default 0). + * @return [m] Radius of the cylindrical area of the group or formation (default 0). Used only for swarms/multiple UA. */ static inline uint16_t mavlink_msg_open_drone_id_system_get_area_radius(const mavlink_message_t* msg) { - return _MAV_RETURN_uint16_t(msg, 18); + return _MAV_RETURN_uint16_t(msg, 26); } /** * @brief Get field area_ceiling from open_drone_id_system message * - * @return [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. + * @return [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. Used only for swarms/multiple UA. */ static inline float mavlink_msg_open_drone_id_system_get_area_ceiling(const mavlink_message_t* msg) { @@ -448,7 +563,7 @@ static inline float mavlink_msg_open_drone_id_system_get_area_ceiling(const mavl /** * @brief Get field area_floor from open_drone_id_system message * - * @return [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. + * @return [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. Used only for swarms/multiple UA. */ static inline float mavlink_msg_open_drone_id_system_get_area_floor(const mavlink_message_t* msg) { @@ -462,7 +577,7 @@ static inline float mavlink_msg_open_drone_id_system_get_area_floor(const mavlin */ static inline uint8_t mavlink_msg_open_drone_id_system_get_category_eu(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 44); + return _MAV_RETURN_uint8_t(msg, 52); } /** @@ -472,7 +587,27 @@ static inline uint8_t mavlink_msg_open_drone_id_system_get_category_eu(const mav */ static inline uint8_t mavlink_msg_open_drone_id_system_get_class_eu(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 45); + return _MAV_RETURN_uint8_t(msg, 53); +} + +/** + * @brief Get field operator_altitude_geo from open_drone_id_system message + * + * @return [m] Geodetic altitude of the operator relative to WGS84. If unknown: -1000 m. + */ +static inline float mavlink_msg_open_drone_id_system_get_operator_altitude_geo(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field timestamp from open_drone_id_system message + * + * @return [s] 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + */ +static inline uint32_t mavlink_msg_open_drone_id_system_get_timestamp(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 20); } /** @@ -488,6 +623,8 @@ static inline void mavlink_msg_open_drone_id_system_decode(const mavlink_message open_drone_id_system->operator_longitude = mavlink_msg_open_drone_id_system_get_operator_longitude(msg); open_drone_id_system->area_ceiling = mavlink_msg_open_drone_id_system_get_area_ceiling(msg); open_drone_id_system->area_floor = mavlink_msg_open_drone_id_system_get_area_floor(msg); + open_drone_id_system->operator_altitude_geo = mavlink_msg_open_drone_id_system_get_operator_altitude_geo(msg); + open_drone_id_system->timestamp = mavlink_msg_open_drone_id_system_get_timestamp(msg); open_drone_id_system->area_count = mavlink_msg_open_drone_id_system_get_area_count(msg); open_drone_id_system->area_radius = mavlink_msg_open_drone_id_system_get_area_radius(msg); open_drone_id_system->target_system = mavlink_msg_open_drone_id_system_get_target_system(msg); diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_system_update.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_system_update.h new file mode 100644 index 00000000000..438f3698305 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_system_update.h @@ -0,0 +1,400 @@ +#pragma once +// MESSAGE OPEN_DRONE_ID_SYSTEM_UPDATE PACKING + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE 12919 + + +typedef struct __mavlink_open_drone_id_system_update_t { + int32_t operator_latitude; /*< [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon).*/ + int32_t operator_longitude; /*< [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon).*/ + float operator_altitude_geo; /*< [m] Geodetic altitude of the operator relative to WGS84. If unknown: -1000 m.*/ + uint32_t timestamp; /*< [s] 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019.*/ + uint8_t target_system; /*< System ID (0 for broadcast).*/ + uint8_t target_component; /*< Component ID (0 for broadcast).*/ +} mavlink_open_drone_id_system_update_t; + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN 18 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_MIN_LEN 18 +#define MAVLINK_MSG_ID_12919_LEN 18 +#define MAVLINK_MSG_ID_12919_MIN_LEN 18 + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_CRC 7 +#define MAVLINK_MSG_ID_12919_CRC 7 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SYSTEM_UPDATE { \ + 12919, \ + "OPEN_DRONE_ID_SYSTEM_UPDATE", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_open_drone_id_system_update_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_open_drone_id_system_update_t, target_component) }, \ + { "operator_latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_open_drone_id_system_update_t, operator_latitude) }, \ + { "operator_longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_open_drone_id_system_update_t, operator_longitude) }, \ + { "operator_altitude_geo", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_open_drone_id_system_update_t, operator_altitude_geo) }, \ + { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_open_drone_id_system_update_t, timestamp) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SYSTEM_UPDATE { \ + "OPEN_DRONE_ID_SYSTEM_UPDATE", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_open_drone_id_system_update_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_open_drone_id_system_update_t, target_component) }, \ + { "operator_latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_open_drone_id_system_update_t, operator_latitude) }, \ + { "operator_longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_open_drone_id_system_update_t, operator_longitude) }, \ + { "operator_altitude_geo", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_open_drone_id_system_update_t, operator_altitude_geo) }, \ + { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_open_drone_id_system_update_t, timestamp) }, \ + } \ +} +#endif + +/** + * @brief Pack a open_drone_id_system_update message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param operator_latitude [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon). + * @param operator_longitude [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon). + * @param operator_altitude_geo [m] Geodetic altitude of the operator relative to WGS84. If unknown: -1000 m. + * @param timestamp [s] 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_system_update_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, int32_t operator_latitude, int32_t operator_longitude, float operator_altitude_geo, uint32_t timestamp) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN]; + _mav_put_int32_t(buf, 0, operator_latitude); + _mav_put_int32_t(buf, 4, operator_longitude); + _mav_put_float(buf, 8, operator_altitude_geo); + _mav_put_uint32_t(buf, 12, timestamp); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN); +#else + mavlink_open_drone_id_system_update_t packet; + packet.operator_latitude = operator_latitude; + packet.operator_longitude = operator_longitude; + packet.operator_altitude_geo = operator_altitude_geo; + packet.timestamp = timestamp; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_CRC); +} + +/** + * @brief Pack a open_drone_id_system_update message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param operator_latitude [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon). + * @param operator_longitude [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon). + * @param operator_altitude_geo [m] Geodetic altitude of the operator relative to WGS84. If unknown: -1000 m. + * @param timestamp [s] 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_system_update_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, int32_t operator_latitude, int32_t operator_longitude, float operator_altitude_geo, uint32_t timestamp) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN]; + _mav_put_int32_t(buf, 0, operator_latitude); + _mav_put_int32_t(buf, 4, operator_longitude); + _mav_put_float(buf, 8, operator_altitude_geo); + _mav_put_uint32_t(buf, 12, timestamp); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN); +#else + mavlink_open_drone_id_system_update_t packet; + packet.operator_latitude = operator_latitude; + packet.operator_longitude = operator_longitude; + packet.operator_altitude_geo = operator_altitude_geo; + packet.timestamp = timestamp; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN); +#endif +} + +/** + * @brief Pack a open_drone_id_system_update message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param operator_latitude [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon). + * @param operator_longitude [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon). + * @param operator_altitude_geo [m] Geodetic altitude of the operator relative to WGS84. If unknown: -1000 m. + * @param timestamp [s] 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_system_update_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,int32_t operator_latitude,int32_t operator_longitude,float operator_altitude_geo,uint32_t timestamp) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN]; + _mav_put_int32_t(buf, 0, operator_latitude); + _mav_put_int32_t(buf, 4, operator_longitude); + _mav_put_float(buf, 8, operator_altitude_geo); + _mav_put_uint32_t(buf, 12, timestamp); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN); +#else + mavlink_open_drone_id_system_update_t packet; + packet.operator_latitude = operator_latitude; + packet.operator_longitude = operator_longitude; + packet.operator_altitude_geo = operator_altitude_geo; + packet.timestamp = timestamp; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_CRC); +} + +/** + * @brief Encode a open_drone_id_system_update struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_system_update C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_system_update_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_system_update_t* open_drone_id_system_update) +{ + return mavlink_msg_open_drone_id_system_update_pack(system_id, component_id, msg, open_drone_id_system_update->target_system, open_drone_id_system_update->target_component, open_drone_id_system_update->operator_latitude, open_drone_id_system_update->operator_longitude, open_drone_id_system_update->operator_altitude_geo, open_drone_id_system_update->timestamp); +} + +/** + * @brief Encode a open_drone_id_system_update struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_system_update C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_system_update_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_system_update_t* open_drone_id_system_update) +{ + return mavlink_msg_open_drone_id_system_update_pack_chan(system_id, component_id, chan, msg, open_drone_id_system_update->target_system, open_drone_id_system_update->target_component, open_drone_id_system_update->operator_latitude, open_drone_id_system_update->operator_longitude, open_drone_id_system_update->operator_altitude_geo, open_drone_id_system_update->timestamp); +} + +/** + * @brief Encode a open_drone_id_system_update struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_system_update C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_system_update_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_open_drone_id_system_update_t* open_drone_id_system_update) +{ + return mavlink_msg_open_drone_id_system_update_pack_status(system_id, component_id, _status, msg, open_drone_id_system_update->target_system, open_drone_id_system_update->target_component, open_drone_id_system_update->operator_latitude, open_drone_id_system_update->operator_longitude, open_drone_id_system_update->operator_altitude_geo, open_drone_id_system_update->timestamp); +} + +/** + * @brief Send a open_drone_id_system_update message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param operator_latitude [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon). + * @param operator_longitude [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon). + * @param operator_altitude_geo [m] Geodetic altitude of the operator relative to WGS84. If unknown: -1000 m. + * @param timestamp [s] 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_open_drone_id_system_update_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t operator_latitude, int32_t operator_longitude, float operator_altitude_geo, uint32_t timestamp) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN]; + _mav_put_int32_t(buf, 0, operator_latitude); + _mav_put_int32_t(buf, 4, operator_longitude); + _mav_put_float(buf, 8, operator_altitude_geo); + _mav_put_uint32_t(buf, 12, timestamp); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_CRC); +#else + mavlink_open_drone_id_system_update_t packet; + packet.operator_latitude = operator_latitude; + packet.operator_longitude = operator_longitude; + packet.operator_altitude_geo = operator_altitude_geo; + packet.timestamp = timestamp; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_CRC); +#endif +} + +/** + * @brief Send a open_drone_id_system_update message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_open_drone_id_system_update_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_system_update_t* open_drone_id_system_update) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_open_drone_id_system_update_send(chan, open_drone_id_system_update->target_system, open_drone_id_system_update->target_component, open_drone_id_system_update->operator_latitude, open_drone_id_system_update->operator_longitude, open_drone_id_system_update->operator_altitude_geo, open_drone_id_system_update->timestamp); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE, (const char *)open_drone_id_system_update, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_open_drone_id_system_update_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t operator_latitude, int32_t operator_longitude, float operator_altitude_geo, uint32_t timestamp) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, operator_latitude); + _mav_put_int32_t(buf, 4, operator_longitude); + _mav_put_float(buf, 8, operator_altitude_geo); + _mav_put_uint32_t(buf, 12, timestamp); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_CRC); +#else + mavlink_open_drone_id_system_update_t *packet = (mavlink_open_drone_id_system_update_t *)msgbuf; + packet->operator_latitude = operator_latitude; + packet->operator_longitude = operator_longitude; + packet->operator_altitude_geo = operator_altitude_geo; + packet->timestamp = timestamp; + packet->target_system = target_system; + packet->target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE OPEN_DRONE_ID_SYSTEM_UPDATE UNPACKING + + +/** + * @brief Get field target_system from open_drone_id_system_update message + * + * @return System ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_system_update_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 16); +} + +/** + * @brief Get field target_component from open_drone_id_system_update message + * + * @return Component ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_system_update_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 17); +} + +/** + * @brief Get field operator_latitude from open_drone_id_system_update message + * + * @return [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon). + */ +static inline int32_t mavlink_msg_open_drone_id_system_update_get_operator_latitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field operator_longitude from open_drone_id_system_update message + * + * @return [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon). + */ +static inline int32_t mavlink_msg_open_drone_id_system_update_get_operator_longitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field operator_altitude_geo from open_drone_id_system_update message + * + * @return [m] Geodetic altitude of the operator relative to WGS84. If unknown: -1000 m. + */ +static inline float mavlink_msg_open_drone_id_system_update_get_operator_altitude_geo(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field timestamp from open_drone_id_system_update message + * + * @return [s] 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + */ +static inline uint32_t mavlink_msg_open_drone_id_system_update_get_timestamp(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 12); +} + +/** + * @brief Decode a open_drone_id_system_update message into a struct + * + * @param msg The message to decode + * @param open_drone_id_system_update C-struct to decode the message contents into + */ +static inline void mavlink_msg_open_drone_id_system_update_decode(const mavlink_message_t* msg, mavlink_open_drone_id_system_update_t* open_drone_id_system_update) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + open_drone_id_system_update->operator_latitude = mavlink_msg_open_drone_id_system_update_get_operator_latitude(msg); + open_drone_id_system_update->operator_longitude = mavlink_msg_open_drone_id_system_update_get_operator_longitude(msg); + open_drone_id_system_update->operator_altitude_geo = mavlink_msg_open_drone_id_system_update_get_operator_altitude_geo(msg); + open_drone_id_system_update->timestamp = mavlink_msg_open_drone_id_system_update_get_timestamp(msg); + open_drone_id_system_update->target_system = mavlink_msg_open_drone_id_system_update_get_target_system(msg); + open_drone_id_system_update->target_component = mavlink_msg_open_drone_id_system_update_get_target_component(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN? msg->len : MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN; + memset(open_drone_id_system_update, 0, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_LEN); + memcpy(open_drone_id_system_update, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_optical_flow.h b/lib/main/MAVLink/common/mavlink_msg_optical_flow.h index 25c88864213..689cd8d4e40 100755 --- a/lib/main/MAVLink/common/mavlink_msg_optical_flow.h +++ b/lib/main/MAVLink/common/mavlink_msg_optical_flow.h @@ -117,6 +117,66 @@ static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPTICAL_FLOW_MIN_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); } +/** + * @brief Pack a optical_flow message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param sensor_id Sensor ID + * @param flow_x [dpix] Flow in x-sensor direction + * @param flow_y [dpix] Flow in y-sensor direction + * @param flow_comp_m_x [m/s] Flow in x-sensor direction, angular-speed compensated + * @param flow_comp_m_y [m/s] Flow in y-sensor direction, angular-speed compensated + * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality + * @param ground_distance [m] Ground distance. Positive value: distance known. Negative value: Unknown distance + * @param flow_rate_x [rad/s] Flow rate about X axis + * @param flow_rate_y [rad/s] Flow rate about Y axis + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_optical_flow_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance, float flow_rate_x, float flow_rate_y) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, flow_comp_m_x); + _mav_put_float(buf, 12, flow_comp_m_y); + _mav_put_float(buf, 16, ground_distance); + _mav_put_int16_t(buf, 20, flow_x); + _mav_put_int16_t(buf, 22, flow_y); + _mav_put_uint8_t(buf, 24, sensor_id); + _mav_put_uint8_t(buf, 25, quality); + _mav_put_float(buf, 26, flow_rate_x); + _mav_put_float(buf, 30, flow_rate_y); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#else + mavlink_optical_flow_t packet; + packet.time_usec = time_usec; + packet.flow_comp_m_x = flow_comp_m_x; + packet.flow_comp_m_y = flow_comp_m_y; + packet.ground_distance = ground_distance; + packet.flow_x = flow_x; + packet.flow_y = flow_y; + packet.sensor_id = sensor_id; + packet.quality = quality; + packet.flow_rate_x = flow_rate_x; + packet.flow_rate_y = flow_rate_y; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPTICAL_FLOW_MIN_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPTICAL_FLOW_MIN_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#endif +} + /** * @brief Pack a optical_flow message on a channel * @param system_id ID of this system @@ -200,6 +260,20 @@ static inline uint16_t mavlink_msg_optical_flow_encode_chan(uint8_t system_id, u return mavlink_msg_optical_flow_pack_chan(system_id, component_id, chan, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance, optical_flow->flow_rate_x, optical_flow->flow_rate_y); } +/** + * @brief Encode a optical_flow struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param optical_flow C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_optical_flow_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_optical_flow_t* optical_flow) +{ + return mavlink_msg_optical_flow_pack_status(system_id, component_id, _status, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance, optical_flow->flow_rate_x, optical_flow->flow_rate_y); +} + /** * @brief Send a optical_flow message * @param chan MAVLink channel to send the message @@ -266,7 +340,7 @@ static inline void mavlink_msg_optical_flow_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_OPTICAL_FLOW_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_optical_flow_rad.h b/lib/main/MAVLink/common/mavlink_msg_optical_flow_rad.h index 593533bf66e..0ab098abd8a 100755 --- a/lib/main/MAVLink/common/mavlink_msg_optical_flow_rad.h +++ b/lib/main/MAVLink/common/mavlink_msg_optical_flow_rad.h @@ -129,6 +129,72 @@ static inline uint16_t mavlink_msg_optical_flow_rad_pack(uint8_t system_id, uint return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_MIN_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC); } +/** + * @brief Pack a optical_flow_rad message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param sensor_id Sensor ID + * @param integration_time_us [us] Integration time. Divide integrated_x and integrated_y by the integration time to obtain average flow. The integration time also indicates the. + * @param integrated_x [rad] Flow around X axis (Sensor RH rotation about the X axis induces a positive flow. Sensor linear motion along the positive Y axis induces a negative flow.) + * @param integrated_y [rad] Flow around Y axis (Sensor RH rotation about the Y axis induces a positive flow. Sensor linear motion along the positive X axis induces a positive flow.) + * @param integrated_xgyro [rad] RH rotation around X axis + * @param integrated_ygyro [rad] RH rotation around Y axis + * @param integrated_zgyro [rad] RH rotation around Z axis + * @param temperature [cdegC] Temperature + * @param quality Optical flow quality / confidence. 0: no valid flow, 255: maximum quality + * @param time_delta_distance_us [us] Time since the distance was sampled. + * @param distance [m] Distance to the center of the flow field. Positive value (including zero): distance known. Negative value: Unknown distance. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_optical_flow_rad_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint8_t sensor_id, uint32_t integration_time_us, float integrated_x, float integrated_y, float integrated_xgyro, float integrated_ygyro, float integrated_zgyro, int16_t temperature, uint8_t quality, uint32_t time_delta_distance_us, float distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, integration_time_us); + _mav_put_float(buf, 12, integrated_x); + _mav_put_float(buf, 16, integrated_y); + _mav_put_float(buf, 20, integrated_xgyro); + _mav_put_float(buf, 24, integrated_ygyro); + _mav_put_float(buf, 28, integrated_zgyro); + _mav_put_uint32_t(buf, 32, time_delta_distance_us); + _mav_put_float(buf, 36, distance); + _mav_put_int16_t(buf, 40, temperature); + _mav_put_uint8_t(buf, 42, sensor_id); + _mav_put_uint8_t(buf, 43, quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#else + mavlink_optical_flow_rad_t packet; + packet.time_usec = time_usec; + packet.integration_time_us = integration_time_us; + packet.integrated_x = integrated_x; + packet.integrated_y = integrated_y; + packet.integrated_xgyro = integrated_xgyro; + packet.integrated_ygyro = integrated_ygyro; + packet.integrated_zgyro = integrated_zgyro; + packet.time_delta_distance_us = time_delta_distance_us; + packet.distance = distance; + packet.temperature = temperature; + packet.sensor_id = sensor_id; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW_RAD; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_MIN_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_MIN_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN); +#endif +} + /** * @brief Pack a optical_flow_rad message on a channel * @param system_id ID of this system @@ -218,6 +284,20 @@ static inline uint16_t mavlink_msg_optical_flow_rad_encode_chan(uint8_t system_i return mavlink_msg_optical_flow_rad_pack_chan(system_id, component_id, chan, msg, optical_flow_rad->time_usec, optical_flow_rad->sensor_id, optical_flow_rad->integration_time_us, optical_flow_rad->integrated_x, optical_flow_rad->integrated_y, optical_flow_rad->integrated_xgyro, optical_flow_rad->integrated_ygyro, optical_flow_rad->integrated_zgyro, optical_flow_rad->temperature, optical_flow_rad->quality, optical_flow_rad->time_delta_distance_us, optical_flow_rad->distance); } +/** + * @brief Encode a optical_flow_rad struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param optical_flow_rad C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_optical_flow_rad_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_optical_flow_rad_t* optical_flow_rad) +{ + return mavlink_msg_optical_flow_rad_pack_status(system_id, component_id, _status, msg, optical_flow_rad->time_usec, optical_flow_rad->sensor_id, optical_flow_rad->integration_time_us, optical_flow_rad->integrated_x, optical_flow_rad->integrated_y, optical_flow_rad->integrated_xgyro, optical_flow_rad->integrated_ygyro, optical_flow_rad->integrated_zgyro, optical_flow_rad->temperature, optical_flow_rad->quality, optical_flow_rad->time_delta_distance_us, optical_flow_rad->distance); +} + /** * @brief Send a optical_flow_rad message * @param chan MAVLink channel to send the message @@ -290,7 +370,7 @@ static inline void mavlink_msg_optical_flow_rad_send_struct(mavlink_channel_t ch #if MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_orbit_execution_status.h b/lib/main/MAVLink/common/mavlink_msg_orbit_execution_status.h index 29e2915a741..a5df2d12b0a 100644 --- a/lib/main/MAVLink/common/mavlink_msg_orbit_execution_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_orbit_execution_status.h @@ -93,6 +93,54 @@ static inline uint16_t mavlink_msg_orbit_execution_status_pack(uint8_t system_id return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_CRC); } +/** + * @brief Pack a orbit_execution_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param radius [m] Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. + * @param frame The coordinate system of the fields: x, y, z. + * @param x X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + * @param y Y coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + * @param z [m] Altitude of center point. Coordinate system depends on frame field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_orbit_execution_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, float radius, uint8_t frame, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, radius); + _mav_put_int32_t(buf, 12, x); + _mav_put_int32_t(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_uint8_t(buf, 24, frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN); +#else + mavlink_orbit_execution_status_t packet; + packet.time_usec = time_usec; + packet.radius = radius; + packet.x = x; + packet.y = y; + packet.z = z; + packet.frame = frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN); +#endif +} + /** * @brief Pack a orbit_execution_status message on a channel * @param system_id ID of this system @@ -164,6 +212,20 @@ static inline uint16_t mavlink_msg_orbit_execution_status_encode_chan(uint8_t sy return mavlink_msg_orbit_execution_status_pack_chan(system_id, component_id, chan, msg, orbit_execution_status->time_usec, orbit_execution_status->radius, orbit_execution_status->frame, orbit_execution_status->x, orbit_execution_status->y, orbit_execution_status->z); } +/** + * @brief Encode a orbit_execution_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param orbit_execution_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_orbit_execution_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_orbit_execution_status_t* orbit_execution_status) +{ + return mavlink_msg_orbit_execution_status_pack_status(system_id, component_id, _status, msg, orbit_execution_status->time_usec, orbit_execution_status->radius, orbit_execution_status->frame, orbit_execution_status->x, orbit_execution_status->y, orbit_execution_status->z); +} + /** * @brief Send a orbit_execution_status message * @param chan MAVLink channel to send the message @@ -218,7 +280,7 @@ static inline void mavlink_msg_orbit_execution_status_send_struct(mavlink_channe #if MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_param_ack_transaction.h b/lib/main/MAVLink/common/mavlink_msg_param_ack_transaction.h deleted file mode 100644 index 404b084ab93..00000000000 --- a/lib/main/MAVLink/common/mavlink_msg_param_ack_transaction.h +++ /dev/null @@ -1,330 +0,0 @@ -#pragma once -// MESSAGE PARAM_ACK_TRANSACTION PACKING - -#define MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION 19 - - -typedef struct __mavlink_param_ack_transaction_t { - float param_value; /*< Parameter value (new value if PARAM_ACCEPTED, current value otherwise)*/ - uint8_t target_system; /*< Id of system that sent PARAM_SET message.*/ - uint8_t target_component; /*< Id of system that sent PARAM_SET message.*/ - char param_id[16]; /*< Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string*/ - uint8_t param_type; /*< Parameter type.*/ - uint8_t param_result; /*< Result code.*/ -} mavlink_param_ack_transaction_t; - -#define MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN 24 -#define MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_MIN_LEN 24 -#define MAVLINK_MSG_ID_19_LEN 24 -#define MAVLINK_MSG_ID_19_MIN_LEN 24 - -#define MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_CRC 137 -#define MAVLINK_MSG_ID_19_CRC 137 - -#define MAVLINK_MSG_PARAM_ACK_TRANSACTION_FIELD_PARAM_ID_LEN 16 - -#if MAVLINK_COMMAND_24BIT -#define MAVLINK_MESSAGE_INFO_PARAM_ACK_TRANSACTION { \ - 19, \ - "PARAM_ACK_TRANSACTION", \ - 6, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_param_ack_transaction_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_param_ack_transaction_t, target_component) }, \ - { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 6, offsetof(mavlink_param_ack_transaction_t, param_id) }, \ - { "param_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_param_ack_transaction_t, param_value) }, \ - { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_param_ack_transaction_t, param_type) }, \ - { "param_result", NULL, MAVLINK_TYPE_UINT8_T, 0, 23, offsetof(mavlink_param_ack_transaction_t, param_result) }, \ - } \ -} -#else -#define MAVLINK_MESSAGE_INFO_PARAM_ACK_TRANSACTION { \ - "PARAM_ACK_TRANSACTION", \ - 6, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_param_ack_transaction_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_param_ack_transaction_t, target_component) }, \ - { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 6, offsetof(mavlink_param_ack_transaction_t, param_id) }, \ - { "param_value", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_param_ack_transaction_t, param_value) }, \ - { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_param_ack_transaction_t, param_type) }, \ - { "param_result", NULL, MAVLINK_TYPE_UINT8_T, 0, 23, offsetof(mavlink_param_ack_transaction_t, param_result) }, \ - } \ -} -#endif - -/** - * @brief Pack a param_ack_transaction message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system Id of system that sent PARAM_SET message. - * @param target_component Id of system that sent PARAM_SET message. - * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_value Parameter value (new value if PARAM_ACCEPTED, current value otherwise) - * @param param_type Parameter type. - * @param param_result Result code. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_param_ack_transaction_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type, uint8_t param_result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN]; - _mav_put_float(buf, 0, param_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 22, param_type); - _mav_put_uint8_t(buf, 23, param_result); - _mav_put_char_array(buf, 6, param_id, 16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN); -#else - mavlink_param_ack_transaction_t packet; - packet.param_value = param_value; - packet.target_system = target_system; - packet.target_component = target_component; - packet.param_type = param_type; - packet.param_result = param_result; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_MIN_LEN, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_CRC); -} - -/** - * @brief Pack a param_ack_transaction message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param target_system Id of system that sent PARAM_SET message. - * @param target_component Id of system that sent PARAM_SET message. - * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_value Parameter value (new value if PARAM_ACCEPTED, current value otherwise) - * @param param_type Parameter type. - * @param param_result Result code. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_param_ack_transaction_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,const char *param_id,float param_value,uint8_t param_type,uint8_t param_result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN]; - _mav_put_float(buf, 0, param_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 22, param_type); - _mav_put_uint8_t(buf, 23, param_result); - _mav_put_char_array(buf, 6, param_id, 16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN); -#else - mavlink_param_ack_transaction_t packet; - packet.param_value = param_value; - packet.target_system = target_system; - packet.target_component = target_component; - packet.param_type = param_type; - packet.param_result = param_result; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN); -#endif - - msg->msgid = MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_MIN_LEN, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_CRC); -} - -/** - * @brief Encode a param_ack_transaction struct - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param param_ack_transaction C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_param_ack_transaction_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_ack_transaction_t* param_ack_transaction) -{ - return mavlink_msg_param_ack_transaction_pack(system_id, component_id, msg, param_ack_transaction->target_system, param_ack_transaction->target_component, param_ack_transaction->param_id, param_ack_transaction->param_value, param_ack_transaction->param_type, param_ack_transaction->param_result); -} - -/** - * @brief Encode a param_ack_transaction struct on a channel - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message will be sent over - * @param msg The MAVLink message to compress the data into - * @param param_ack_transaction C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_param_ack_transaction_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_ack_transaction_t* param_ack_transaction) -{ - return mavlink_msg_param_ack_transaction_pack_chan(system_id, component_id, chan, msg, param_ack_transaction->target_system, param_ack_transaction->target_component, param_ack_transaction->param_id, param_ack_transaction->param_value, param_ack_transaction->param_type, param_ack_transaction->param_result); -} - -/** - * @brief Send a param_ack_transaction message - * @param chan MAVLink channel to send the message - * - * @param target_system Id of system that sent PARAM_SET message. - * @param target_component Id of system that sent PARAM_SET message. - * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - * @param param_value Parameter value (new value if PARAM_ACCEPTED, current value otherwise) - * @param param_type Parameter type. - * @param param_result Result code. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_param_ack_transaction_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type, uint8_t param_result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN]; - _mav_put_float(buf, 0, param_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 22, param_type); - _mav_put_uint8_t(buf, 23, param_result); - _mav_put_char_array(buf, 6, param_id, 16); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION, buf, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_MIN_LEN, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_CRC); -#else - mavlink_param_ack_transaction_t packet; - packet.param_value = param_value; - packet.target_system = target_system; - packet.target_component = target_component; - packet.param_type = param_type; - packet.param_result = param_result; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION, (const char *)&packet, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_MIN_LEN, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_CRC); -#endif -} - -/** - * @brief Send a param_ack_transaction message - * @param chan MAVLink channel to send the message - * @param struct The MAVLink struct to serialize - */ -static inline void mavlink_msg_param_ack_transaction_send_struct(mavlink_channel_t chan, const mavlink_param_ack_transaction_t* param_ack_transaction) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_param_ack_transaction_send(chan, param_ack_transaction->target_system, param_ack_transaction->target_component, param_ack_transaction->param_id, param_ack_transaction->param_value, param_ack_transaction->param_type, param_ack_transaction->param_result); -#else - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION, (const char *)param_ack_transaction, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_MIN_LEN, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_CRC); -#endif -} - -#if MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN <= MAVLINK_MAX_PAYLOAD_LEN -/* - This varient of _send() can be used to save stack space by re-using - memory from the receive buffer. The caller provides a - mavlink_message_t which is the size of a full mavlink message. This - is usually the receive buffer for the channel, and allows a reply to an - incoming message with minimum stack space usage. - */ -static inline void mavlink_msg_param_ack_transaction_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type, uint8_t param_result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char *buf = (char *)msgbuf; - _mav_put_float(buf, 0, param_value); - _mav_put_uint8_t(buf, 4, target_system); - _mav_put_uint8_t(buf, 5, target_component); - _mav_put_uint8_t(buf, 22, param_type); - _mav_put_uint8_t(buf, 23, param_result); - _mav_put_char_array(buf, 6, param_id, 16); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION, buf, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_MIN_LEN, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_CRC); -#else - mavlink_param_ack_transaction_t *packet = (mavlink_param_ack_transaction_t *)msgbuf; - packet->param_value = param_value; - packet->target_system = target_system; - packet->target_component = target_component; - packet->param_type = param_type; - packet->param_result = param_result; - mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION, (const char *)packet, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_MIN_LEN, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_CRC); -#endif -} -#endif - -#endif - -// MESSAGE PARAM_ACK_TRANSACTION UNPACKING - - -/** - * @brief Get field target_system from param_ack_transaction message - * - * @return Id of system that sent PARAM_SET message. - */ -static inline uint8_t mavlink_msg_param_ack_transaction_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field target_component from param_ack_transaction message - * - * @return Id of system that sent PARAM_SET message. - */ -static inline uint8_t mavlink_msg_param_ack_transaction_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @brief Get field param_id from param_ack_transaction message - * - * @return Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string - */ -static inline uint16_t mavlink_msg_param_ack_transaction_get_param_id(const mavlink_message_t* msg, char *param_id) -{ - return _MAV_RETURN_char_array(msg, param_id, 16, 6); -} - -/** - * @brief Get field param_value from param_ack_transaction message - * - * @return Parameter value (new value if PARAM_ACCEPTED, current value otherwise) - */ -static inline float mavlink_msg_param_ack_transaction_get_param_value(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field param_type from param_ack_transaction message - * - * @return Parameter type. - */ -static inline uint8_t mavlink_msg_param_ack_transaction_get_param_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 22); -} - -/** - * @brief Get field param_result from param_ack_transaction message - * - * @return Result code. - */ -static inline uint8_t mavlink_msg_param_ack_transaction_get_param_result(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 23); -} - -/** - * @brief Decode a param_ack_transaction message into a struct - * - * @param msg The message to decode - * @param param_ack_transaction C-struct to decode the message contents into - */ -static inline void mavlink_msg_param_ack_transaction_decode(const mavlink_message_t* msg, mavlink_param_ack_transaction_t* param_ack_transaction) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - param_ack_transaction->param_value = mavlink_msg_param_ack_transaction_get_param_value(msg); - param_ack_transaction->target_system = mavlink_msg_param_ack_transaction_get_target_system(msg); - param_ack_transaction->target_component = mavlink_msg_param_ack_transaction_get_target_component(msg); - mavlink_msg_param_ack_transaction_get_param_id(msg, param_ack_transaction->param_id); - param_ack_transaction->param_type = mavlink_msg_param_ack_transaction_get_param_type(msg); - param_ack_transaction->param_result = mavlink_msg_param_ack_transaction_get_param_result(msg); -#else - uint8_t len = msg->len < MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN? msg->len : MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN; - memset(param_ack_transaction, 0, MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_LEN); - memcpy(param_ack_transaction, _MAV_PAYLOAD(msg), len); -#endif -} diff --git a/lib/main/MAVLink/common/mavlink_msg_param_ext_ack.h b/lib/main/MAVLink/common/mavlink_msg_param_ext_ack.h index 3a560d2b8fe..aebb8c4e368 100644 --- a/lib/main/MAVLink/common/mavlink_msg_param_ext_ack.h +++ b/lib/main/MAVLink/common/mavlink_msg_param_ext_ack.h @@ -60,6 +60,42 @@ typedef struct __mavlink_param_ext_ack_t { static inline uint16_t mavlink_msg_param_ext_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char *param_id, const char *param_value, uint8_t param_type, uint8_t param_result) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN]; + _mav_put_uint8_t(buf, 144, param_type); + _mav_put_uint8_t(buf, 145, param_result); + _mav_put_char_array(buf, 0, param_id, 16); + _mav_put_char_array(buf, 16, param_value, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN); +#else + mavlink_param_ext_ack_t packet; + packet.param_type = param_type; + packet.param_result = param_result; + mav_array_assign_char(packet.param_id, param_id, 16); + mav_array_assign_char(packet.param_value, param_value, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_ACK; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); +} + +/** + * @brief Pack a param_ext_ack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value (new value if PARAM_ACK_ACCEPTED, current value otherwise) + * @param param_type Parameter type. + * @param param_result Result code. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_ack_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + const char *param_id, const char *param_value, uint8_t param_type, uint8_t param_result) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN]; _mav_put_uint8_t(buf, 144, param_type); @@ -77,7 +113,11 @@ static inline uint16_t mavlink_msg_param_ext_ack_pack(uint8_t system_id, uint8_t #endif msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_ACK; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN); +#endif } /** @@ -107,8 +147,8 @@ static inline uint16_t mavlink_msg_param_ext_ack_pack_chan(uint8_t system_id, ui mavlink_param_ext_ack_t packet; packet.param_type = param_type; packet.param_result = param_result; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); - mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + mav_array_assign_char(packet.param_id, param_id, 16); + mav_array_assign_char(packet.param_value, param_value, 128); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN); #endif @@ -143,6 +183,20 @@ static inline uint16_t mavlink_msg_param_ext_ack_encode_chan(uint8_t system_id, return mavlink_msg_param_ext_ack_pack_chan(system_id, component_id, chan, msg, param_ext_ack->param_id, param_ext_ack->param_value, param_ext_ack->param_type, param_ext_ack->param_result); } +/** + * @brief Encode a param_ext_ack struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param param_ext_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_ack_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_param_ext_ack_t* param_ext_ack) +{ + return mavlink_msg_param_ext_ack_pack_status(system_id, component_id, _status, msg, param_ext_ack->param_id, param_ext_ack->param_value, param_ext_ack->param_type, param_ext_ack->param_result); +} + /** * @brief Send a param_ext_ack message * @param chan MAVLink channel to send the message @@ -167,8 +221,8 @@ static inline void mavlink_msg_param_ext_ack_send(mavlink_channel_t chan, const mavlink_param_ext_ack_t packet; packet.param_type = param_type; packet.param_result = param_result; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); - mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + mav_array_assign_char(packet.param_id, param_id, 16); + mav_array_assign_char(packet.param_value, param_value, 128); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_ACK, (const char *)&packet, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); #endif } @@ -189,7 +243,7 @@ static inline void mavlink_msg_param_ext_ack_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -208,8 +262,8 @@ static inline void mavlink_msg_param_ext_ack_send_buf(mavlink_message_t *msgbuf, mavlink_param_ext_ack_t *packet = (mavlink_param_ext_ack_t *)msgbuf; packet->param_type = param_type; packet->param_result = param_result; - mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); - mav_array_memcpy(packet->param_value, param_value, sizeof(char)*128); + mav_array_assign_char(packet->param_id, param_id, 16); + mav_array_assign_char(packet->param_value, param_value, 128); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_ACK, (const char *)packet, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_param_ext_request_list.h b/lib/main/MAVLink/common/mavlink_msg_param_ext_request_list.h index a3983dfb853..d8561e3845f 100644 --- a/lib/main/MAVLink/common/mavlink_msg_param_ext_request_list.h +++ b/lib/main/MAVLink/common/mavlink_msg_param_ext_request_list.h @@ -69,6 +69,42 @@ static inline uint16_t mavlink_msg_param_ext_request_list_pack(uint8_t system_id return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_CRC); } +/** + * @brief Pack a param_ext_request_list message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_request_list_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN); +#else + mavlink_param_ext_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN); +#endif +} + /** * @brief Pack a param_ext_request_list message on a channel * @param system_id ID of this system @@ -128,6 +164,20 @@ static inline uint16_t mavlink_msg_param_ext_request_list_encode_chan(uint8_t sy return mavlink_msg_param_ext_request_list_pack_chan(system_id, component_id, chan, msg, param_ext_request_list->target_system, param_ext_request_list->target_component); } +/** + * @brief Encode a param_ext_request_list struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param param_ext_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_request_list_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_param_ext_request_list_t* param_ext_request_list) +{ + return mavlink_msg_param_ext_request_list_pack_status(system_id, component_id, _status, msg, param_ext_request_list->target_system, param_ext_request_list->target_component); +} + /** * @brief Send a param_ext_request_list message * @param chan MAVLink channel to send the message @@ -170,7 +220,7 @@ static inline void mavlink_msg_param_ext_request_list_send_struct(mavlink_channe #if MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_param_ext_request_read.h b/lib/main/MAVLink/common/mavlink_msg_param_ext_request_read.h index 974f68566e4..ed540960687 100644 --- a/lib/main/MAVLink/common/mavlink_msg_param_ext_request_read.h +++ b/lib/main/MAVLink/common/mavlink_msg_param_ext_request_read.h @@ -71,7 +71,7 @@ static inline uint16_t mavlink_msg_param_ext_request_read_pack(uint8_t system_id packet.param_index = param_index; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet.param_id, param_id, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN); #endif @@ -79,6 +79,46 @@ static inline uint16_t mavlink_msg_param_ext_request_read_pack(uint8_t system_id return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC); } +/** + * @brief Pack a param_ext_request_read message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Set to -1 to use the Parameter ID field as identifier (else param_id will be ignored) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_request_read_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN]; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN); +#else + mavlink_param_ext_request_read_t packet; + packet.param_index = param_index; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN); +#endif +} + /** * @brief Pack a param_ext_request_read message on a channel * @param system_id ID of this system @@ -107,7 +147,7 @@ static inline uint16_t mavlink_msg_param_ext_request_read_pack_chan(uint8_t syst packet.param_index = param_index; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet.param_id, param_id, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN); #endif @@ -142,6 +182,20 @@ static inline uint16_t mavlink_msg_param_ext_request_read_encode_chan(uint8_t sy return mavlink_msg_param_ext_request_read_pack_chan(system_id, component_id, chan, msg, param_ext_request_read->target_system, param_ext_request_read->target_component, param_ext_request_read->param_id, param_ext_request_read->param_index); } +/** + * @brief Encode a param_ext_request_read struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param param_ext_request_read C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_request_read_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_param_ext_request_read_t* param_ext_request_read) +{ + return mavlink_msg_param_ext_request_read_pack_status(system_id, component_id, _status, msg, param_ext_request_read->target_system, param_ext_request_read->target_component, param_ext_request_read->param_id, param_ext_request_read->param_index); +} + /** * @brief Send a param_ext_request_read message * @param chan MAVLink channel to send the message @@ -167,7 +221,7 @@ static inline void mavlink_msg_param_ext_request_read_send(mavlink_channel_t cha packet.param_index = param_index; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet.param_id, param_id, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ, (const char *)&packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC); #endif } @@ -188,7 +242,7 @@ static inline void mavlink_msg_param_ext_request_read_send_struct(mavlink_channe #if MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -208,7 +262,7 @@ static inline void mavlink_msg_param_ext_request_read_send_buf(mavlink_message_t packet->param_index = param_index; packet->target_system = target_system; packet->target_component = target_component; - mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet->param_id, param_id, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ, (const char *)packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_param_ext_set.h b/lib/main/MAVLink/common/mavlink_msg_param_ext_set.h index bb4c7ccf049..b536e41798b 100644 --- a/lib/main/MAVLink/common/mavlink_msg_param_ext_set.h +++ b/lib/main/MAVLink/common/mavlink_msg_param_ext_set.h @@ -64,6 +64,45 @@ typedef struct __mavlink_param_ext_set_t { static inline uint16_t mavlink_msg_param_ext_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char *param_id, const char *param_value, uint8_t param_type) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_SET_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 146, param_type); + _mav_put_char_array(buf, 2, param_id, 16); + _mav_put_char_array(buf, 18, param_value, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN); +#else + mavlink_param_ext_set_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_type = param_type; + mav_array_assign_char(packet.param_id, param_id, 16); + mav_array_assign_char(packet.param_value, param_value, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_SET; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); +} + +/** + * @brief Pack a param_ext_set message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value + * @param param_type Parameter type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_set_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const char *param_id, const char *param_value, uint8_t param_type) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_PARAM_EXT_SET_LEN]; _mav_put_uint8_t(buf, 0, target_system); @@ -83,7 +122,11 @@ static inline uint16_t mavlink_msg_param_ext_set_pack(uint8_t system_id, uint8_t #endif msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_SET; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN); +#endif } /** @@ -116,8 +159,8 @@ static inline uint16_t mavlink_msg_param_ext_set_pack_chan(uint8_t system_id, ui packet.target_system = target_system; packet.target_component = target_component; packet.param_type = param_type; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); - mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + mav_array_assign_char(packet.param_id, param_id, 16); + mav_array_assign_char(packet.param_value, param_value, 128); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN); #endif @@ -152,6 +195,20 @@ static inline uint16_t mavlink_msg_param_ext_set_encode_chan(uint8_t system_id, return mavlink_msg_param_ext_set_pack_chan(system_id, component_id, chan, msg, param_ext_set->target_system, param_ext_set->target_component, param_ext_set->param_id, param_ext_set->param_value, param_ext_set->param_type); } +/** + * @brief Encode a param_ext_set struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param param_ext_set C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_set_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_param_ext_set_t* param_ext_set) +{ + return mavlink_msg_param_ext_set_pack_status(system_id, component_id, _status, msg, param_ext_set->target_system, param_ext_set->target_component, param_ext_set->param_id, param_ext_set->param_value, param_ext_set->param_type); +} + /** * @brief Send a param_ext_set message * @param chan MAVLink channel to send the message @@ -179,8 +236,8 @@ static inline void mavlink_msg_param_ext_set_send(mavlink_channel_t chan, uint8_ packet.target_system = target_system; packet.target_component = target_component; packet.param_type = param_type; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); - mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + mav_array_assign_char(packet.param_id, param_id, 16); + mav_array_assign_char(packet.param_value, param_value, 128); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_SET, (const char *)&packet, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); #endif } @@ -201,7 +258,7 @@ static inline void mavlink_msg_param_ext_set_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_PARAM_EXT_SET_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -222,8 +279,8 @@ static inline void mavlink_msg_param_ext_set_send_buf(mavlink_message_t *msgbuf, packet->target_system = target_system; packet->target_component = target_component; packet->param_type = param_type; - mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); - mav_array_memcpy(packet->param_value, param_value, sizeof(char)*128); + mav_array_assign_char(packet->param_id, param_id, 16); + mav_array_assign_char(packet->param_value, param_value, 128); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_SET, (const char *)packet, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_param_ext_value.h b/lib/main/MAVLink/common/mavlink_msg_param_ext_value.h index 1bf131c3900..7953dcdad89 100644 --- a/lib/main/MAVLink/common/mavlink_msg_param_ext_value.h +++ b/lib/main/MAVLink/common/mavlink_msg_param_ext_value.h @@ -64,6 +64,45 @@ typedef struct __mavlink_param_ext_value_t { static inline uint16_t mavlink_msg_param_ext_value_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char *param_id, const char *param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN]; + _mav_put_uint16_t(buf, 0, param_count); + _mav_put_uint16_t(buf, 2, param_index); + _mav_put_uint8_t(buf, 148, param_type); + _mav_put_char_array(buf, 4, param_id, 16); + _mav_put_char_array(buf, 20, param_value, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN); +#else + mavlink_param_ext_value_t packet; + packet.param_count = param_count; + packet.param_index = param_index; + packet.param_type = param_type; + mav_array_assign_char(packet.param_id, param_id, 16); + mav_array_assign_char(packet.param_value, param_value, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_VALUE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); +} + +/** + * @brief Pack a param_ext_value message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value + * @param param_type Parameter type. + * @param param_count Total number of parameters + * @param param_index Index of this parameter + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_value_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + const char *param_id, const char *param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN]; _mav_put_uint16_t(buf, 0, param_count); @@ -83,7 +122,11 @@ static inline uint16_t mavlink_msg_param_ext_value_pack(uint8_t system_id, uint8 #endif msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_VALUE; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN); +#endif } /** @@ -116,8 +159,8 @@ static inline uint16_t mavlink_msg_param_ext_value_pack_chan(uint8_t system_id, packet.param_count = param_count; packet.param_index = param_index; packet.param_type = param_type; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); - mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + mav_array_assign_char(packet.param_id, param_id, 16); + mav_array_assign_char(packet.param_value, param_value, 128); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN); #endif @@ -152,6 +195,20 @@ static inline uint16_t mavlink_msg_param_ext_value_encode_chan(uint8_t system_id return mavlink_msg_param_ext_value_pack_chan(system_id, component_id, chan, msg, param_ext_value->param_id, param_ext_value->param_value, param_ext_value->param_type, param_ext_value->param_count, param_ext_value->param_index); } +/** + * @brief Encode a param_ext_value struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param param_ext_value C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_value_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_param_ext_value_t* param_ext_value) +{ + return mavlink_msg_param_ext_value_pack_status(system_id, component_id, _status, msg, param_ext_value->param_id, param_ext_value->param_value, param_ext_value->param_type, param_ext_value->param_count, param_ext_value->param_index); +} + /** * @brief Send a param_ext_value message * @param chan MAVLink channel to send the message @@ -179,8 +236,8 @@ static inline void mavlink_msg_param_ext_value_send(mavlink_channel_t chan, cons packet.param_count = param_count; packet.param_index = param_index; packet.param_type = param_type; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); - mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + mav_array_assign_char(packet.param_id, param_id, 16); + mav_array_assign_char(packet.param_value, param_value, 128); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_VALUE, (const char *)&packet, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); #endif } @@ -201,7 +258,7 @@ static inline void mavlink_msg_param_ext_value_send_struct(mavlink_channel_t cha #if MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -222,8 +279,8 @@ static inline void mavlink_msg_param_ext_value_send_buf(mavlink_message_t *msgbu packet->param_count = param_count; packet->param_index = param_index; packet->param_type = param_type; - mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); - mav_array_memcpy(packet->param_value, param_value, sizeof(char)*128); + mav_array_assign_char(packet->param_id, param_id, 16); + mav_array_assign_char(packet->param_value, param_value, 128); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_VALUE, (const char *)packet, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_param_map_rc.h b/lib/main/MAVLink/common/mavlink_msg_param_map_rc.h index 5bea16c7ce0..8009d89984f 100755 --- a/lib/main/MAVLink/common/mavlink_msg_param_map_rc.h +++ b/lib/main/MAVLink/common/mavlink_msg_param_map_rc.h @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_param_map_rc_pack(uint8_t system_id, uint8_t packet.target_system = target_system; packet.target_component = target_component; packet.parameter_rc_channel_index = parameter_rc_channel_index; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet.param_id, param_id, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN); #endif @@ -109,6 +109,61 @@ static inline uint16_t mavlink_msg_param_map_rc_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_MAP_RC_MIN_LEN, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN, MAVLINK_MSG_ID_PARAM_MAP_RC_CRC); } +/** + * @brief Pack a param_map_rc message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored), send -2 to disable any existing map for this rc_channel_index. + * @param parameter_rc_channel_index Index of parameter RC channel. Not equal to the RC channel id. Typically corresponds to a potentiometer-knob on the RC. + * @param param_value0 Initial parameter value + * @param scale Scale, maps the RC range [-1, 1] to a parameter value + * @param param_value_min Minimum param value. The protocol does not define if this overwrites an onboard minimum value. (Depends on implementation) + * @param param_value_max Maximum param value. The protocol does not define if this overwrites an onboard maximum value. (Depends on implementation) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_map_rc_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index, uint8_t parameter_rc_channel_index, float param_value0, float scale, float param_value_min, float param_value_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_MAP_RC_LEN]; + _mav_put_float(buf, 0, param_value0); + _mav_put_float(buf, 4, scale); + _mav_put_float(buf, 8, param_value_min); + _mav_put_float(buf, 12, param_value_max); + _mav_put_int16_t(buf, 16, param_index); + _mav_put_uint8_t(buf, 18, target_system); + _mav_put_uint8_t(buf, 19, target_component); + _mav_put_uint8_t(buf, 36, parameter_rc_channel_index); + _mav_put_char_array(buf, 20, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN); +#else + mavlink_param_map_rc_t packet; + packet.param_value0 = param_value0; + packet.scale = scale; + packet.param_value_min = param_value_min; + packet.param_value_max = param_value_max; + packet.param_index = param_index; + packet.target_system = target_system; + packet.target_component = target_component; + packet.parameter_rc_channel_index = parameter_rc_channel_index; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_MAP_RC; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_MAP_RC_MIN_LEN, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN, MAVLINK_MSG_ID_PARAM_MAP_RC_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_MAP_RC_MIN_LEN, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN); +#endif +} + /** * @brief Pack a param_map_rc message on a channel * @param system_id ID of this system @@ -152,7 +207,7 @@ static inline uint16_t mavlink_msg_param_map_rc_pack_chan(uint8_t system_id, uin packet.target_system = target_system; packet.target_component = target_component; packet.parameter_rc_channel_index = parameter_rc_channel_index; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet.param_id, param_id, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN); #endif @@ -187,6 +242,20 @@ static inline uint16_t mavlink_msg_param_map_rc_encode_chan(uint8_t system_id, u return mavlink_msg_param_map_rc_pack_chan(system_id, component_id, chan, msg, param_map_rc->target_system, param_map_rc->target_component, param_map_rc->param_id, param_map_rc->param_index, param_map_rc->parameter_rc_channel_index, param_map_rc->param_value0, param_map_rc->scale, param_map_rc->param_value_min, param_map_rc->param_value_max); } +/** + * @brief Encode a param_map_rc struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param param_map_rc C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_map_rc_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_param_map_rc_t* param_map_rc) +{ + return mavlink_msg_param_map_rc_pack_status(system_id, component_id, _status, msg, param_map_rc->target_system, param_map_rc->target_component, param_map_rc->param_id, param_map_rc->param_index, param_map_rc->parameter_rc_channel_index, param_map_rc->param_value0, param_map_rc->scale, param_map_rc->param_value_min, param_map_rc->param_value_max); +} + /** * @brief Send a param_map_rc message * @param chan MAVLink channel to send the message @@ -227,7 +296,7 @@ static inline void mavlink_msg_param_map_rc_send(mavlink_channel_t chan, uint8_t packet.target_system = target_system; packet.target_component = target_component; packet.parameter_rc_channel_index = parameter_rc_channel_index; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet.param_id, param_id, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_MAP_RC, (const char *)&packet, MAVLINK_MSG_ID_PARAM_MAP_RC_MIN_LEN, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN, MAVLINK_MSG_ID_PARAM_MAP_RC_CRC); #endif } @@ -248,7 +317,7 @@ static inline void mavlink_msg_param_map_rc_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_PARAM_MAP_RC_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -278,7 +347,7 @@ static inline void mavlink_msg_param_map_rc_send_buf(mavlink_message_t *msgbuf, packet->target_system = target_system; packet->target_component = target_component; packet->parameter_rc_channel_index = parameter_rc_channel_index; - mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet->param_id, param_id, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_MAP_RC, (const char *)packet, MAVLINK_MSG_ID_PARAM_MAP_RC_MIN_LEN, MAVLINK_MSG_ID_PARAM_MAP_RC_LEN, MAVLINK_MSG_ID_PARAM_MAP_RC_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_param_request_list.h b/lib/main/MAVLink/common/mavlink_msg_param_request_list.h index e8ad2157d4b..0b95a1ca2b2 100755 --- a/lib/main/MAVLink/common/mavlink_msg_param_request_list.h +++ b/lib/main/MAVLink/common/mavlink_msg_param_request_list.h @@ -69,6 +69,42 @@ static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, ui return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); } +/** + * @brief Pack a param_request_list message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_request_list_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); +#else + mavlink_param_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); +#endif +} + /** * @brief Pack a param_request_list message on a channel * @param system_id ID of this system @@ -128,6 +164,20 @@ static inline uint16_t mavlink_msg_param_request_list_encode_chan(uint8_t system return mavlink_msg_param_request_list_pack_chan(system_id, component_id, chan, msg, param_request_list->target_system, param_request_list->target_component); } +/** + * @brief Encode a param_request_list struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param param_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_request_list_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_param_request_list_t* param_request_list) +{ + return mavlink_msg_param_request_list_pack_status(system_id, component_id, _status, msg, param_request_list->target_system, param_request_list->target_component); +} + /** * @brief Send a param_request_list message * @param chan MAVLink channel to send the message @@ -170,7 +220,7 @@ static inline void mavlink_msg_param_request_list_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_param_request_read.h b/lib/main/MAVLink/common/mavlink_msg_param_request_read.h index 92a3e866859..2b1dc5d8d96 100755 --- a/lib/main/MAVLink/common/mavlink_msg_param_request_read.h +++ b/lib/main/MAVLink/common/mavlink_msg_param_request_read.h @@ -71,7 +71,7 @@ static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, ui packet.param_index = param_index; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet.param_id, param_id, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); #endif @@ -79,6 +79,46 @@ static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, ui return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); } +/** + * @brief Pack a param_request_read message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_request_read_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN]; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); +#else + mavlink_param_request_read_t packet; + packet.param_index = param_index; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); +#endif +} + /** * @brief Pack a param_request_read message on a channel * @param system_id ID of this system @@ -107,7 +147,7 @@ static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_i packet.param_index = param_index; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet.param_id, param_id, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); #endif @@ -142,6 +182,20 @@ static inline uint16_t mavlink_msg_param_request_read_encode_chan(uint8_t system return mavlink_msg_param_request_read_pack_chan(system_id, component_id, chan, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); } +/** + * @brief Encode a param_request_read struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param param_request_read C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_request_read_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_param_request_read_t* param_request_read) +{ + return mavlink_msg_param_request_read_pack_status(system_id, component_id, _status, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); +} + /** * @brief Send a param_request_read message * @param chan MAVLink channel to send the message @@ -167,7 +221,7 @@ static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, u packet.param_index = param_index; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet.param_id, param_id, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)&packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); #endif } @@ -188,7 +242,7 @@ static inline void mavlink_msg_param_request_read_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -208,7 +262,7 @@ static inline void mavlink_msg_param_request_read_send_buf(mavlink_message_t *ms packet->param_index = param_index; packet->target_system = target_system; packet->target_component = target_component; - mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet->param_id, param_id, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_param_set.h b/lib/main/MAVLink/common/mavlink_msg_param_set.h index 72039bca9ab..98e493f55b4 100755 --- a/lib/main/MAVLink/common/mavlink_msg_param_set.h +++ b/lib/main/MAVLink/common/mavlink_msg_param_set.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t com packet.target_system = target_system; packet.target_component = target_component; packet.param_type = param_type; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet.param_id, param_id, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_SET_LEN); #endif @@ -85,6 +85,49 @@ static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t com return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); } +/** + * @brief Pack a param_set message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Onboard parameter value + * @param param_type Onboard parameter type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_set_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_SET_LEN]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 22, param_type); + _mav_put_char_array(buf, 6, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_SET_LEN); +#else + mavlink_param_set_t packet; + packet.param_value = param_value; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_SET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_SET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_SET_LEN); +#endif +} + /** * @brief Pack a param_set message on a channel * @param system_id ID of this system @@ -116,7 +159,7 @@ static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_ packet.target_system = target_system; packet.target_component = target_component; packet.param_type = param_type; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet.param_id, param_id, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_SET_LEN); #endif @@ -151,6 +194,20 @@ static inline uint16_t mavlink_msg_param_set_encode_chan(uint8_t system_id, uint return mavlink_msg_param_set_pack_chan(system_id, component_id, chan, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type); } +/** + * @brief Encode a param_set struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param param_set C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_set_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_param_set_t* param_set) +{ + return mavlink_msg_param_set_pack_status(system_id, component_id, _status, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type); +} + /** * @brief Send a param_set message * @param chan MAVLink channel to send the message @@ -179,7 +236,7 @@ static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t ta packet.target_system = target_system; packet.target_component = target_component; packet.param_type = param_type; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet.param_id, param_id, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)&packet, MAVLINK_MSG_ID_PARAM_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); #endif } @@ -200,7 +257,7 @@ static inline void mavlink_msg_param_set_send_struct(mavlink_channel_t chan, con #if MAVLINK_MSG_ID_PARAM_SET_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -222,7 +279,7 @@ static inline void mavlink_msg_param_set_send_buf(mavlink_message_t *msgbuf, mav packet->target_system = target_system; packet->target_component = target_component; packet->param_type = param_type; - mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet->param_id, param_id, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)packet, MAVLINK_MSG_ID_PARAM_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_param_value.h b/lib/main/MAVLink/common/mavlink_msg_param_value.h index e487917cc03..ad33f333c99 100755 --- a/lib/main/MAVLink/common/mavlink_msg_param_value.h +++ b/lib/main/MAVLink/common/mavlink_msg_param_value.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t c packet.param_count = param_count; packet.param_index = param_index; packet.param_type = param_type; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet.param_id, param_id, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN); #endif @@ -85,6 +85,49 @@ static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t c return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); } +/** + * @brief Pack a param_value message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Onboard parameter value + * @param param_type Onboard parameter type. + * @param param_count Total number of onboard parameters + * @param param_index Index of this onboard parameter + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_value_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + const char *param_id, float param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_VALUE_LEN]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint16_t(buf, 4, param_count); + _mav_put_uint16_t(buf, 6, param_index); + _mav_put_uint8_t(buf, 24, param_type); + _mav_put_char_array(buf, 8, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#else + mavlink_param_value_t packet; + packet.param_value = param_value; + packet.param_count = param_count; + packet.param_index = param_index; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PARAM_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#endif +} + /** * @brief Pack a param_value message on a channel * @param system_id ID of this system @@ -116,7 +159,7 @@ static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint packet.param_count = param_count; packet.param_index = param_index; packet.param_type = param_type; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet.param_id, param_id, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN); #endif @@ -151,6 +194,20 @@ static inline uint16_t mavlink_msg_param_value_encode_chan(uint8_t system_id, ui return mavlink_msg_param_value_pack_chan(system_id, component_id, chan, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); } +/** + * @brief Encode a param_value struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param param_value C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_value_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_param_value_t* param_value) +{ + return mavlink_msg_param_value_pack_status(system_id, component_id, _status, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); +} + /** * @brief Send a param_value message * @param chan MAVLink channel to send the message @@ -179,7 +236,7 @@ static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const ch packet.param_count = param_count; packet.param_index = param_index; packet.param_type = param_type; - mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet.param_id, param_id, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)&packet, MAVLINK_MSG_ID_PARAM_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); #endif } @@ -200,7 +257,7 @@ static inline void mavlink_msg_param_value_send_struct(mavlink_channel_t chan, c #if MAVLINK_MSG_ID_PARAM_VALUE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -222,7 +279,7 @@ static inline void mavlink_msg_param_value_send_buf(mavlink_message_t *msgbuf, m packet->param_count = param_count; packet->param_index = param_index; packet->param_type = param_type; - mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); + mav_array_assign_char(packet->param_id, param_id, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)packet, MAVLINK_MSG_ID_PARAM_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_ping.h b/lib/main/MAVLink/common/mavlink_msg_ping.h index 1e5508d442d..cd451873870 100755 --- a/lib/main/MAVLink/common/mavlink_msg_ping.h +++ b/lib/main/MAVLink/common/mavlink_msg_ping.h @@ -81,6 +81,48 @@ static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t componen return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PING_MIN_LEN, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); } +/** + * @brief Pack a ping message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param seq PING sequence + * @param target_system 0: request ping from all receiving systems. If greater than 0: message is a ping response and number is the system id of the requesting system + * @param target_component 0: request ping from all receiving components. If greater than 0: message is a ping response and number is the component id of the requesting component. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ping_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint32_t seq, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PING_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, seq); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PING_LEN); +#else + mavlink_ping_t packet; + packet.time_usec = time_usec; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PING_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PING; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PING_MIN_LEN, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PING_MIN_LEN, MAVLINK_MSG_ID_PING_LEN); +#endif +} + /** * @brief Pack a ping message on a channel * @param system_id ID of this system @@ -146,6 +188,20 @@ static inline uint16_t mavlink_msg_ping_encode_chan(uint8_t system_id, uint8_t c return mavlink_msg_ping_pack_chan(system_id, component_id, chan, msg, ping->time_usec, ping->seq, ping->target_system, ping->target_component); } +/** + * @brief Encode a ping struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param ping C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ping_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_ping_t* ping) +{ + return mavlink_msg_ping_pack_status(system_id, component_id, _status, msg, ping->time_usec, ping->seq, ping->target_system, ping->target_component); +} + /** * @brief Send a ping message * @param chan MAVLink channel to send the message @@ -194,7 +250,7 @@ static inline void mavlink_msg_ping_send_struct(mavlink_channel_t chan, const ma #if MAVLINK_MSG_ID_PING_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_play_tune.h b/lib/main/MAVLink/common/mavlink_msg_play_tune.h index c318448e240..1e8de9ce443 100644 --- a/lib/main/MAVLink/common/mavlink_msg_play_tune.h +++ b/lib/main/MAVLink/common/mavlink_msg_play_tune.h @@ -60,6 +60,42 @@ typedef struct __mavlink_play_tune_t { static inline uint16_t mavlink_msg_play_tune_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char *tune, const char *tune2) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PLAY_TUNE_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_char_array(buf, 2, tune, 30); + _mav_put_char_array(buf, 32, tune2, 200); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PLAY_TUNE_LEN); +#else + mavlink_play_tune_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_assign_char(packet.tune, tune, 30); + mav_array_assign_char(packet.tune2, tune2, 200); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PLAY_TUNE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PLAY_TUNE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); +} + +/** + * @brief Pack a play_tune message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param tune tune in board specific format + * @param tune2 tune extension (appended to tune) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_play_tune_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const char *tune, const char *tune2) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_PLAY_TUNE_LEN]; _mav_put_uint8_t(buf, 0, target_system); @@ -77,7 +113,11 @@ static inline uint16_t mavlink_msg_play_tune_pack(uint8_t system_id, uint8_t com #endif msg->msgid = MAVLINK_MSG_ID_PLAY_TUNE; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN); +#endif } /** @@ -107,8 +147,8 @@ static inline uint16_t mavlink_msg_play_tune_pack_chan(uint8_t system_id, uint8_ mavlink_play_tune_t packet; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.tune, tune, sizeof(char)*30); - mav_array_memcpy(packet.tune2, tune2, sizeof(char)*200); + mav_array_assign_char(packet.tune, tune, 30); + mav_array_assign_char(packet.tune2, tune2, 200); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PLAY_TUNE_LEN); #endif @@ -143,6 +183,20 @@ static inline uint16_t mavlink_msg_play_tune_encode_chan(uint8_t system_id, uint return mavlink_msg_play_tune_pack_chan(system_id, component_id, chan, msg, play_tune->target_system, play_tune->target_component, play_tune->tune, play_tune->tune2); } +/** + * @brief Encode a play_tune struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param play_tune C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_play_tune_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_play_tune_t* play_tune) +{ + return mavlink_msg_play_tune_pack_status(system_id, component_id, _status, msg, play_tune->target_system, play_tune->target_component, play_tune->tune, play_tune->tune2); +} + /** * @brief Send a play_tune message * @param chan MAVLink channel to send the message @@ -167,8 +221,8 @@ static inline void mavlink_msg_play_tune_send(mavlink_channel_t chan, uint8_t ta mavlink_play_tune_t packet; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.tune, tune, sizeof(char)*30); - mav_array_memcpy(packet.tune2, tune2, sizeof(char)*200); + mav_array_assign_char(packet.tune, tune, 30); + mav_array_assign_char(packet.tune2, tune2, 200); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE, (const char *)&packet, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); #endif } @@ -189,7 +243,7 @@ static inline void mavlink_msg_play_tune_send_struct(mavlink_channel_t chan, con #if MAVLINK_MSG_ID_PLAY_TUNE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -208,8 +262,8 @@ static inline void mavlink_msg_play_tune_send_buf(mavlink_message_t *msgbuf, mav mavlink_play_tune_t *packet = (mavlink_play_tune_t *)msgbuf; packet->target_system = target_system; packet->target_component = target_component; - mav_array_memcpy(packet->tune, tune, sizeof(char)*30); - mav_array_memcpy(packet->tune2, tune2, sizeof(char)*200); + mav_array_assign_char(packet->tune, tune, 30); + mav_array_assign_char(packet->tune2, tune2, 200); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE, (const char *)packet, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_play_tune_v2.h b/lib/main/MAVLink/common/mavlink_msg_play_tune_v2.h index c47d6e6a8a7..b81de530706 100644 --- a/lib/main/MAVLink/common/mavlink_msg_play_tune_v2.h +++ b/lib/main/MAVLink/common/mavlink_msg_play_tune_v2.h @@ -71,7 +71,7 @@ static inline uint16_t mavlink_msg_play_tune_v2_pack(uint8_t system_id, uint8_t packet.format = format; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.tune, tune, sizeof(char)*248); + mav_array_assign_char(packet.tune, tune, 248); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN); #endif @@ -79,6 +79,46 @@ static inline uint16_t mavlink_msg_play_tune_v2_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC); } +/** + * @brief Pack a play_tune_v2 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param format Tune format + * @param tune Tune definition as a NULL-terminated string. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_play_tune_v2_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t format, const char *tune) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN]; + _mav_put_uint32_t(buf, 0, format); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_char_array(buf, 6, tune, 248); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN); +#else + mavlink_play_tune_v2_t packet; + packet.format = format; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.tune, tune, sizeof(char)*248); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PLAY_TUNE_V2; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN); +#endif +} + /** * @brief Pack a play_tune_v2 message on a channel * @param system_id ID of this system @@ -107,7 +147,7 @@ static inline uint16_t mavlink_msg_play_tune_v2_pack_chan(uint8_t system_id, uin packet.format = format; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.tune, tune, sizeof(char)*248); + mav_array_assign_char(packet.tune, tune, 248); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN); #endif @@ -142,6 +182,20 @@ static inline uint16_t mavlink_msg_play_tune_v2_encode_chan(uint8_t system_id, u return mavlink_msg_play_tune_v2_pack_chan(system_id, component_id, chan, msg, play_tune_v2->target_system, play_tune_v2->target_component, play_tune_v2->format, play_tune_v2->tune); } +/** + * @brief Encode a play_tune_v2 struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param play_tune_v2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_play_tune_v2_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_play_tune_v2_t* play_tune_v2) +{ + return mavlink_msg_play_tune_v2_pack_status(system_id, component_id, _status, msg, play_tune_v2->target_system, play_tune_v2->target_component, play_tune_v2->format, play_tune_v2->tune); +} + /** * @brief Send a play_tune_v2 message * @param chan MAVLink channel to send the message @@ -167,7 +221,7 @@ static inline void mavlink_msg_play_tune_v2_send(mavlink_channel_t chan, uint8_t packet.format = format; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.tune, tune, sizeof(char)*248); + mav_array_assign_char(packet.tune, tune, 248); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE_V2, (const char *)&packet, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC); #endif } @@ -188,7 +242,7 @@ static inline void mavlink_msg_play_tune_v2_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -208,7 +262,7 @@ static inline void mavlink_msg_play_tune_v2_send_buf(mavlink_message_t *msgbuf, packet->format = format; packet->target_system = target_system; packet->target_component = target_component; - mav_array_memcpy(packet->tune, tune, sizeof(char)*248); + mav_array_assign_char(packet->tune, tune, 248); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE_V2, (const char *)packet, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_position_target_global_int.h b/lib/main/MAVLink/common/mavlink_msg_position_target_global_int.h index 93386fab756..aec85b66dff 100755 --- a/lib/main/MAVLink/common/mavlink_msg_position_target_global_int.h +++ b/lib/main/MAVLink/common/mavlink_msg_position_target_global_int.h @@ -6,8 +6,8 @@ typedef struct __mavlink_position_target_global_int_t { uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot). The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency.*/ - int32_t lat_int; /*< [degE7] X Position in WGS84 frame*/ - int32_t lon_int; /*< [degE7] Y Position in WGS84 frame*/ + int32_t lat_int; /*< [degE7] Latitude in WGS84 frame*/ + int32_t lon_int; /*< [degE7] Longitude in WGS84 frame*/ float alt; /*< [m] Altitude (MSL, AGL or relative to home altitude, depending on frame)*/ float vx; /*< [m/s] X velocity in NED frame*/ float vy; /*< [m/s] Y velocity in NED frame*/ @@ -18,7 +18,7 @@ typedef struct __mavlink_position_target_global_int_t { float yaw; /*< [rad] yaw setpoint*/ float yaw_rate; /*< [rad/s] yaw rate setpoint*/ uint16_t type_mask; /*< Bitmap to indicate which dimensions should be ignored by the vehicle.*/ - uint8_t coordinate_frame; /*< Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11*/ + uint8_t coordinate_frame; /*< Valid options are: MAV_FRAME_GLOBAL = 0, MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, MAV_FRAME_GLOBAL_TERRAIN_ALT = 10 (MAV_FRAME_GLOBAL_INT, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT are allowed synonyms, but have been deprecated)*/ } mavlink_position_target_global_int_t; #define MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN 51 @@ -81,10 +81,10 @@ typedef struct __mavlink_position_target_global_int_t { * @param msg The MAVLink message to compress the data into * * @param time_boot_ms [ms] Timestamp (time since system boot). The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. - * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL = 0, MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, MAV_FRAME_GLOBAL_TERRAIN_ALT = 10 (MAV_FRAME_GLOBAL_INT, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT are allowed synonyms, but have been deprecated) * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. - * @param lat_int [degE7] X Position in WGS84 frame - * @param lon_int [degE7] Y Position in WGS84 frame + * @param lat_int [degE7] Latitude in WGS84 frame + * @param lon_int [degE7] Longitude in WGS84 frame * @param alt [m] Altitude (MSL, AGL or relative to home altitude, depending on frame) * @param vx [m/s] X velocity in NED frame * @param vy [m/s] Y velocity in NED frame @@ -141,6 +141,78 @@ static inline uint16_t mavlink_msg_position_target_global_int_pack(uint8_t syste return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_MIN_LEN, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_CRC); } +/** + * @brief Pack a position_target_global_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL = 0, MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, MAV_FRAME_GLOBAL_TERRAIN_ALT = 10 (MAV_FRAME_GLOBAL_INT, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT are allowed synonyms, but have been deprecated) + * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. + * @param lat_int [degE7] Latitude in WGS84 frame + * @param lon_int [degE7] Longitude in WGS84 frame + * @param alt [m] Altitude (MSL, AGL or relative to home altitude, depending on frame) + * @param vx [m/s] X velocity in NED frame + * @param vy [m/s] Y velocity in NED frame + * @param vz [m/s] Z velocity in NED frame + * @param afx [m/s/s] X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy [m/s/s] Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz [m/s/s] Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw [rad] yaw setpoint + * @param yaw_rate [rad/s] yaw rate setpoint + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_position_target_global_int_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#else + mavlink_position_target_global_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_int = lat_int; + packet.lon_int = lon_int; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_MIN_LEN, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_MIN_LEN, MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +} + /** * @brief Pack a position_target_global_int message on a channel * @param system_id ID of this system @@ -148,10 +220,10 @@ static inline uint16_t mavlink_msg_position_target_global_int_pack(uint8_t syste * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms [ms] Timestamp (time since system boot). The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. - * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL = 0, MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, MAV_FRAME_GLOBAL_TERRAIN_ALT = 10 (MAV_FRAME_GLOBAL_INT, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT are allowed synonyms, but have been deprecated) * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. - * @param lat_int [degE7] X Position in WGS84 frame - * @param lon_int [degE7] Y Position in WGS84 frame + * @param lat_int [degE7] Latitude in WGS84 frame + * @param lon_int [degE7] Longitude in WGS84 frame * @param alt [m] Altitude (MSL, AGL or relative to home altitude, depending on frame) * @param vx [m/s] X velocity in NED frame * @param vy [m/s] Y velocity in NED frame @@ -236,15 +308,29 @@ static inline uint16_t mavlink_msg_position_target_global_int_encode_chan(uint8_ return mavlink_msg_position_target_global_int_pack_chan(system_id, component_id, chan, msg, position_target_global_int->time_boot_ms, position_target_global_int->coordinate_frame, position_target_global_int->type_mask, position_target_global_int->lat_int, position_target_global_int->lon_int, position_target_global_int->alt, position_target_global_int->vx, position_target_global_int->vy, position_target_global_int->vz, position_target_global_int->afx, position_target_global_int->afy, position_target_global_int->afz, position_target_global_int->yaw, position_target_global_int->yaw_rate); } +/** + * @brief Encode a position_target_global_int struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param position_target_global_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_position_target_global_int_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_position_target_global_int_t* position_target_global_int) +{ + return mavlink_msg_position_target_global_int_pack_status(system_id, component_id, _status, msg, position_target_global_int->time_boot_ms, position_target_global_int->coordinate_frame, position_target_global_int->type_mask, position_target_global_int->lat_int, position_target_global_int->lon_int, position_target_global_int->alt, position_target_global_int->vx, position_target_global_int->vy, position_target_global_int->vz, position_target_global_int->afx, position_target_global_int->afy, position_target_global_int->afz, position_target_global_int->yaw, position_target_global_int->yaw_rate); +} + /** * @brief Send a position_target_global_int message * @param chan MAVLink channel to send the message * * @param time_boot_ms [ms] Timestamp (time since system boot). The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. - * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL = 0, MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, MAV_FRAME_GLOBAL_TERRAIN_ALT = 10 (MAV_FRAME_GLOBAL_INT, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT are allowed synonyms, but have been deprecated) * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. - * @param lat_int [degE7] X Position in WGS84 frame - * @param lon_int [degE7] Y Position in WGS84 frame + * @param lat_int [degE7] Latitude in WGS84 frame + * @param lon_int [degE7] Longitude in WGS84 frame * @param alt [m] Altitude (MSL, AGL or relative to home altitude, depending on frame) * @param vx [m/s] X velocity in NED frame * @param vy [m/s] Y velocity in NED frame @@ -314,7 +400,7 @@ static inline void mavlink_msg_position_target_global_int_send_struct(mavlink_ch #if MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -380,7 +466,7 @@ static inline uint32_t mavlink_msg_position_target_global_int_get_time_boot_ms(c /** * @brief Get field coordinate_frame from position_target_global_int message * - * @return Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @return Valid options are: MAV_FRAME_GLOBAL = 0, MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, MAV_FRAME_GLOBAL_TERRAIN_ALT = 10 (MAV_FRAME_GLOBAL_INT, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT are allowed synonyms, but have been deprecated) */ static inline uint8_t mavlink_msg_position_target_global_int_get_coordinate_frame(const mavlink_message_t* msg) { @@ -400,7 +486,7 @@ static inline uint16_t mavlink_msg_position_target_global_int_get_type_mask(cons /** * @brief Get field lat_int from position_target_global_int message * - * @return [degE7] X Position in WGS84 frame + * @return [degE7] Latitude in WGS84 frame */ static inline int32_t mavlink_msg_position_target_global_int_get_lat_int(const mavlink_message_t* msg) { @@ -410,7 +496,7 @@ static inline int32_t mavlink_msg_position_target_global_int_get_lat_int(const m /** * @brief Get field lon_int from position_target_global_int message * - * @return [degE7] Y Position in WGS84 frame + * @return [degE7] Longitude in WGS84 frame */ static inline int32_t mavlink_msg_position_target_global_int_get_lon_int(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_position_target_local_ned.h b/lib/main/MAVLink/common/mavlink_msg_position_target_local_ned.h index f3cfde26e3d..9ea5ec630c7 100755 --- a/lib/main/MAVLink/common/mavlink_msg_position_target_local_ned.h +++ b/lib/main/MAVLink/common/mavlink_msg_position_target_local_ned.h @@ -141,6 +141,78 @@ static inline uint16_t mavlink_msg_position_target_local_ned_pack(uint8_t system return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_MIN_LEN, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_CRC); } +/** + * @brief Pack a position_target_local_ned message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. + * @param x [m] X Position in NED frame + * @param y [m] Y Position in NED frame + * @param z [m] Z Position in NED frame (note, altitude is negative in NED) + * @param vx [m/s] X velocity in NED frame + * @param vy [m/s] Y velocity in NED frame + * @param vz [m/s] Z velocity in NED frame + * @param afx [m/s/s] X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy [m/s/s] Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz [m/s/s] Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw [rad] yaw setpoint + * @param yaw_rate [rad/s] yaw rate setpoint + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_position_target_local_ned_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t coordinate_frame, uint16_t type_mask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#else + mavlink_position_target_local_ned_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_MIN_LEN, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_MIN_LEN, MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN); +#endif +} + /** * @brief Pack a position_target_local_ned message on a channel * @param system_id ID of this system @@ -236,6 +308,20 @@ static inline uint16_t mavlink_msg_position_target_local_ned_encode_chan(uint8_t return mavlink_msg_position_target_local_ned_pack_chan(system_id, component_id, chan, msg, position_target_local_ned->time_boot_ms, position_target_local_ned->coordinate_frame, position_target_local_ned->type_mask, position_target_local_ned->x, position_target_local_ned->y, position_target_local_ned->z, position_target_local_ned->vx, position_target_local_ned->vy, position_target_local_ned->vz, position_target_local_ned->afx, position_target_local_ned->afy, position_target_local_ned->afz, position_target_local_ned->yaw, position_target_local_ned->yaw_rate); } +/** + * @brief Encode a position_target_local_ned struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param position_target_local_ned C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_position_target_local_ned_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_position_target_local_ned_t* position_target_local_ned) +{ + return mavlink_msg_position_target_local_ned_pack_status(system_id, component_id, _status, msg, position_target_local_ned->time_boot_ms, position_target_local_ned->coordinate_frame, position_target_local_ned->type_mask, position_target_local_ned->x, position_target_local_ned->y, position_target_local_ned->z, position_target_local_ned->vx, position_target_local_ned->vy, position_target_local_ned->vz, position_target_local_ned->afx, position_target_local_ned->afy, position_target_local_ned->afz, position_target_local_ned->yaw, position_target_local_ned->yaw_rate); +} + /** * @brief Send a position_target_local_ned message * @param chan MAVLink channel to send the message @@ -314,7 +400,7 @@ static inline void mavlink_msg_position_target_local_ned_send_struct(mavlink_cha #if MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_power_status.h b/lib/main/MAVLink/common/mavlink_msg_power_status.h index 67567680cd8..e9436c7e5ca 100755 --- a/lib/main/MAVLink/common/mavlink_msg_power_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_power_status.h @@ -75,6 +75,45 @@ static inline uint16_t mavlink_msg_power_status_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POWER_STATUS_MIN_LEN, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); } +/** + * @brief Pack a power_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param Vcc [mV] 5V rail voltage. + * @param Vservo [mV] Servo rail voltage. + * @param flags Bitmap of power supply status flags. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_power_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint16_t Vcc, uint16_t Vservo, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint16_t(buf, 2, Vservo); + _mav_put_uint16_t(buf, 4, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#else + mavlink_power_status_t packet; + packet.Vcc = Vcc; + packet.Vservo = Vservo; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POWER_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_POWER_STATUS_MIN_LEN, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_POWER_STATUS_MIN_LEN, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +} + /** * @brief Pack a power_status message on a channel * @param system_id ID of this system @@ -137,6 +176,20 @@ static inline uint16_t mavlink_msg_power_status_encode_chan(uint8_t system_id, u return mavlink_msg_power_status_pack_chan(system_id, component_id, chan, msg, power_status->Vcc, power_status->Vservo, power_status->flags); } +/** + * @brief Encode a power_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param power_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_power_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_power_status_t* power_status) +{ + return mavlink_msg_power_status_pack_status(system_id, component_id, _status, msg, power_status->Vcc, power_status->Vservo, power_status->flags); +} + /** * @brief Send a power_status message * @param chan MAVLink channel to send the message @@ -182,7 +235,7 @@ static inline void mavlink_msg_power_status_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_POWER_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_radio_status.h b/lib/main/MAVLink/common/mavlink_msg_radio_status.h index 8983a20a4c9..4c5f2ab4d23 100755 --- a/lib/main/MAVLink/common/mavlink_msg_radio_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_radio_status.h @@ -7,11 +7,11 @@ typedef struct __mavlink_radio_status_t { uint16_t rxerrors; /*< Count of radio packet receive errors (since boot).*/ uint16_t fixed; /*< Count of error corrected radio packets (since boot).*/ - uint8_t rssi; /*< Local (message sender) recieved signal strength indication in device-dependent units/scale. Values: [0-254], 255: invalid/unknown.*/ - uint8_t remrssi; /*< Remote (message receiver) signal strength indication in device-dependent units/scale. Values: [0-254], 255: invalid/unknown.*/ + uint8_t rssi; /*< Local (message sender) received signal strength indication in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown.*/ + uint8_t remrssi; /*< Remote (message receiver) signal strength indication in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown.*/ uint8_t txbuf; /*< [%] Remaining free transmitter buffer space.*/ - uint8_t noise; /*< Local background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], 255: invalid/unknown.*/ - uint8_t remnoise; /*< Remote background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], 255: invalid/unknown.*/ + uint8_t noise; /*< Local background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], UINT8_MAX: invalid/unknown.*/ + uint8_t remnoise; /*< Remote background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], UINT8_MAX: invalid/unknown.*/ } mavlink_radio_status_t; #define MAVLINK_MSG_ID_RADIO_STATUS_LEN 9 @@ -59,11 +59,11 @@ typedef struct __mavlink_radio_status_t { * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param rssi Local (message sender) recieved signal strength indication in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. - * @param remrssi Remote (message receiver) signal strength indication in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @param rssi Local (message sender) received signal strength indication in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. + * @param remrssi Remote (message receiver) signal strength indication in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. * @param txbuf [%] Remaining free transmitter buffer space. - * @param noise Local background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], 255: invalid/unknown. - * @param remnoise Remote background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], 255: invalid/unknown. + * @param noise Local background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], UINT8_MAX: invalid/unknown. + * @param remnoise Remote background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], UINT8_MAX: invalid/unknown. * @param rxerrors Count of radio packet receive errors (since boot). * @param fixed Count of error corrected radio packets (since boot). * @return length of the message in bytes (excluding serial stream start sign) @@ -99,17 +99,68 @@ static inline uint16_t mavlink_msg_radio_status_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_STATUS_MIN_LEN, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); } +/** + * @brief Pack a radio_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param rssi Local (message sender) received signal strength indication in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. + * @param remrssi Remote (message receiver) signal strength indication in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. + * @param txbuf [%] Remaining free transmitter buffer space. + * @param noise Local background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], UINT8_MAX: invalid/unknown. + * @param remnoise Remote background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], UINT8_MAX: invalid/unknown. + * @param rxerrors Count of radio packet receive errors (since boot). + * @param fixed Count of error corrected radio packets (since boot). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_radio_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RADIO_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, fixed); + _mav_put_uint8_t(buf, 4, rssi); + _mav_put_uint8_t(buf, 5, remrssi); + _mav_put_uint8_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 7, noise); + _mav_put_uint8_t(buf, 8, remnoise); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#else + mavlink_radio_status_t packet; + packet.rxerrors = rxerrors; + packet.fixed = fixed; + packet.rssi = rssi; + packet.remrssi = remrssi; + packet.txbuf = txbuf; + packet.noise = noise; + packet.remnoise = remnoise; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RADIO_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RADIO_STATUS_MIN_LEN, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RADIO_STATUS_MIN_LEN, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +} + /** * @brief Pack a radio_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into - * @param rssi Local (message sender) recieved signal strength indication in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. - * @param remrssi Remote (message receiver) signal strength indication in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @param rssi Local (message sender) received signal strength indication in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. + * @param remrssi Remote (message receiver) signal strength indication in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. * @param txbuf [%] Remaining free transmitter buffer space. - * @param noise Local background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], 255: invalid/unknown. - * @param remnoise Remote background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], 255: invalid/unknown. + * @param noise Local background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], UINT8_MAX: invalid/unknown. + * @param remnoise Remote background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], UINT8_MAX: invalid/unknown. * @param rxerrors Count of radio packet receive errors (since boot). * @param fixed Count of error corrected radio packets (since boot). * @return length of the message in bytes (excluding serial stream start sign) @@ -173,15 +224,29 @@ static inline uint16_t mavlink_msg_radio_status_encode_chan(uint8_t system_id, u return mavlink_msg_radio_status_pack_chan(system_id, component_id, chan, msg, radio_status->rssi, radio_status->remrssi, radio_status->txbuf, radio_status->noise, radio_status->remnoise, radio_status->rxerrors, radio_status->fixed); } +/** + * @brief Encode a radio_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param radio_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_radio_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_radio_status_t* radio_status) +{ + return mavlink_msg_radio_status_pack_status(system_id, component_id, _status, msg, radio_status->rssi, radio_status->remrssi, radio_status->txbuf, radio_status->noise, radio_status->remnoise, radio_status->rxerrors, radio_status->fixed); +} + /** * @brief Send a radio_status message * @param chan MAVLink channel to send the message * - * @param rssi Local (message sender) recieved signal strength indication in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. - * @param remrssi Remote (message receiver) signal strength indication in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @param rssi Local (message sender) received signal strength indication in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. + * @param remrssi Remote (message receiver) signal strength indication in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. * @param txbuf [%] Remaining free transmitter buffer space. - * @param noise Local background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], 255: invalid/unknown. - * @param remnoise Remote background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], 255: invalid/unknown. + * @param noise Local background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], UINT8_MAX: invalid/unknown. + * @param remnoise Remote background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], UINT8_MAX: invalid/unknown. * @param rxerrors Count of radio packet receive errors (since boot). * @param fixed Count of error corrected radio packets (since boot). */ @@ -230,7 +295,7 @@ static inline void mavlink_msg_radio_status_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_RADIO_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -272,7 +337,7 @@ static inline void mavlink_msg_radio_status_send_buf(mavlink_message_t *msgbuf, /** * @brief Get field rssi from radio_status message * - * @return Local (message sender) recieved signal strength indication in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @return Local (message sender) received signal strength indication in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. */ static inline uint8_t mavlink_msg_radio_status_get_rssi(const mavlink_message_t* msg) { @@ -282,7 +347,7 @@ static inline uint8_t mavlink_msg_radio_status_get_rssi(const mavlink_message_t* /** * @brief Get field remrssi from radio_status message * - * @return Remote (message receiver) signal strength indication in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @return Remote (message receiver) signal strength indication in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. */ static inline uint8_t mavlink_msg_radio_status_get_remrssi(const mavlink_message_t* msg) { @@ -302,7 +367,7 @@ static inline uint8_t mavlink_msg_radio_status_get_txbuf(const mavlink_message_t /** * @brief Get field noise from radio_status message * - * @return Local background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], 255: invalid/unknown. + * @return Local background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], UINT8_MAX: invalid/unknown. */ static inline uint8_t mavlink_msg_radio_status_get_noise(const mavlink_message_t* msg) { @@ -312,7 +377,7 @@ static inline uint8_t mavlink_msg_radio_status_get_noise(const mavlink_message_t /** * @brief Get field remnoise from radio_status message * - * @return Remote background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], 255: invalid/unknown. + * @return Remote background noise level. These are device dependent RSSI values (scale as approx 2x dB on SiK radios). Values: [0-254], UINT8_MAX: invalid/unknown. */ static inline uint8_t mavlink_msg_radio_status_get_remnoise(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_raw_imu.h b/lib/main/MAVLink/common/mavlink_msg_raw_imu.h index eeb4eacb2e3..2819a3bc4ba 100755 --- a/lib/main/MAVLink/common/mavlink_msg_raw_imu.h +++ b/lib/main/MAVLink/common/mavlink_msg_raw_imu.h @@ -129,6 +129,72 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_IMU_MIN_LEN, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); } +/** + * @brief Pack a raw_imu message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param xacc X acceleration (raw) + * @param yacc Y acceleration (raw) + * @param zacc Z acceleration (raw) + * @param xgyro Angular speed around X axis (raw) + * @param ygyro Angular speed around Y axis (raw) + * @param zgyro Angular speed around Z axis (raw) + * @param xmag X Magnetic field (raw) + * @param ymag Y Magnetic field (raw) + * @param zmag Z Magnetic field (raw) + * @param id Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_raw_imu_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, uint8_t id, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RAW_IMU_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, xacc); + _mav_put_int16_t(buf, 10, yacc); + _mav_put_int16_t(buf, 12, zacc); + _mav_put_int16_t(buf, 14, xgyro); + _mav_put_int16_t(buf, 16, ygyro); + _mav_put_int16_t(buf, 18, zgyro); + _mav_put_int16_t(buf, 20, xmag); + _mav_put_int16_t(buf, 22, ymag); + _mav_put_int16_t(buf, 24, zmag); + _mav_put_uint8_t(buf, 26, id); + _mav_put_int16_t(buf, 27, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_IMU_LEN); +#else + mavlink_raw_imu_t packet; + packet.time_usec = time_usec; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + packet.id = id; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_IMU_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RAW_IMU; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RAW_IMU_MIN_LEN, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RAW_IMU_MIN_LEN, MAVLINK_MSG_ID_RAW_IMU_LEN); +#endif +} + /** * @brief Pack a raw_imu message on a channel * @param system_id ID of this system @@ -218,6 +284,20 @@ static inline uint16_t mavlink_msg_raw_imu_encode_chan(uint8_t system_id, uint8_ return mavlink_msg_raw_imu_pack_chan(system_id, component_id, chan, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag, raw_imu->id, raw_imu->temperature); } +/** + * @brief Encode a raw_imu struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param raw_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_imu_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_raw_imu_t* raw_imu) +{ + return mavlink_msg_raw_imu_pack_status(system_id, component_id, _status, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag, raw_imu->id, raw_imu->temperature); +} + /** * @brief Send a raw_imu message * @param chan MAVLink channel to send the message @@ -290,7 +370,7 @@ static inline void mavlink_msg_raw_imu_send_struct(mavlink_channel_t chan, const #if MAVLINK_MSG_ID_RAW_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_raw_pressure.h b/lib/main/MAVLink/common/mavlink_msg_raw_pressure.h index 9b80a848e7a..62273820f83 100755 --- a/lib/main/MAVLink/common/mavlink_msg_raw_pressure.h +++ b/lib/main/MAVLink/common/mavlink_msg_raw_pressure.h @@ -87,6 +87,51 @@ static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); } +/** + * @brief Pack a raw_pressure message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param press_abs Absolute pressure (raw) + * @param press_diff1 Differential pressure 1 (raw, 0 if nonexistent) + * @param press_diff2 Differential pressure 2 (raw, 0 if nonexistent) + * @param temperature Raw Temperature measurement (raw) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_raw_pressure_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RAW_PRESSURE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, press_abs); + _mav_put_int16_t(buf, 10, press_diff1); + _mav_put_int16_t(buf, 12, press_diff2); + _mav_put_int16_t(buf, 14, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#else + mavlink_raw_pressure_t packet; + packet.time_usec = time_usec; + packet.press_abs = press_abs; + packet.press_diff1 = press_diff1; + packet.press_diff2 = press_diff2; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RAW_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RAW_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#endif +} + /** * @brief Pack a raw_pressure message on a channel * @param system_id ID of this system @@ -155,6 +200,20 @@ static inline uint16_t mavlink_msg_raw_pressure_encode_chan(uint8_t system_id, u return mavlink_msg_raw_pressure_pack_chan(system_id, component_id, chan, msg, raw_pressure->time_usec, raw_pressure->press_abs, raw_pressure->press_diff1, raw_pressure->press_diff2, raw_pressure->temperature); } +/** + * @brief Encode a raw_pressure struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param raw_pressure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_pressure_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_raw_pressure_t* raw_pressure) +{ + return mavlink_msg_raw_pressure_pack_status(system_id, component_id, _status, msg, raw_pressure->time_usec, raw_pressure->press_abs, raw_pressure->press_diff1, raw_pressure->press_diff2, raw_pressure->temperature); +} + /** * @brief Send a raw_pressure message * @param chan MAVLink channel to send the message @@ -206,7 +265,7 @@ static inline void mavlink_msg_raw_pressure_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_RAW_PRESSURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_raw_rpm.h b/lib/main/MAVLink/common/mavlink_msg_raw_rpm.h index 0fa4f1a0b5f..946f1524202 100644 --- a/lib/main/MAVLink/common/mavlink_msg_raw_rpm.h +++ b/lib/main/MAVLink/common/mavlink_msg_raw_rpm.h @@ -69,6 +69,42 @@ static inline uint16_t mavlink_msg_raw_rpm_pack(uint8_t system_id, uint8_t compo return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_RPM_MIN_LEN, MAVLINK_MSG_ID_RAW_RPM_LEN, MAVLINK_MSG_ID_RAW_RPM_CRC); } +/** + * @brief Pack a raw_rpm message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param index Index of this RPM sensor (0-indexed) + * @param frequency [rpm] Indicated rate + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_raw_rpm_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t index, float frequency) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RAW_RPM_LEN]; + _mav_put_float(buf, 0, frequency); + _mav_put_uint8_t(buf, 4, index); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_RPM_LEN); +#else + mavlink_raw_rpm_t packet; + packet.frequency = frequency; + packet.index = index; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_RPM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RAW_RPM; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RAW_RPM_MIN_LEN, MAVLINK_MSG_ID_RAW_RPM_LEN, MAVLINK_MSG_ID_RAW_RPM_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RAW_RPM_MIN_LEN, MAVLINK_MSG_ID_RAW_RPM_LEN); +#endif +} + /** * @brief Pack a raw_rpm message on a channel * @param system_id ID of this system @@ -128,6 +164,20 @@ static inline uint16_t mavlink_msg_raw_rpm_encode_chan(uint8_t system_id, uint8_ return mavlink_msg_raw_rpm_pack_chan(system_id, component_id, chan, msg, raw_rpm->index, raw_rpm->frequency); } +/** + * @brief Encode a raw_rpm struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param raw_rpm C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_rpm_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_raw_rpm_t* raw_rpm) +{ + return mavlink_msg_raw_rpm_pack_status(system_id, component_id, _status, msg, raw_rpm->index, raw_rpm->frequency); +} + /** * @brief Send a raw_rpm message * @param chan MAVLink channel to send the message @@ -170,7 +220,7 @@ static inline void mavlink_msg_raw_rpm_send_struct(mavlink_channel_t chan, const #if MAVLINK_MSG_ID_RAW_RPM_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_rc_channels.h b/lib/main/MAVLink/common/mavlink_msg_rc_channels.h index f4694f1b89e..55207fa1f3a 100755 --- a/lib/main/MAVLink/common/mavlink_msg_rc_channels.h +++ b/lib/main/MAVLink/common/mavlink_msg_rc_channels.h @@ -25,7 +25,7 @@ typedef struct __mavlink_rc_channels_t { uint16_t chan17_raw; /*< [us] RC channel 17 value.*/ uint16_t chan18_raw; /*< [us] RC channel 18 value.*/ uint8_t chancount; /*< Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available.*/ - uint8_t rssi; /*< Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown.*/ + uint8_t rssi; /*< Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown.*/ } mavlink_rc_channels_t; #define MAVLINK_MSG_ID_RC_CHANNELS_LEN 42 @@ -121,7 +121,7 @@ typedef struct __mavlink_rc_channels_t { * @param chan16_raw [us] RC channel 16 value. * @param chan17_raw [us] RC channel 17 value. * @param chan18_raw [us] RC channel 18 value. - * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rc_channels_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -183,6 +183,99 @@ static inline uint16_t mavlink_msg_rc_channels_pack(uint8_t system_id, uint8_t c return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); } +/** + * @brief Pack a rc_channels message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + * @param chan1_raw [us] RC channel 1 value. + * @param chan2_raw [us] RC channel 2 value. + * @param chan3_raw [us] RC channel 3 value. + * @param chan4_raw [us] RC channel 4 value. + * @param chan5_raw [us] RC channel 5 value. + * @param chan6_raw [us] RC channel 6 value. + * @param chan7_raw [us] RC channel 7 value. + * @param chan8_raw [us] RC channel 8 value. + * @param chan9_raw [us] RC channel 9 value. + * @param chan10_raw [us] RC channel 10 value. + * @param chan11_raw [us] RC channel 11 value. + * @param chan12_raw [us] RC channel 12 value. + * @param chan13_raw [us] RC channel 13 value. + * @param chan14_raw [us] RC channel 14 value. + * @param chan15_raw [us] RC channel 15 value. + * @param chan16_raw [us] RC channel 16 value. + * @param chan17_raw [us] RC channel 17 value. + * @param chan18_raw [us] RC channel 18 value. + * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t chancount, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint16_t(buf, 20, chan9_raw); + _mav_put_uint16_t(buf, 22, chan10_raw); + _mav_put_uint16_t(buf, 24, chan11_raw); + _mav_put_uint16_t(buf, 26, chan12_raw); + _mav_put_uint16_t(buf, 28, chan13_raw); + _mav_put_uint16_t(buf, 30, chan14_raw); + _mav_put_uint16_t(buf, 32, chan15_raw); + _mav_put_uint16_t(buf, 34, chan16_raw); + _mav_put_uint16_t(buf, 36, chan17_raw); + _mav_put_uint16_t(buf, 38, chan18_raw); + _mav_put_uint8_t(buf, 40, chancount); + _mav_put_uint8_t(buf, 41, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#else + mavlink_rc_channels_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.chan13_raw = chan13_raw; + packet.chan14_raw = chan14_raw; + packet.chan15_raw = chan15_raw; + packet.chan16_raw = chan16_raw; + packet.chan17_raw = chan17_raw; + packet.chan18_raw = chan18_raw; + packet.chancount = chancount; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RC_CHANNELS_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RC_CHANNELS_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +} + /** * @brief Pack a rc_channels message on a channel * @param system_id ID of this system @@ -209,7 +302,7 @@ static inline uint16_t mavlink_msg_rc_channels_pack(uint8_t system_id, uint8_t c * @param chan16_raw [us] RC channel 16 value. * @param chan17_raw [us] RC channel 17 value. * @param chan18_raw [us] RC channel 18 value. - * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rc_channels_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -299,6 +392,20 @@ static inline uint16_t mavlink_msg_rc_channels_encode_chan(uint8_t system_id, ui return mavlink_msg_rc_channels_pack_chan(system_id, component_id, chan, msg, rc_channels->time_boot_ms, rc_channels->chancount, rc_channels->chan1_raw, rc_channels->chan2_raw, rc_channels->chan3_raw, rc_channels->chan4_raw, rc_channels->chan5_raw, rc_channels->chan6_raw, rc_channels->chan7_raw, rc_channels->chan8_raw, rc_channels->chan9_raw, rc_channels->chan10_raw, rc_channels->chan11_raw, rc_channels->chan12_raw, rc_channels->chan13_raw, rc_channels->chan14_raw, rc_channels->chan15_raw, rc_channels->chan16_raw, rc_channels->chan17_raw, rc_channels->chan18_raw, rc_channels->rssi); } +/** + * @brief Encode a rc_channels struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param rc_channels C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_rc_channels_t* rc_channels) +{ + return mavlink_msg_rc_channels_pack_status(system_id, component_id, _status, msg, rc_channels->time_boot_ms, rc_channels->chancount, rc_channels->chan1_raw, rc_channels->chan2_raw, rc_channels->chan3_raw, rc_channels->chan4_raw, rc_channels->chan5_raw, rc_channels->chan6_raw, rc_channels->chan7_raw, rc_channels->chan8_raw, rc_channels->chan9_raw, rc_channels->chan10_raw, rc_channels->chan11_raw, rc_channels->chan12_raw, rc_channels->chan13_raw, rc_channels->chan14_raw, rc_channels->chan15_raw, rc_channels->chan16_raw, rc_channels->chan17_raw, rc_channels->chan18_raw, rc_channels->rssi); +} + /** * @brief Send a rc_channels message * @param chan MAVLink channel to send the message @@ -323,7 +430,7 @@ static inline uint16_t mavlink_msg_rc_channels_encode_chan(uint8_t system_id, ui * @param chan16_raw [us] RC channel 16 value. * @param chan17_raw [us] RC channel 17 value. * @param chan18_raw [us] RC channel 18 value. - * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -398,7 +505,7 @@ static inline void mavlink_msg_rc_channels_send_struct(mavlink_channel_t chan, c #if MAVLINK_MSG_ID_RC_CHANNELS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -668,7 +775,7 @@ static inline uint16_t mavlink_msg_rc_channels_get_chan18_raw(const mavlink_mess /** * @brief Get field rssi from rc_channels message * - * @return Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @return Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. */ static inline uint8_t mavlink_msg_rc_channels_get_rssi(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_rc_channels_override.h b/lib/main/MAVLink/common/mavlink_msg_rc_channels_override.h index a0d2a767ec2..3e22869477a 100755 --- a/lib/main/MAVLink/common/mavlink_msg_rc_channels_override.h +++ b/lib/main/MAVLink/common/mavlink_msg_rc_channels_override.h @@ -177,6 +177,96 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); } +/** + * @brief Pack a rc_channels_override message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param chan1_raw [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan2_raw [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan3_raw [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan4_raw [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan5_raw [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan6_raw [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan7_raw [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan8_raw [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan9_raw [us] RC channel 9 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan10_raw [us] RC channel 10 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan11_raw [us] RC channel 11 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan12_raw [us] RC channel 12 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan13_raw [us] RC channel 13 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan14_raw [us] RC channel 14 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan15_raw [us] RC channel 15 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan16_raw [us] RC channel 16 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan17_raw [us] RC channel 17 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan18_raw [us] RC channel 18 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_override_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN]; + _mav_put_uint16_t(buf, 0, chan1_raw); + _mav_put_uint16_t(buf, 2, chan2_raw); + _mav_put_uint16_t(buf, 4, chan3_raw); + _mav_put_uint16_t(buf, 6, chan4_raw); + _mav_put_uint16_t(buf, 8, chan5_raw); + _mav_put_uint16_t(buf, 10, chan6_raw); + _mav_put_uint16_t(buf, 12, chan7_raw); + _mav_put_uint16_t(buf, 14, chan8_raw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + _mav_put_uint16_t(buf, 18, chan9_raw); + _mav_put_uint16_t(buf, 20, chan10_raw); + _mav_put_uint16_t(buf, 22, chan11_raw); + _mav_put_uint16_t(buf, 24, chan12_raw); + _mav_put_uint16_t(buf, 26, chan13_raw); + _mav_put_uint16_t(buf, 28, chan14_raw); + _mav_put_uint16_t(buf, 30, chan15_raw); + _mav_put_uint16_t(buf, 32, chan16_raw); + _mav_put_uint16_t(buf, 34, chan17_raw); + _mav_put_uint16_t(buf, 36, chan18_raw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#else + mavlink_rc_channels_override_t packet; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.target_system = target_system; + packet.target_component = target_component; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.chan13_raw = chan13_raw; + packet.chan14_raw = chan14_raw; + packet.chan15_raw = chan15_raw; + packet.chan16_raw = chan16_raw; + packet.chan17_raw = chan17_raw; + packet.chan18_raw = chan18_raw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#endif +} + /** * @brief Pack a rc_channels_override message on a channel * @param system_id ID of this system @@ -290,6 +380,20 @@ static inline uint16_t mavlink_msg_rc_channels_override_encode_chan(uint8_t syst return mavlink_msg_rc_channels_override_pack_chan(system_id, component_id, chan, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw, rc_channels_override->chan9_raw, rc_channels_override->chan10_raw, rc_channels_override->chan11_raw, rc_channels_override->chan12_raw, rc_channels_override->chan13_raw, rc_channels_override->chan14_raw, rc_channels_override->chan15_raw, rc_channels_override->chan16_raw, rc_channels_override->chan17_raw, rc_channels_override->chan18_raw); } +/** + * @brief Encode a rc_channels_override struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param rc_channels_override C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_override_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override) +{ + return mavlink_msg_rc_channels_override_pack_status(system_id, component_id, _status, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw, rc_channels_override->chan9_raw, rc_channels_override->chan10_raw, rc_channels_override->chan11_raw, rc_channels_override->chan12_raw, rc_channels_override->chan13_raw, rc_channels_override->chan14_raw, rc_channels_override->chan15_raw, rc_channels_override->chan16_raw, rc_channels_override->chan17_raw, rc_channels_override->chan18_raw); +} + /** * @brief Send a rc_channels_override message * @param chan MAVLink channel to send the message @@ -386,7 +490,7 @@ static inline void mavlink_msg_rc_channels_override_send_struct(mavlink_channel_ #if MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_rc_channels_raw.h b/lib/main/MAVLink/common/mavlink_msg_rc_channels_raw.h index cbd7fd84289..80a7b9ee88e 100755 --- a/lib/main/MAVLink/common/mavlink_msg_rc_channels_raw.h +++ b/lib/main/MAVLink/common/mavlink_msg_rc_channels_raw.h @@ -15,7 +15,7 @@ typedef struct __mavlink_rc_channels_raw_t { uint16_t chan7_raw; /*< [us] RC channel 7 value.*/ uint16_t chan8_raw; /*< [us] RC channel 8 value.*/ uint8_t port; /*< Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk should use: 0 = MAIN, 1 = AUX.*/ - uint8_t rssi; /*< Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown.*/ + uint8_t rssi; /*< Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown.*/ } mavlink_rc_channels_raw_t; #define MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN 22 @@ -81,7 +81,7 @@ typedef struct __mavlink_rc_channels_raw_t { * @param chan6_raw [us] RC channel 6 value. * @param chan7_raw [us] RC channel 7 value. * @param chan8_raw [us] RC channel 8 value. - * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -123,6 +123,69 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_RAW_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); } +/** + * @brief Pack a rc_channels_raw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param port Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk should use: 0 = MAIN, 1 = AUX. + * @param chan1_raw [us] RC channel 1 value. + * @param chan2_raw [us] RC channel 2 value. + * @param chan3_raw [us] RC channel 3 value. + * @param chan4_raw [us] RC channel 4 value. + * @param chan5_raw [us] RC channel 5 value. + * @param chan6_raw [us] RC channel 6 value. + * @param chan7_raw [us] RC channel 7 value. + * @param chan8_raw [us] RC channel 8 value. + * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_raw_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t port, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#else + mavlink_rc_channels_raw_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.port = port; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RC_CHANNELS_RAW_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RC_CHANNELS_RAW_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#endif +} + /** * @brief Pack a rc_channels_raw message on a channel * @param system_id ID of this system @@ -139,7 +202,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8 * @param chan6_raw [us] RC channel 6 value. * @param chan7_raw [us] RC channel 7 value. * @param chan8_raw [us] RC channel 8 value. - * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rc_channels_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -209,6 +272,20 @@ static inline uint16_t mavlink_msg_rc_channels_raw_encode_chan(uint8_t system_id return mavlink_msg_rc_channels_raw_pack_chan(system_id, component_id, chan, msg, rc_channels_raw->time_boot_ms, rc_channels_raw->port, rc_channels_raw->chan1_raw, rc_channels_raw->chan2_raw, rc_channels_raw->chan3_raw, rc_channels_raw->chan4_raw, rc_channels_raw->chan5_raw, rc_channels_raw->chan6_raw, rc_channels_raw->chan7_raw, rc_channels_raw->chan8_raw, rc_channels_raw->rssi); } +/** + * @brief Encode a rc_channels_raw struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param rc_channels_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_raw_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_rc_channels_raw_t* rc_channels_raw) +{ + return mavlink_msg_rc_channels_raw_pack_status(system_id, component_id, _status, msg, rc_channels_raw->time_boot_ms, rc_channels_raw->port, rc_channels_raw->chan1_raw, rc_channels_raw->chan2_raw, rc_channels_raw->chan3_raw, rc_channels_raw->chan4_raw, rc_channels_raw->chan5_raw, rc_channels_raw->chan6_raw, rc_channels_raw->chan7_raw, rc_channels_raw->chan8_raw, rc_channels_raw->rssi); +} + /** * @brief Send a rc_channels_raw message * @param chan MAVLink channel to send the message @@ -223,7 +300,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_encode_chan(uint8_t system_id * @param chan6_raw [us] RC channel 6 value. * @param chan7_raw [us] RC channel 7 value. * @param chan8_raw [us] RC channel 8 value. - * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -278,7 +355,7 @@ static inline void mavlink_msg_rc_channels_raw_send_struct(mavlink_channel_t cha #if MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -428,7 +505,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan8_raw(const mavlink_m /** * @brief Get field rssi from rc_channels_raw message * - * @return Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @return Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. */ static inline uint8_t mavlink_msg_rc_channels_raw_get_rssi(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_rc_channels_scaled.h b/lib/main/MAVLink/common/mavlink_msg_rc_channels_scaled.h index 9d516d1ccc0..3a74b26ad31 100755 --- a/lib/main/MAVLink/common/mavlink_msg_rc_channels_scaled.h +++ b/lib/main/MAVLink/common/mavlink_msg_rc_channels_scaled.h @@ -15,7 +15,7 @@ typedef struct __mavlink_rc_channels_scaled_t { int16_t chan7_scaled; /*< RC channel 7 value scaled.*/ int16_t chan8_scaled; /*< RC channel 8 value scaled.*/ uint8_t port; /*< Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk should use: 0 = MAIN, 1 = AUX.*/ - uint8_t rssi; /*< Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown.*/ + uint8_t rssi; /*< Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown.*/ } mavlink_rc_channels_scaled_t; #define MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN 22 @@ -81,7 +81,7 @@ typedef struct __mavlink_rc_channels_scaled_t { * @param chan6_scaled RC channel 6 value scaled. * @param chan7_scaled RC channel 7 value scaled. * @param chan8_scaled RC channel 8 value scaled. - * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -123,6 +123,69 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, ui return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); } +/** + * @brief Pack a rc_channels_scaled message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param port Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk should use: 0 = MAIN, 1 = AUX. + * @param chan1_scaled RC channel 1 value scaled. + * @param chan2_scaled RC channel 2 value scaled. + * @param chan3_scaled RC channel 3 value scaled. + * @param chan4_scaled RC channel 4 value scaled. + * @param chan5_scaled RC channel 5 value scaled. + * @param chan6_scaled RC channel 6 value scaled. + * @param chan7_scaled RC channel 7 value scaled. + * @param chan8_scaled RC channel 8 value scaled. + * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_scaled_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t port, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, chan1_scaled); + _mav_put_int16_t(buf, 6, chan2_scaled); + _mav_put_int16_t(buf, 8, chan3_scaled); + _mav_put_int16_t(buf, 10, chan4_scaled); + _mav_put_int16_t(buf, 12, chan5_scaled); + _mav_put_int16_t(buf, 14, chan6_scaled); + _mav_put_int16_t(buf, 16, chan7_scaled); + _mav_put_int16_t(buf, 18, chan8_scaled); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#else + mavlink_rc_channels_scaled_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_scaled = chan1_scaled; + packet.chan2_scaled = chan2_scaled; + packet.chan3_scaled = chan3_scaled; + packet.chan4_scaled = chan4_scaled; + packet.chan5_scaled = chan5_scaled; + packet.chan6_scaled = chan6_scaled; + packet.chan7_scaled = chan7_scaled; + packet.chan8_scaled = chan8_scaled; + packet.port = port; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#endif +} + /** * @brief Pack a rc_channels_scaled message on a channel * @param system_id ID of this system @@ -139,7 +202,7 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, ui * @param chan6_scaled RC channel 6 value scaled. * @param chan7_scaled RC channel 7 value scaled. * @param chan8_scaled RC channel 8 value scaled. - * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rc_channels_scaled_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -209,6 +272,20 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_encode_chan(uint8_t system return mavlink_msg_rc_channels_scaled_pack_chan(system_id, component_id, chan, msg, rc_channels_scaled->time_boot_ms, rc_channels_scaled->port, rc_channels_scaled->chan1_scaled, rc_channels_scaled->chan2_scaled, rc_channels_scaled->chan3_scaled, rc_channels_scaled->chan4_scaled, rc_channels_scaled->chan5_scaled, rc_channels_scaled->chan6_scaled, rc_channels_scaled->chan7_scaled, rc_channels_scaled->chan8_scaled, rc_channels_scaled->rssi); } +/** + * @brief Encode a rc_channels_scaled struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param rc_channels_scaled C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_scaled_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_rc_channels_scaled_t* rc_channels_scaled) +{ + return mavlink_msg_rc_channels_scaled_pack_status(system_id, component_id, _status, msg, rc_channels_scaled->time_boot_ms, rc_channels_scaled->port, rc_channels_scaled->chan1_scaled, rc_channels_scaled->chan2_scaled, rc_channels_scaled->chan3_scaled, rc_channels_scaled->chan4_scaled, rc_channels_scaled->chan5_scaled, rc_channels_scaled->chan6_scaled, rc_channels_scaled->chan7_scaled, rc_channels_scaled->chan8_scaled, rc_channels_scaled->rssi); +} + /** * @brief Send a rc_channels_scaled message * @param chan MAVLink channel to send the message @@ -223,7 +300,7 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_encode_chan(uint8_t system * @param chan6_scaled RC channel 6 value scaled. * @param chan7_scaled RC channel 7 value scaled. * @param chan8_scaled RC channel 8 value scaled. - * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @param rssi Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -278,7 +355,7 @@ static inline void mavlink_msg_rc_channels_scaled_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -428,7 +505,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan8_scaled(const mavl /** * @brief Get field rssi from rc_channels_scaled message * - * @return Receive signal strength indicator in device-dependent units/scale. Values: [0-254], 255: invalid/unknown. + * @return Receive signal strength indicator in device-dependent units/scale. Values: [0-254], UINT8_MAX: invalid/unknown. */ static inline uint8_t mavlink_msg_rc_channels_scaled_get_rssi(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_request_data_stream.h b/lib/main/MAVLink/common/mavlink_msg_request_data_stream.h index f1d739588e8..f3a81d5cfe1 100755 --- a/lib/main/MAVLink/common/mavlink_msg_request_data_stream.h +++ b/lib/main/MAVLink/common/mavlink_msg_request_data_stream.h @@ -87,6 +87,51 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_MIN_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); } +/** + * @brief Pack a request_data_stream message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system The target requested to send the message stream. + * @param target_component The target requested to send the message stream. + * @param req_stream_id The ID of the requested data stream + * @param req_message_rate [Hz] The requested message rate + * @param start_stop 1 to start sending, 0 to stop sending. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_request_data_stream_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN]; + _mav_put_uint16_t(buf, 0, req_message_rate); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, req_stream_id); + _mav_put_uint8_t(buf, 5, start_stop); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#else + mavlink_request_data_stream_t packet; + packet.req_message_rate = req_message_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.req_stream_id = req_stream_id; + packet.start_stop = start_stop; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_MIN_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_MIN_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#endif +} + /** * @brief Pack a request_data_stream message on a channel * @param system_id ID of this system @@ -155,6 +200,20 @@ static inline uint16_t mavlink_msg_request_data_stream_encode_chan(uint8_t syste return mavlink_msg_request_data_stream_pack_chan(system_id, component_id, chan, msg, request_data_stream->target_system, request_data_stream->target_component, request_data_stream->req_stream_id, request_data_stream->req_message_rate, request_data_stream->start_stop); } +/** + * @brief Encode a request_data_stream struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param request_data_stream C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_request_data_stream_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_request_data_stream_t* request_data_stream) +{ + return mavlink_msg_request_data_stream_pack_status(system_id, component_id, _status, msg, request_data_stream->target_system, request_data_stream->target_component, request_data_stream->req_stream_id, request_data_stream->req_message_rate, request_data_stream->start_stop); +} + /** * @brief Send a request_data_stream message * @param chan MAVLink channel to send the message @@ -206,7 +265,7 @@ static inline void mavlink_msg_request_data_stream_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_request_event.h b/lib/main/MAVLink/common/mavlink_msg_request_event.h new file mode 100644 index 00000000000..6b596de9a1b --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_request_event.h @@ -0,0 +1,344 @@ +#pragma once +// MESSAGE REQUEST_EVENT PACKING + +#define MAVLINK_MSG_ID_REQUEST_EVENT 412 + + +typedef struct __mavlink_request_event_t { + uint16_t first_sequence; /*< First sequence number of the requested event.*/ + uint16_t last_sequence; /*< Last sequence number of the requested event.*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ +} mavlink_request_event_t; + +#define MAVLINK_MSG_ID_REQUEST_EVENT_LEN 6 +#define MAVLINK_MSG_ID_REQUEST_EVENT_MIN_LEN 6 +#define MAVLINK_MSG_ID_412_LEN 6 +#define MAVLINK_MSG_ID_412_MIN_LEN 6 + +#define MAVLINK_MSG_ID_REQUEST_EVENT_CRC 33 +#define MAVLINK_MSG_ID_412_CRC 33 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_REQUEST_EVENT { \ + 412, \ + "REQUEST_EVENT", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_request_event_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_request_event_t, target_component) }, \ + { "first_sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_request_event_t, first_sequence) }, \ + { "last_sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_request_event_t, last_sequence) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_REQUEST_EVENT { \ + "REQUEST_EVENT", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_request_event_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_request_event_t, target_component) }, \ + { "first_sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_request_event_t, first_sequence) }, \ + { "last_sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_request_event_t, last_sequence) }, \ + } \ +} +#endif + +/** + * @brief Pack a request_event message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param first_sequence First sequence number of the requested event. + * @param last_sequence Last sequence number of the requested event. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_request_event_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t first_sequence, uint16_t last_sequence) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_REQUEST_EVENT_LEN]; + _mav_put_uint16_t(buf, 0, first_sequence); + _mav_put_uint16_t(buf, 2, last_sequence); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_REQUEST_EVENT_LEN); +#else + mavlink_request_event_t packet; + packet.first_sequence = first_sequence; + packet.last_sequence = last_sequence; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_REQUEST_EVENT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_REQUEST_EVENT; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_REQUEST_EVENT_MIN_LEN, MAVLINK_MSG_ID_REQUEST_EVENT_LEN, MAVLINK_MSG_ID_REQUEST_EVENT_CRC); +} + +/** + * @brief Pack a request_event message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param first_sequence First sequence number of the requested event. + * @param last_sequence Last sequence number of the requested event. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_request_event_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t first_sequence, uint16_t last_sequence) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_REQUEST_EVENT_LEN]; + _mav_put_uint16_t(buf, 0, first_sequence); + _mav_put_uint16_t(buf, 2, last_sequence); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_REQUEST_EVENT_LEN); +#else + mavlink_request_event_t packet; + packet.first_sequence = first_sequence; + packet.last_sequence = last_sequence; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_REQUEST_EVENT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_REQUEST_EVENT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_REQUEST_EVENT_MIN_LEN, MAVLINK_MSG_ID_REQUEST_EVENT_LEN, MAVLINK_MSG_ID_REQUEST_EVENT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_REQUEST_EVENT_MIN_LEN, MAVLINK_MSG_ID_REQUEST_EVENT_LEN); +#endif +} + +/** + * @brief Pack a request_event message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param first_sequence First sequence number of the requested event. + * @param last_sequence Last sequence number of the requested event. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_request_event_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t first_sequence,uint16_t last_sequence) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_REQUEST_EVENT_LEN]; + _mav_put_uint16_t(buf, 0, first_sequence); + _mav_put_uint16_t(buf, 2, last_sequence); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_REQUEST_EVENT_LEN); +#else + mavlink_request_event_t packet; + packet.first_sequence = first_sequence; + packet.last_sequence = last_sequence; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_REQUEST_EVENT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_REQUEST_EVENT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_REQUEST_EVENT_MIN_LEN, MAVLINK_MSG_ID_REQUEST_EVENT_LEN, MAVLINK_MSG_ID_REQUEST_EVENT_CRC); +} + +/** + * @brief Encode a request_event struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param request_event C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_request_event_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_request_event_t* request_event) +{ + return mavlink_msg_request_event_pack(system_id, component_id, msg, request_event->target_system, request_event->target_component, request_event->first_sequence, request_event->last_sequence); +} + +/** + * @brief Encode a request_event struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param request_event C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_request_event_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_request_event_t* request_event) +{ + return mavlink_msg_request_event_pack_chan(system_id, component_id, chan, msg, request_event->target_system, request_event->target_component, request_event->first_sequence, request_event->last_sequence); +} + +/** + * @brief Encode a request_event struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param request_event C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_request_event_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_request_event_t* request_event) +{ + return mavlink_msg_request_event_pack_status(system_id, component_id, _status, msg, request_event->target_system, request_event->target_component, request_event->first_sequence, request_event->last_sequence); +} + +/** + * @brief Send a request_event message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param first_sequence First sequence number of the requested event. + * @param last_sequence Last sequence number of the requested event. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_request_event_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t first_sequence, uint16_t last_sequence) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_REQUEST_EVENT_LEN]; + _mav_put_uint16_t(buf, 0, first_sequence); + _mav_put_uint16_t(buf, 2, last_sequence); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_EVENT, buf, MAVLINK_MSG_ID_REQUEST_EVENT_MIN_LEN, MAVLINK_MSG_ID_REQUEST_EVENT_LEN, MAVLINK_MSG_ID_REQUEST_EVENT_CRC); +#else + mavlink_request_event_t packet; + packet.first_sequence = first_sequence; + packet.last_sequence = last_sequence; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_EVENT, (const char *)&packet, MAVLINK_MSG_ID_REQUEST_EVENT_MIN_LEN, MAVLINK_MSG_ID_REQUEST_EVENT_LEN, MAVLINK_MSG_ID_REQUEST_EVENT_CRC); +#endif +} + +/** + * @brief Send a request_event message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_request_event_send_struct(mavlink_channel_t chan, const mavlink_request_event_t* request_event) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_request_event_send(chan, request_event->target_system, request_event->target_component, request_event->first_sequence, request_event->last_sequence); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_EVENT, (const char *)request_event, MAVLINK_MSG_ID_REQUEST_EVENT_MIN_LEN, MAVLINK_MSG_ID_REQUEST_EVENT_LEN, MAVLINK_MSG_ID_REQUEST_EVENT_CRC); +#endif +} + +#if MAVLINK_MSG_ID_REQUEST_EVENT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_request_event_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t first_sequence, uint16_t last_sequence) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, first_sequence); + _mav_put_uint16_t(buf, 2, last_sequence); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_EVENT, buf, MAVLINK_MSG_ID_REQUEST_EVENT_MIN_LEN, MAVLINK_MSG_ID_REQUEST_EVENT_LEN, MAVLINK_MSG_ID_REQUEST_EVENT_CRC); +#else + mavlink_request_event_t *packet = (mavlink_request_event_t *)msgbuf; + packet->first_sequence = first_sequence; + packet->last_sequence = last_sequence; + packet->target_system = target_system; + packet->target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_EVENT, (const char *)packet, MAVLINK_MSG_ID_REQUEST_EVENT_MIN_LEN, MAVLINK_MSG_ID_REQUEST_EVENT_LEN, MAVLINK_MSG_ID_REQUEST_EVENT_CRC); +#endif +} +#endif + +#endif + +// MESSAGE REQUEST_EVENT UNPACKING + + +/** + * @brief Get field target_system from request_event message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_request_event_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from request_event message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_request_event_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field first_sequence from request_event message + * + * @return First sequence number of the requested event. + */ +static inline uint16_t mavlink_msg_request_event_get_first_sequence(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field last_sequence from request_event message + * + * @return Last sequence number of the requested event. + */ +static inline uint16_t mavlink_msg_request_event_get_last_sequence(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Decode a request_event message into a struct + * + * @param msg The message to decode + * @param request_event C-struct to decode the message contents into + */ +static inline void mavlink_msg_request_event_decode(const mavlink_message_t* msg, mavlink_request_event_t* request_event) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + request_event->first_sequence = mavlink_msg_request_event_get_first_sequence(msg); + request_event->last_sequence = mavlink_msg_request_event_get_last_sequence(msg); + request_event->target_system = mavlink_msg_request_event_get_target_system(msg); + request_event->target_component = mavlink_msg_request_event_get_target_component(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_REQUEST_EVENT_LEN? msg->len : MAVLINK_MSG_ID_REQUEST_EVENT_LEN; + memset(request_event, 0, MAVLINK_MSG_ID_REQUEST_EVENT_LEN); + memcpy(request_event, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_resource_request.h b/lib/main/MAVLink/common/mavlink_msg_resource_request.h index a8ee9a6398c..e68aadcec3e 100755 --- a/lib/main/MAVLink/common/mavlink_msg_resource_request.h +++ b/lib/main/MAVLink/common/mavlink_msg_resource_request.h @@ -5,7 +5,7 @@ typedef struct __mavlink_resource_request_t { - uint8_t request_id; /*< Request ID. This ID should be re-used when sending back URI contents*/ + uint8_t request_id; /*< Request ID. This ID should be reused when sending back URI contents*/ uint8_t uri_type; /*< The type of requested URI. 0 = a file via URL. 1 = a UAVCAN binary*/ uint8_t uri[120]; /*< The requested unique resource identifier (URI). It is not necessarily a straight domain name (depends on the URI type enum)*/ uint8_t transfer_type; /*< The way the autopilot wants to receive the URI. 0 = MAVLink FTP. 1 = binary stream.*/ @@ -54,7 +54,7 @@ typedef struct __mavlink_resource_request_t { * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param request_id Request ID. This ID should be re-used when sending back URI contents + * @param request_id Request ID. This ID should be reused when sending back URI contents * @param uri_type The type of requested URI. 0 = a file via URL. 1 = a UAVCAN binary * @param uri The requested unique resource identifier (URI). It is not necessarily a straight domain name (depends on the URI type enum) * @param transfer_type The way the autopilot wants to receive the URI. 0 = MAVLink FTP. 1 = binary stream. @@ -64,6 +64,45 @@ typedef struct __mavlink_resource_request_t { static inline uint16_t mavlink_msg_resource_request_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t request_id, uint8_t uri_type, const uint8_t *uri, uint8_t transfer_type, const uint8_t *storage) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RESOURCE_REQUEST_LEN]; + _mav_put_uint8_t(buf, 0, request_id); + _mav_put_uint8_t(buf, 1, uri_type); + _mav_put_uint8_t(buf, 122, transfer_type); + _mav_put_uint8_t_array(buf, 2, uri, 120); + _mav_put_uint8_t_array(buf, 123, storage, 120); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RESOURCE_REQUEST_LEN); +#else + mavlink_resource_request_t packet; + packet.request_id = request_id; + packet.uri_type = uri_type; + packet.transfer_type = transfer_type; + mav_array_assign_uint8_t(packet.uri, uri, 120); + mav_array_assign_uint8_t(packet.storage, storage, 120); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RESOURCE_REQUEST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RESOURCE_REQUEST; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RESOURCE_REQUEST_MIN_LEN, MAVLINK_MSG_ID_RESOURCE_REQUEST_LEN, MAVLINK_MSG_ID_RESOURCE_REQUEST_CRC); +} + +/** + * @brief Pack a resource_request message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param request_id Request ID. This ID should be reused when sending back URI contents + * @param uri_type The type of requested URI. 0 = a file via URL. 1 = a UAVCAN binary + * @param uri The requested unique resource identifier (URI). It is not necessarily a straight domain name (depends on the URI type enum) + * @param transfer_type The way the autopilot wants to receive the URI. 0 = MAVLink FTP. 1 = binary stream. + * @param storage The storage path the autopilot wants the URI to be stored in. Will only be valid if the transfer_type has a storage associated (e.g. MAVLink FTP). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_resource_request_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t request_id, uint8_t uri_type, const uint8_t *uri, uint8_t transfer_type, const uint8_t *storage) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_RESOURCE_REQUEST_LEN]; _mav_put_uint8_t(buf, 0, request_id); @@ -83,7 +122,11 @@ static inline uint16_t mavlink_msg_resource_request_pack(uint8_t system_id, uint #endif msg->msgid = MAVLINK_MSG_ID_RESOURCE_REQUEST; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RESOURCE_REQUEST_MIN_LEN, MAVLINK_MSG_ID_RESOURCE_REQUEST_LEN, MAVLINK_MSG_ID_RESOURCE_REQUEST_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RESOURCE_REQUEST_MIN_LEN, MAVLINK_MSG_ID_RESOURCE_REQUEST_LEN, MAVLINK_MSG_ID_RESOURCE_REQUEST_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RESOURCE_REQUEST_MIN_LEN, MAVLINK_MSG_ID_RESOURCE_REQUEST_LEN); +#endif } /** @@ -92,7 +135,7 @@ static inline uint16_t mavlink_msg_resource_request_pack(uint8_t system_id, uint * @param component_id ID of this component (e.g. 200 for IMU) * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into - * @param request_id Request ID. This ID should be re-used when sending back URI contents + * @param request_id Request ID. This ID should be reused when sending back URI contents * @param uri_type The type of requested URI. 0 = a file via URL. 1 = a UAVCAN binary * @param uri The requested unique resource identifier (URI). It is not necessarily a straight domain name (depends on the URI type enum) * @param transfer_type The way the autopilot wants to receive the URI. 0 = MAVLink FTP. 1 = binary stream. @@ -116,8 +159,8 @@ static inline uint16_t mavlink_msg_resource_request_pack_chan(uint8_t system_id, packet.request_id = request_id; packet.uri_type = uri_type; packet.transfer_type = transfer_type; - mav_array_memcpy(packet.uri, uri, sizeof(uint8_t)*120); - mav_array_memcpy(packet.storage, storage, sizeof(uint8_t)*120); + mav_array_assign_uint8_t(packet.uri, uri, 120); + mav_array_assign_uint8_t(packet.storage, storage, 120); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RESOURCE_REQUEST_LEN); #endif @@ -152,11 +195,25 @@ static inline uint16_t mavlink_msg_resource_request_encode_chan(uint8_t system_i return mavlink_msg_resource_request_pack_chan(system_id, component_id, chan, msg, resource_request->request_id, resource_request->uri_type, resource_request->uri, resource_request->transfer_type, resource_request->storage); } +/** + * @brief Encode a resource_request struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param resource_request C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_resource_request_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_resource_request_t* resource_request) +{ + return mavlink_msg_resource_request_pack_status(system_id, component_id, _status, msg, resource_request->request_id, resource_request->uri_type, resource_request->uri, resource_request->transfer_type, resource_request->storage); +} + /** * @brief Send a resource_request message * @param chan MAVLink channel to send the message * - * @param request_id Request ID. This ID should be re-used when sending back URI contents + * @param request_id Request ID. This ID should be reused when sending back URI contents * @param uri_type The type of requested URI. 0 = a file via URL. 1 = a UAVCAN binary * @param uri The requested unique resource identifier (URI). It is not necessarily a straight domain name (depends on the URI type enum) * @param transfer_type The way the autopilot wants to receive the URI. 0 = MAVLink FTP. 1 = binary stream. @@ -179,8 +236,8 @@ static inline void mavlink_msg_resource_request_send(mavlink_channel_t chan, uin packet.request_id = request_id; packet.uri_type = uri_type; packet.transfer_type = transfer_type; - mav_array_memcpy(packet.uri, uri, sizeof(uint8_t)*120); - mav_array_memcpy(packet.storage, storage, sizeof(uint8_t)*120); + mav_array_assign_uint8_t(packet.uri, uri, 120); + mav_array_assign_uint8_t(packet.storage, storage, 120); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RESOURCE_REQUEST, (const char *)&packet, MAVLINK_MSG_ID_RESOURCE_REQUEST_MIN_LEN, MAVLINK_MSG_ID_RESOURCE_REQUEST_LEN, MAVLINK_MSG_ID_RESOURCE_REQUEST_CRC); #endif } @@ -201,7 +258,7 @@ static inline void mavlink_msg_resource_request_send_struct(mavlink_channel_t ch #if MAVLINK_MSG_ID_RESOURCE_REQUEST_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -222,8 +279,8 @@ static inline void mavlink_msg_resource_request_send_buf(mavlink_message_t *msgb packet->request_id = request_id; packet->uri_type = uri_type; packet->transfer_type = transfer_type; - mav_array_memcpy(packet->uri, uri, sizeof(uint8_t)*120); - mav_array_memcpy(packet->storage, storage, sizeof(uint8_t)*120); + mav_array_assign_uint8_t(packet->uri, uri, 120); + mav_array_assign_uint8_t(packet->storage, storage, 120); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RESOURCE_REQUEST, (const char *)packet, MAVLINK_MSG_ID_RESOURCE_REQUEST_MIN_LEN, MAVLINK_MSG_ID_RESOURCE_REQUEST_LEN, MAVLINK_MSG_ID_RESOURCE_REQUEST_CRC); #endif } @@ -237,7 +294,7 @@ static inline void mavlink_msg_resource_request_send_buf(mavlink_message_t *msgb /** * @brief Get field request_id from resource_request message * - * @return Request ID. This ID should be re-used when sending back URI contents + * @return Request ID. This ID should be reused when sending back URI contents */ static inline uint8_t mavlink_msg_resource_request_get_request_id(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_response_event_error.h b/lib/main/MAVLink/common/mavlink_msg_response_event_error.h new file mode 100644 index 00000000000..3182cea2e2f --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_response_event_error.h @@ -0,0 +1,372 @@ +#pragma once +// MESSAGE RESPONSE_EVENT_ERROR PACKING + +#define MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR 413 + + +typedef struct __mavlink_response_event_error_t { + uint16_t sequence; /*< Sequence number.*/ + uint16_t sequence_oldest_available; /*< Oldest Sequence number that is still available after the sequence set in REQUEST_EVENT.*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + uint8_t reason; /*< Error reason.*/ +} mavlink_response_event_error_t; + +#define MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN 7 +#define MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_MIN_LEN 7 +#define MAVLINK_MSG_ID_413_LEN 7 +#define MAVLINK_MSG_ID_413_MIN_LEN 7 + +#define MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_CRC 77 +#define MAVLINK_MSG_ID_413_CRC 77 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_RESPONSE_EVENT_ERROR { \ + 413, \ + "RESPONSE_EVENT_ERROR", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_response_event_error_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_response_event_error_t, target_component) }, \ + { "sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_response_event_error_t, sequence) }, \ + { "sequence_oldest_available", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_response_event_error_t, sequence_oldest_available) }, \ + { "reason", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_response_event_error_t, reason) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_RESPONSE_EVENT_ERROR { \ + "RESPONSE_EVENT_ERROR", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_response_event_error_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_response_event_error_t, target_component) }, \ + { "sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_response_event_error_t, sequence) }, \ + { "sequence_oldest_available", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_response_event_error_t, sequence_oldest_available) }, \ + { "reason", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_response_event_error_t, reason) }, \ + } \ +} +#endif + +/** + * @brief Pack a response_event_error message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param sequence Sequence number. + * @param sequence_oldest_available Oldest Sequence number that is still available after the sequence set in REQUEST_EVENT. + * @param reason Error reason. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_response_event_error_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t sequence, uint16_t sequence_oldest_available, uint8_t reason) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint16_t(buf, 2, sequence_oldest_available); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, reason); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN); +#else + mavlink_response_event_error_t packet; + packet.sequence = sequence; + packet.sequence_oldest_available = sequence_oldest_available; + packet.target_system = target_system; + packet.target_component = target_component; + packet.reason = reason; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_MIN_LEN, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_CRC); +} + +/** + * @brief Pack a response_event_error message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param sequence Sequence number. + * @param sequence_oldest_available Oldest Sequence number that is still available after the sequence set in REQUEST_EVENT. + * @param reason Error reason. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_response_event_error_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t sequence, uint16_t sequence_oldest_available, uint8_t reason) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint16_t(buf, 2, sequence_oldest_available); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, reason); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN); +#else + mavlink_response_event_error_t packet; + packet.sequence = sequence; + packet.sequence_oldest_available = sequence_oldest_available; + packet.target_system = target_system; + packet.target_component = target_component; + packet.reason = reason; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_MIN_LEN, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_MIN_LEN, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN); +#endif +} + +/** + * @brief Pack a response_event_error message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param sequence Sequence number. + * @param sequence_oldest_available Oldest Sequence number that is still available after the sequence set in REQUEST_EVENT. + * @param reason Error reason. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_response_event_error_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t sequence,uint16_t sequence_oldest_available,uint8_t reason) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint16_t(buf, 2, sequence_oldest_available); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, reason); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN); +#else + mavlink_response_event_error_t packet; + packet.sequence = sequence; + packet.sequence_oldest_available = sequence_oldest_available; + packet.target_system = target_system; + packet.target_component = target_component; + packet.reason = reason; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_MIN_LEN, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_CRC); +} + +/** + * @brief Encode a response_event_error struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param response_event_error C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_response_event_error_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_response_event_error_t* response_event_error) +{ + return mavlink_msg_response_event_error_pack(system_id, component_id, msg, response_event_error->target_system, response_event_error->target_component, response_event_error->sequence, response_event_error->sequence_oldest_available, response_event_error->reason); +} + +/** + * @brief Encode a response_event_error struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param response_event_error C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_response_event_error_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_response_event_error_t* response_event_error) +{ + return mavlink_msg_response_event_error_pack_chan(system_id, component_id, chan, msg, response_event_error->target_system, response_event_error->target_component, response_event_error->sequence, response_event_error->sequence_oldest_available, response_event_error->reason); +} + +/** + * @brief Encode a response_event_error struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param response_event_error C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_response_event_error_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_response_event_error_t* response_event_error) +{ + return mavlink_msg_response_event_error_pack_status(system_id, component_id, _status, msg, response_event_error->target_system, response_event_error->target_component, response_event_error->sequence, response_event_error->sequence_oldest_available, response_event_error->reason); +} + +/** + * @brief Send a response_event_error message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param sequence Sequence number. + * @param sequence_oldest_available Oldest Sequence number that is still available after the sequence set in REQUEST_EVENT. + * @param reason Error reason. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_response_event_error_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint16_t sequence_oldest_available, uint8_t reason) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint16_t(buf, 2, sequence_oldest_available); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, reason); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR, buf, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_MIN_LEN, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_CRC); +#else + mavlink_response_event_error_t packet; + packet.sequence = sequence; + packet.sequence_oldest_available = sequence_oldest_available; + packet.target_system = target_system; + packet.target_component = target_component; + packet.reason = reason; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR, (const char *)&packet, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_MIN_LEN, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_CRC); +#endif +} + +/** + * @brief Send a response_event_error message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_response_event_error_send_struct(mavlink_channel_t chan, const mavlink_response_event_error_t* response_event_error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_response_event_error_send(chan, response_event_error->target_system, response_event_error->target_component, response_event_error->sequence, response_event_error->sequence_oldest_available, response_event_error->reason); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR, (const char *)response_event_error, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_MIN_LEN, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_CRC); +#endif +} + +#if MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This variant of _send() can be used to save stack space by reusing + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_response_event_error_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint16_t sequence_oldest_available, uint8_t reason) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint16_t(buf, 2, sequence_oldest_available); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, reason); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR, buf, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_MIN_LEN, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_CRC); +#else + mavlink_response_event_error_t *packet = (mavlink_response_event_error_t *)msgbuf; + packet->sequence = sequence; + packet->sequence_oldest_available = sequence_oldest_available; + packet->target_system = target_system; + packet->target_component = target_component; + packet->reason = reason; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR, (const char *)packet, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_MIN_LEN, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_CRC); +#endif +} +#endif + +#endif + +// MESSAGE RESPONSE_EVENT_ERROR UNPACKING + + +/** + * @brief Get field target_system from response_event_error message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_response_event_error_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from response_event_error message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_response_event_error_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field sequence from response_event_error message + * + * @return Sequence number. + */ +static inline uint16_t mavlink_msg_response_event_error_get_sequence(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field sequence_oldest_available from response_event_error message + * + * @return Oldest Sequence number that is still available after the sequence set in REQUEST_EVENT. + */ +static inline uint16_t mavlink_msg_response_event_error_get_sequence_oldest_available(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field reason from response_event_error message + * + * @return Error reason. + */ +static inline uint8_t mavlink_msg_response_event_error_get_reason(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Decode a response_event_error message into a struct + * + * @param msg The message to decode + * @param response_event_error C-struct to decode the message contents into + */ +static inline void mavlink_msg_response_event_error_decode(const mavlink_message_t* msg, mavlink_response_event_error_t* response_event_error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + response_event_error->sequence = mavlink_msg_response_event_error_get_sequence(msg); + response_event_error->sequence_oldest_available = mavlink_msg_response_event_error_get_sequence_oldest_available(msg); + response_event_error->target_system = mavlink_msg_response_event_error_get_target_system(msg); + response_event_error->target_component = mavlink_msg_response_event_error_get_target_component(msg); + response_event_error->reason = mavlink_msg_response_event_error_get_reason(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN? msg->len : MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN; + memset(response_event_error, 0, MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_LEN); + memcpy(response_event_error, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_safety_allowed_area.h b/lib/main/MAVLink/common/mavlink_msg_safety_allowed_area.h index bad669a0cb3..dcbfffebaa2 100755 --- a/lib/main/MAVLink/common/mavlink_msg_safety_allowed_area.h +++ b/lib/main/MAVLink/common/mavlink_msg_safety_allowed_area.h @@ -99,6 +99,57 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, u return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_MIN_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); } +/** + * @brief Pack a safety_allowed_area message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param frame Coordinate frame. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x [m] x position 1 / Latitude 1 + * @param p1y [m] y position 1 / Longitude 1 + * @param p1z [m] z position 1 / Altitude 1 + * @param p2x [m] x position 2 / Latitude 2 + * @param p2y [m] y position 2 / Longitude 2 + * @param p2z [m] z position 2 / Altitude 2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_safety_allowed_area_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN]; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#else + mavlink_safety_allowed_area_t packet; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + packet.frame = frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_MIN_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_MIN_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#endif +} + /** * @brief Pack a safety_allowed_area message on a channel * @param system_id ID of this system @@ -173,6 +224,20 @@ static inline uint16_t mavlink_msg_safety_allowed_area_encode_chan(uint8_t syste return mavlink_msg_safety_allowed_area_pack_chan(system_id, component_id, chan, msg, safety_allowed_area->frame, safety_allowed_area->p1x, safety_allowed_area->p1y, safety_allowed_area->p1z, safety_allowed_area->p2x, safety_allowed_area->p2y, safety_allowed_area->p2z); } +/** + * @brief Encode a safety_allowed_area struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param safety_allowed_area C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_safety_allowed_area_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_safety_allowed_area_t* safety_allowed_area) +{ + return mavlink_msg_safety_allowed_area_pack_status(system_id, component_id, _status, msg, safety_allowed_area->frame, safety_allowed_area->p1x, safety_allowed_area->p1y, safety_allowed_area->p1z, safety_allowed_area->p2x, safety_allowed_area->p2y, safety_allowed_area->p2z); +} + /** * @brief Send a safety_allowed_area message * @param chan MAVLink channel to send the message @@ -230,7 +295,7 @@ static inline void mavlink_msg_safety_allowed_area_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_safety_set_allowed_area.h b/lib/main/MAVLink/common/mavlink_msg_safety_set_allowed_area.h index eca9b274f70..c2deea3f25d 100755 --- a/lib/main/MAVLink/common/mavlink_msg_safety_set_allowed_area.h +++ b/lib/main/MAVLink/common/mavlink_msg_safety_set_allowed_area.h @@ -111,6 +111,63 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_i return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_MIN_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); } +/** + * @brief Pack a safety_set_allowed_area message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param frame Coordinate frame. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x [m] x position 1 / Latitude 1 + * @param p1y [m] y position 1 / Longitude 1 + * @param p1z [m] z position 1 / Altitude 1 + * @param p2x [m] x position 2 / Latitude 2 + * @param p2y [m] y position 2 / Longitude 2 + * @param p2z [m] z position 2 / Altitude 2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_safety_set_allowed_area_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN]; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, target_system); + _mav_put_uint8_t(buf, 25, target_component); + _mav_put_uint8_t(buf, 26, frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#else + mavlink_safety_set_allowed_area_t packet; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_MIN_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_MIN_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#endif +} + /** * @brief Pack a safety_set_allowed_area message on a channel * @param system_id ID of this system @@ -191,6 +248,20 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_encode_chan(uint8_t s return mavlink_msg_safety_set_allowed_area_pack_chan(system_id, component_id, chan, msg, safety_set_allowed_area->target_system, safety_set_allowed_area->target_component, safety_set_allowed_area->frame, safety_set_allowed_area->p1x, safety_set_allowed_area->p1y, safety_set_allowed_area->p1z, safety_set_allowed_area->p2x, safety_set_allowed_area->p2y, safety_set_allowed_area->p2z); } +/** + * @brief Encode a safety_set_allowed_area struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param safety_set_allowed_area C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_safety_set_allowed_area_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_safety_set_allowed_area_t* safety_set_allowed_area) +{ + return mavlink_msg_safety_set_allowed_area_pack_status(system_id, component_id, _status, msg, safety_set_allowed_area->target_system, safety_set_allowed_area->target_component, safety_set_allowed_area->frame, safety_set_allowed_area->p1x, safety_set_allowed_area->p1y, safety_set_allowed_area->p1z, safety_set_allowed_area->p2x, safety_set_allowed_area->p2y, safety_set_allowed_area->p2z); +} + /** * @brief Send a safety_set_allowed_area message * @param chan MAVLink channel to send the message @@ -254,7 +325,7 @@ static inline void mavlink_msg_safety_set_allowed_area_send_struct(mavlink_chann #if MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_scaled_imu.h b/lib/main/MAVLink/common/mavlink_msg_scaled_imu.h index 14b1257ce90..cebcefcd066 100755 --- a/lib/main/MAVLink/common/mavlink_msg_scaled_imu.h +++ b/lib/main/MAVLink/common/mavlink_msg_scaled_imu.h @@ -123,6 +123,69 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); } +/** + * @brief Pack a scaled_imu message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param xacc [mG] X acceleration + * @param yacc [mG] Y acceleration + * @param zacc [mG] Z acceleration + * @param xgyro [mrad/s] Angular speed around X axis + * @param ygyro [mrad/s] Angular speed around Y axis + * @param zgyro [mrad/s] Angular speed around Z axis + * @param xmag [mgauss] X Magnetic field + * @param ymag [mgauss] Y Magnetic field + * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_imu_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_IMU_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#else + mavlink_scaled_imu_t packet; + packet.time_boot_ms = time_boot_ms; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SCALED_IMU_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SCALED_IMU_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#endif +} + /** * @brief Pack a scaled_imu message on a channel * @param system_id ID of this system @@ -209,6 +272,20 @@ static inline uint16_t mavlink_msg_scaled_imu_encode_chan(uint8_t system_id, uin return mavlink_msg_scaled_imu_pack_chan(system_id, component_id, chan, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag, scaled_imu->temperature); } +/** + * @brief Encode a scaled_imu struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param scaled_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_imu_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_scaled_imu_t* scaled_imu) +{ + return mavlink_msg_scaled_imu_pack_status(system_id, component_id, _status, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag, scaled_imu->temperature); +} + /** * @brief Send a scaled_imu message * @param chan MAVLink channel to send the message @@ -278,7 +355,7 @@ static inline void mavlink_msg_scaled_imu_send_struct(mavlink_channel_t chan, co #if MAVLINK_MSG_ID_SCALED_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_scaled_imu2.h b/lib/main/MAVLink/common/mavlink_msg_scaled_imu2.h index 2106cefd1ea..287d045f00b 100755 --- a/lib/main/MAVLink/common/mavlink_msg_scaled_imu2.h +++ b/lib/main/MAVLink/common/mavlink_msg_scaled_imu2.h @@ -123,6 +123,69 @@ static inline uint16_t mavlink_msg_scaled_imu2_pack(uint8_t system_id, uint8_t c return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU2_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); } +/** + * @brief Pack a scaled_imu2 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param xacc [mG] X acceleration + * @param yacc [mG] Y acceleration + * @param zacc [mG] Z acceleration + * @param xgyro [mrad/s] Angular speed around X axis + * @param ygyro [mrad/s] Angular speed around Y axis + * @param zgyro [mrad/s] Angular speed around Z axis + * @param xmag [mgauss] X Magnetic field + * @param ymag [mgauss] Y Magnetic field + * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_imu2_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#else + mavlink_scaled_imu2_t packet; + packet.time_boot_ms = time_boot_ms; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU2; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SCALED_IMU2_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SCALED_IMU2_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +} + /** * @brief Pack a scaled_imu2 message on a channel * @param system_id ID of this system @@ -209,6 +272,20 @@ static inline uint16_t mavlink_msg_scaled_imu2_encode_chan(uint8_t system_id, ui return mavlink_msg_scaled_imu2_pack_chan(system_id, component_id, chan, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag, scaled_imu2->temperature); } +/** + * @brief Encode a scaled_imu2 struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param scaled_imu2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_imu2_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_scaled_imu2_t* scaled_imu2) +{ + return mavlink_msg_scaled_imu2_pack_status(system_id, component_id, _status, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag, scaled_imu2->temperature); +} + /** * @brief Send a scaled_imu2 message * @param chan MAVLink channel to send the message @@ -278,7 +355,7 @@ static inline void mavlink_msg_scaled_imu2_send_struct(mavlink_channel_t chan, c #if MAVLINK_MSG_ID_SCALED_IMU2_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_scaled_imu3.h b/lib/main/MAVLink/common/mavlink_msg_scaled_imu3.h index cd20f235669..5fa8ecd5790 100755 --- a/lib/main/MAVLink/common/mavlink_msg_scaled_imu3.h +++ b/lib/main/MAVLink/common/mavlink_msg_scaled_imu3.h @@ -123,6 +123,69 @@ static inline uint16_t mavlink_msg_scaled_imu3_pack(uint8_t system_id, uint8_t c return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU3_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU3_LEN, MAVLINK_MSG_ID_SCALED_IMU3_CRC); } +/** + * @brief Pack a scaled_imu3 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param xacc [mG] X acceleration + * @param yacc [mG] Y acceleration + * @param zacc [mG] Z acceleration + * @param xgyro [mrad/s] Angular speed around X axis + * @param ygyro [mrad/s] Angular speed around Y axis + * @param zgyro [mrad/s] Angular speed around Z axis + * @param xmag [mgauss] X Magnetic field + * @param ymag [mgauss] Y Magnetic field + * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_imu3_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_IMU3_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU3_LEN); +#else + mavlink_scaled_imu3_t packet; + packet.time_boot_ms = time_boot_ms; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU3_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU3; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SCALED_IMU3_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU3_LEN, MAVLINK_MSG_ID_SCALED_IMU3_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SCALED_IMU3_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU3_LEN); +#endif +} + /** * @brief Pack a scaled_imu3 message on a channel * @param system_id ID of this system @@ -209,6 +272,20 @@ static inline uint16_t mavlink_msg_scaled_imu3_encode_chan(uint8_t system_id, ui return mavlink_msg_scaled_imu3_pack_chan(system_id, component_id, chan, msg, scaled_imu3->time_boot_ms, scaled_imu3->xacc, scaled_imu3->yacc, scaled_imu3->zacc, scaled_imu3->xgyro, scaled_imu3->ygyro, scaled_imu3->zgyro, scaled_imu3->xmag, scaled_imu3->ymag, scaled_imu3->zmag, scaled_imu3->temperature); } +/** + * @brief Encode a scaled_imu3 struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param scaled_imu3 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_imu3_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_scaled_imu3_t* scaled_imu3) +{ + return mavlink_msg_scaled_imu3_pack_status(system_id, component_id, _status, msg, scaled_imu3->time_boot_ms, scaled_imu3->xacc, scaled_imu3->yacc, scaled_imu3->zacc, scaled_imu3->xgyro, scaled_imu3->ygyro, scaled_imu3->zgyro, scaled_imu3->xmag, scaled_imu3->ymag, scaled_imu3->zmag, scaled_imu3->temperature); +} + /** * @brief Send a scaled_imu3 message * @param chan MAVLink channel to send the message @@ -278,7 +355,7 @@ static inline void mavlink_msg_scaled_imu3_send_struct(mavlink_channel_t chan, c #if MAVLINK_MSG_ID_SCALED_IMU3_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_scaled_pressure.h b/lib/main/MAVLink/common/mavlink_msg_scaled_pressure.h index 13344a8a681..06c0ce62de4 100755 --- a/lib/main/MAVLink/common/mavlink_msg_scaled_pressure.h +++ b/lib/main/MAVLink/common/mavlink_msg_scaled_pressure.h @@ -87,6 +87,51 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); } +/** + * @brief Pack a scaled_pressure message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param press_abs [hPa] Absolute pressure + * @param press_diff [hPa] Differential pressure 1 + * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_pressure_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_PRESSURE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, press_abs); + _mav_put_float(buf, 8, press_diff); + _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#else + mavlink_scaled_pressure_t packet; + packet.time_boot_ms = time_boot_ms; + packet.press_abs = press_abs; + packet.press_diff = press_diff; + packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SCALED_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SCALED_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#endif +} + /** * @brief Pack a scaled_pressure message on a channel * @param system_id ID of this system @@ -155,6 +200,20 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode_chan(uint8_t system_id return mavlink_msg_scaled_pressure_pack_chan(system_id, component_id, chan, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature, scaled_pressure->temperature_press_diff); } +/** + * @brief Encode a scaled_pressure struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param scaled_pressure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_pressure_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_scaled_pressure_t* scaled_pressure) +{ + return mavlink_msg_scaled_pressure_pack_status(system_id, component_id, _status, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature, scaled_pressure->temperature_press_diff); +} + /** * @brief Send a scaled_pressure message * @param chan MAVLink channel to send the message @@ -206,7 +265,7 @@ static inline void mavlink_msg_scaled_pressure_send_struct(mavlink_channel_t cha #if MAVLINK_MSG_ID_SCALED_PRESSURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_scaled_pressure2.h b/lib/main/MAVLink/common/mavlink_msg_scaled_pressure2.h index 8dc28850e8a..6203bbc3f09 100755 --- a/lib/main/MAVLink/common/mavlink_msg_scaled_pressure2.h +++ b/lib/main/MAVLink/common/mavlink_msg_scaled_pressure2.h @@ -87,6 +87,51 @@ static inline uint16_t mavlink_msg_scaled_pressure2_pack(uint8_t system_id, uint return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_PRESSURE2_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC); } +/** + * @brief Pack a scaled_pressure2 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param press_abs [hPa] Absolute pressure + * @param press_diff [hPa] Differential pressure + * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_pressure2_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, press_abs); + _mav_put_float(buf, 8, press_diff); + _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); +#else + mavlink_scaled_pressure2_t packet; + packet.time_boot_ms = time_boot_ms; + packet.press_abs = press_abs; + packet.press_diff = press_diff; + packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE2; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SCALED_PRESSURE2_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SCALED_PRESSURE2_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); +#endif +} + /** * @brief Pack a scaled_pressure2 message on a channel * @param system_id ID of this system @@ -155,6 +200,20 @@ static inline uint16_t mavlink_msg_scaled_pressure2_encode_chan(uint8_t system_i return mavlink_msg_scaled_pressure2_pack_chan(system_id, component_id, chan, msg, scaled_pressure2->time_boot_ms, scaled_pressure2->press_abs, scaled_pressure2->press_diff, scaled_pressure2->temperature, scaled_pressure2->temperature_press_diff); } +/** + * @brief Encode a scaled_pressure2 struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param scaled_pressure2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_pressure2_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_scaled_pressure2_t* scaled_pressure2) +{ + return mavlink_msg_scaled_pressure2_pack_status(system_id, component_id, _status, msg, scaled_pressure2->time_boot_ms, scaled_pressure2->press_abs, scaled_pressure2->press_diff, scaled_pressure2->temperature, scaled_pressure2->temperature_press_diff); +} + /** * @brief Send a scaled_pressure2 message * @param chan MAVLink channel to send the message @@ -206,7 +265,7 @@ static inline void mavlink_msg_scaled_pressure2_send_struct(mavlink_channel_t ch #if MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_scaled_pressure3.h b/lib/main/MAVLink/common/mavlink_msg_scaled_pressure3.h index 34368b1efbf..660ca4fae09 100755 --- a/lib/main/MAVLink/common/mavlink_msg_scaled_pressure3.h +++ b/lib/main/MAVLink/common/mavlink_msg_scaled_pressure3.h @@ -87,6 +87,51 @@ static inline uint16_t mavlink_msg_scaled_pressure3_pack(uint8_t system_id, uint return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_PRESSURE3_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_CRC); } +/** + * @brief Pack a scaled_pressure3 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param press_abs [hPa] Absolute pressure + * @param press_diff [hPa] Differential pressure + * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_pressure3_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, press_abs); + _mav_put_float(buf, 8, press_diff); + _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN); +#else + mavlink_scaled_pressure3_t packet; + packet.time_boot_ms = time_boot_ms; + packet.press_abs = press_abs; + packet.press_diff = press_diff; + packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE3; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SCALED_PRESSURE3_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SCALED_PRESSURE3_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN); +#endif +} + /** * @brief Pack a scaled_pressure3 message on a channel * @param system_id ID of this system @@ -155,6 +200,20 @@ static inline uint16_t mavlink_msg_scaled_pressure3_encode_chan(uint8_t system_i return mavlink_msg_scaled_pressure3_pack_chan(system_id, component_id, chan, msg, scaled_pressure3->time_boot_ms, scaled_pressure3->press_abs, scaled_pressure3->press_diff, scaled_pressure3->temperature, scaled_pressure3->temperature_press_diff); } +/** + * @brief Encode a scaled_pressure3 struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param scaled_pressure3 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_pressure3_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_scaled_pressure3_t* scaled_pressure3) +{ + return mavlink_msg_scaled_pressure3_pack_status(system_id, component_id, _status, msg, scaled_pressure3->time_boot_ms, scaled_pressure3->press_abs, scaled_pressure3->press_diff, scaled_pressure3->temperature, scaled_pressure3->temperature_press_diff); +} + /** * @brief Send a scaled_pressure3 message * @param chan MAVLink channel to send the message @@ -206,7 +265,7 @@ static inline void mavlink_msg_scaled_pressure3_send_struct(mavlink_channel_t ch #if MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_serial_control.h b/lib/main/MAVLink/common/mavlink_msg_serial_control.h index 16ed754cf61..a411f26f971 100755 --- a/lib/main/MAVLink/common/mavlink_msg_serial_control.h +++ b/lib/main/MAVLink/common/mavlink_msg_serial_control.h @@ -11,11 +11,13 @@ typedef struct __mavlink_serial_control_t { uint8_t flags; /*< Bitmap of serial control flags.*/ uint8_t count; /*< [bytes] how many bytes in this transfer*/ uint8_t data[70]; /*< serial data*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ } mavlink_serial_control_t; -#define MAVLINK_MSG_ID_SERIAL_CONTROL_LEN 79 +#define MAVLINK_MSG_ID_SERIAL_CONTROL_LEN 81 #define MAVLINK_MSG_ID_SERIAL_CONTROL_MIN_LEN 79 -#define MAVLINK_MSG_ID_126_LEN 79 +#define MAVLINK_MSG_ID_126_LEN 81 #define MAVLINK_MSG_ID_126_MIN_LEN 79 #define MAVLINK_MSG_ID_SERIAL_CONTROL_CRC 220 @@ -27,25 +29,29 @@ typedef struct __mavlink_serial_control_t { #define MAVLINK_MESSAGE_INFO_SERIAL_CONTROL { \ 126, \ "SERIAL_CONTROL", \ - 6, \ + 8, \ { { "device", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_serial_control_t, device) }, \ { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_serial_control_t, flags) }, \ { "timeout", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_serial_control_t, timeout) }, \ { "baudrate", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_serial_control_t, baudrate) }, \ { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_serial_control_t, count) }, \ { "data", NULL, MAVLINK_TYPE_UINT8_T, 70, 9, offsetof(mavlink_serial_control_t, data) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 79, offsetof(mavlink_serial_control_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 80, offsetof(mavlink_serial_control_t, target_component) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SERIAL_CONTROL { \ "SERIAL_CONTROL", \ - 6, \ + 8, \ { { "device", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_serial_control_t, device) }, \ { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_serial_control_t, flags) }, \ { "timeout", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_serial_control_t, timeout) }, \ { "baudrate", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_serial_control_t, baudrate) }, \ { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_serial_control_t, count) }, \ { "data", NULL, MAVLINK_TYPE_UINT8_T, 70, 9, offsetof(mavlink_serial_control_t, data) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 79, offsetof(mavlink_serial_control_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 80, offsetof(mavlink_serial_control_t, target_component) }, \ } \ } #endif @@ -62,10 +68,12 @@ typedef struct __mavlink_serial_control_t { * @param baudrate [bits/s] Baudrate of transfer. Zero means no change. * @param count [bytes] how many bytes in this transfer * @param data serial data + * @param target_system System ID + * @param target_component Component ID * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_serial_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data) + uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data, uint8_t target_system, uint8_t target_component) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; @@ -74,6 +82,8 @@ static inline uint16_t mavlink_msg_serial_control_pack(uint8_t system_id, uint8_ _mav_put_uint8_t(buf, 6, device); _mav_put_uint8_t(buf, 7, flags); _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t(buf, 79, target_system); + _mav_put_uint8_t(buf, 80, target_component); _mav_put_uint8_t_array(buf, 9, data, 70); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); #else @@ -83,7 +93,9 @@ static inline uint16_t mavlink_msg_serial_control_pack(uint8_t system_id, uint8_ packet.device = device; packet.flags = flags; packet.count = count; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_assign_uint8_t(packet.data, data, 70); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); #endif @@ -91,6 +103,58 @@ static inline uint16_t mavlink_msg_serial_control_pack(uint8_t system_id, uint8_ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); } +/** + * @brief Pack a serial_control message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param device Serial control device type. + * @param flags Bitmap of serial control flags. + * @param timeout [ms] Timeout for reply data + * @param baudrate [bits/s] Baudrate of transfer. Zero means no change. + * @param count [bytes] how many bytes in this transfer + * @param data serial data + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_serial_control_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, baudrate); + _mav_put_uint16_t(buf, 4, timeout); + _mav_put_uint8_t(buf, 6, device); + _mav_put_uint8_t(buf, 7, flags); + _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t(buf, 79, target_system); + _mav_put_uint8_t(buf, 80, target_component); + _mav_put_uint8_t_array(buf, 9, data, 70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#else + mavlink_serial_control_t packet; + packet.baudrate = baudrate; + packet.timeout = timeout; + packet.device = device; + packet.flags = flags; + packet.count = count; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SERIAL_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SERIAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SERIAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +} + /** * @brief Pack a serial_control message on a channel * @param system_id ID of this system @@ -103,11 +167,13 @@ static inline uint16_t mavlink_msg_serial_control_pack(uint8_t system_id, uint8_ * @param baudrate [bits/s] Baudrate of transfer. Zero means no change. * @param count [bytes] how many bytes in this transfer * @param data serial data + * @param target_system System ID + * @param target_component Component ID * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_serial_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t device,uint8_t flags,uint16_t timeout,uint32_t baudrate,uint8_t count,const uint8_t *data) + uint8_t device,uint8_t flags,uint16_t timeout,uint32_t baudrate,uint8_t count,const uint8_t *data,uint8_t target_system,uint8_t target_component) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; @@ -116,6 +182,8 @@ static inline uint16_t mavlink_msg_serial_control_pack_chan(uint8_t system_id, u _mav_put_uint8_t(buf, 6, device); _mav_put_uint8_t(buf, 7, flags); _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t(buf, 79, target_system); + _mav_put_uint8_t(buf, 80, target_component); _mav_put_uint8_t_array(buf, 9, data, 70); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); #else @@ -125,7 +193,9 @@ static inline uint16_t mavlink_msg_serial_control_pack_chan(uint8_t system_id, u packet.device = device; packet.flags = flags; packet.count = count; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_assign_uint8_t(packet.data, data, 70); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); #endif @@ -143,7 +213,7 @@ static inline uint16_t mavlink_msg_serial_control_pack_chan(uint8_t system_id, u */ static inline uint16_t mavlink_msg_serial_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_control_t* serial_control) { - return mavlink_msg_serial_control_pack(system_id, component_id, msg, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data); + return mavlink_msg_serial_control_pack(system_id, component_id, msg, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data, serial_control->target_system, serial_control->target_component); } /** @@ -157,7 +227,21 @@ static inline uint16_t mavlink_msg_serial_control_encode(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_serial_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_control_t* serial_control) { - return mavlink_msg_serial_control_pack_chan(system_id, component_id, chan, msg, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data); + return mavlink_msg_serial_control_pack_chan(system_id, component_id, chan, msg, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data, serial_control->target_system, serial_control->target_component); +} + +/** + * @brief Encode a serial_control struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param serial_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_control_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_serial_control_t* serial_control) +{ + return mavlink_msg_serial_control_pack_status(system_id, component_id, _status, msg, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data, serial_control->target_system, serial_control->target_component); } /** @@ -170,10 +254,12 @@ static inline uint16_t mavlink_msg_serial_control_encode_chan(uint8_t system_id, * @param baudrate [bits/s] Baudrate of transfer. Zero means no change. * @param count [bytes] how many bytes in this transfer * @param data serial data + * @param target_system System ID + * @param target_component Component ID */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_serial_control_send(mavlink_channel_t chan, uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data) +static inline void mavlink_msg_serial_control_send(mavlink_channel_t chan, uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data, uint8_t target_system, uint8_t target_component) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; @@ -182,6 +268,8 @@ static inline void mavlink_msg_serial_control_send(mavlink_channel_t chan, uint8 _mav_put_uint8_t(buf, 6, device); _mav_put_uint8_t(buf, 7, flags); _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t(buf, 79, target_system); + _mav_put_uint8_t(buf, 80, target_component); _mav_put_uint8_t_array(buf, 9, data, 70); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); #else @@ -191,7 +279,9 @@ static inline void mavlink_msg_serial_control_send(mavlink_channel_t chan, uint8 packet.device = device; packet.flags = flags; packet.count = count; - mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_assign_uint8_t(packet.data, data, 70); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); #endif } @@ -204,7 +294,7 @@ static inline void mavlink_msg_serial_control_send(mavlink_channel_t chan, uint8 static inline void mavlink_msg_serial_control_send_struct(mavlink_channel_t chan, const mavlink_serial_control_t* serial_control) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_serial_control_send(chan, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data); + mavlink_msg_serial_control_send(chan, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data, serial_control->target_system, serial_control->target_component); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)serial_control, MAVLINK_MSG_ID_SERIAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); #endif @@ -212,13 +302,13 @@ static inline void mavlink_msg_serial_control_send_struct(mavlink_channel_t chan #if MAVLINK_MSG_ID_SERIAL_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_serial_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data) +static inline void mavlink_msg_serial_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data, uint8_t target_system, uint8_t target_component) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -227,6 +317,8 @@ static inline void mavlink_msg_serial_control_send_buf(mavlink_message_t *msgbuf _mav_put_uint8_t(buf, 6, device); _mav_put_uint8_t(buf, 7, flags); _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t(buf, 79, target_system); + _mav_put_uint8_t(buf, 80, target_component); _mav_put_uint8_t_array(buf, 9, data, 70); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); #else @@ -236,7 +328,9 @@ static inline void mavlink_msg_serial_control_send_buf(mavlink_message_t *msgbuf packet->device = device; packet->flags = flags; packet->count = count; - mav_array_memcpy(packet->data, data, sizeof(uint8_t)*70); + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_assign_uint8_t(packet->data, data, 70); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_SERIAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); #endif } @@ -307,6 +401,26 @@ static inline uint16_t mavlink_msg_serial_control_get_data(const mavlink_message return _MAV_RETURN_uint8_t_array(msg, data, 70, 9); } +/** + * @brief Get field target_system from serial_control message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_serial_control_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 79); +} + +/** + * @brief Get field target_component from serial_control message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_serial_control_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 80); +} + /** * @brief Decode a serial_control message into a struct * @@ -322,6 +436,8 @@ static inline void mavlink_msg_serial_control_decode(const mavlink_message_t* ms serial_control->flags = mavlink_msg_serial_control_get_flags(msg); serial_control->count = mavlink_msg_serial_control_get_count(msg); mavlink_msg_serial_control_get_data(msg, serial_control->data); + serial_control->target_system = mavlink_msg_serial_control_get_target_system(msg); + serial_control->target_component = mavlink_msg_serial_control_get_target_component(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SERIAL_CONTROL_LEN? msg->len : MAVLINK_MSG_ID_SERIAL_CONTROL_LEN; memset(serial_control, 0, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_servo_output_raw.h b/lib/main/MAVLink/common/mavlink_msg_servo_output_raw.h index 4b76d20a8ff..92cf5f95fae 100755 --- a/lib/main/MAVLink/common/mavlink_msg_servo_output_raw.h +++ b/lib/main/MAVLink/common/mavlink_msg_servo_output_raw.h @@ -165,6 +165,90 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); } +/** + * @brief Pack a servo_output_raw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param port Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk should use: 0 = MAIN, 1 = AUX. + * @param servo1_raw [us] Servo output 1 value + * @param servo2_raw [us] Servo output 2 value + * @param servo3_raw [us] Servo output 3 value + * @param servo4_raw [us] Servo output 4 value + * @param servo5_raw [us] Servo output 5 value + * @param servo6_raw [us] Servo output 6 value + * @param servo7_raw [us] Servo output 7 value + * @param servo8_raw [us] Servo output 8 value + * @param servo9_raw [us] Servo output 9 value + * @param servo10_raw [us] Servo output 10 value + * @param servo11_raw [us] Servo output 11 value + * @param servo12_raw [us] Servo output 12 value + * @param servo13_raw [us] Servo output 13 value + * @param servo14_raw [us] Servo output 14 value + * @param servo15_raw [us] Servo output 15 value + * @param servo16_raw [us] Servo output 16 value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_servo_output_raw_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw, uint16_t servo9_raw, uint16_t servo10_raw, uint16_t servo11_raw, uint16_t servo12_raw, uint16_t servo13_raw, uint16_t servo14_raw, uint16_t servo15_raw, uint16_t servo16_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN]; + _mav_put_uint32_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 4, servo1_raw); + _mav_put_uint16_t(buf, 6, servo2_raw); + _mav_put_uint16_t(buf, 8, servo3_raw); + _mav_put_uint16_t(buf, 10, servo4_raw); + _mav_put_uint16_t(buf, 12, servo5_raw); + _mav_put_uint16_t(buf, 14, servo6_raw); + _mav_put_uint16_t(buf, 16, servo7_raw); + _mav_put_uint16_t(buf, 18, servo8_raw); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint16_t(buf, 21, servo9_raw); + _mav_put_uint16_t(buf, 23, servo10_raw); + _mav_put_uint16_t(buf, 25, servo11_raw); + _mav_put_uint16_t(buf, 27, servo12_raw); + _mav_put_uint16_t(buf, 29, servo13_raw); + _mav_put_uint16_t(buf, 31, servo14_raw); + _mav_put_uint16_t(buf, 33, servo15_raw); + _mav_put_uint16_t(buf, 35, servo16_raw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#else + mavlink_servo_output_raw_t packet; + packet.time_usec = time_usec; + packet.servo1_raw = servo1_raw; + packet.servo2_raw = servo2_raw; + packet.servo3_raw = servo3_raw; + packet.servo4_raw = servo4_raw; + packet.servo5_raw = servo5_raw; + packet.servo6_raw = servo6_raw; + packet.servo7_raw = servo7_raw; + packet.servo8_raw = servo8_raw; + packet.port = port; + packet.servo9_raw = servo9_raw; + packet.servo10_raw = servo10_raw; + packet.servo11_raw = servo11_raw; + packet.servo12_raw = servo12_raw; + packet.servo13_raw = servo13_raw; + packet.servo14_raw = servo14_raw; + packet.servo15_raw = servo15_raw; + packet.servo16_raw = servo16_raw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#endif +} + /** * @brief Pack a servo_output_raw message on a channel * @param system_id ID of this system @@ -272,6 +356,20 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode_chan(uint8_t system_i return mavlink_msg_servo_output_raw_pack_chan(system_id, component_id, chan, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw, servo_output_raw->servo9_raw, servo_output_raw->servo10_raw, servo_output_raw->servo11_raw, servo_output_raw->servo12_raw, servo_output_raw->servo13_raw, servo_output_raw->servo14_raw, servo_output_raw->servo15_raw, servo_output_raw->servo16_raw); } +/** + * @brief Encode a servo_output_raw struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param servo_output_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_servo_output_raw_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_servo_output_raw_t* servo_output_raw) +{ + return mavlink_msg_servo_output_raw_pack_status(system_id, component_id, _status, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw, servo_output_raw->servo9_raw, servo_output_raw->servo10_raw, servo_output_raw->servo11_raw, servo_output_raw->servo12_raw, servo_output_raw->servo13_raw, servo_output_raw->servo14_raw, servo_output_raw->servo15_raw, servo_output_raw->servo16_raw); +} + /** * @brief Send a servo_output_raw message * @param chan MAVLink channel to send the message @@ -362,7 +460,7 @@ static inline void mavlink_msg_servo_output_raw_send_struct(mavlink_channel_t ch #if MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_set_actuator_control_target.h b/lib/main/MAVLink/common/mavlink_msg_set_actuator_control_target.h index 7d21dac9d38..ee24edd6ee6 100755 --- a/lib/main/MAVLink/common/mavlink_msg_set_actuator_control_target.h +++ b/lib/main/MAVLink/common/mavlink_msg_set_actuator_control_target.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_set_actuator_control_target_pack(uint8_t syst packet.group_mlx = group_mlx; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.controls, controls, sizeof(float)*8); + mav_array_assign_float(packet.controls, controls, 8); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN); #endif @@ -85,6 +85,49 @@ static inline uint16_t mavlink_msg_set_actuator_control_target_pack(uint8_t syst return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_CRC); } +/** + * @brief Pack a set_actuator_control_target message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param group_mlx Actuator group. The "_mlx" indicates this is a multi-instance message and a MAVLink parser should use this field to difference between instances. + * @param target_system System ID + * @param target_component Component ID + * @param controls Actuator controls. Normed to -1..+1 where 0 is neutral position. Throttle for single rotation direction motors is 0..1, negative range for reverse direction. Standard mapping for attitude controls (group 0): (index 0-7): roll, pitch, yaw, throttle, flaps, spoilers, airbrakes, landing gear. Load a pass-through mixer to repurpose them as generic outputs. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_actuator_control_target_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint8_t group_mlx, uint8_t target_system, uint8_t target_component, const float *controls) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 40, group_mlx); + _mav_put_uint8_t(buf, 41, target_system); + _mav_put_uint8_t(buf, 42, target_component); + _mav_put_float_array(buf, 8, controls, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN); +#else + mavlink_set_actuator_control_target_t packet; + packet.time_usec = time_usec; + packet.group_mlx = group_mlx; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.controls, controls, sizeof(float)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN); +#endif +} + /** * @brief Pack a set_actuator_control_target message on a channel * @param system_id ID of this system @@ -116,7 +159,7 @@ static inline uint16_t mavlink_msg_set_actuator_control_target_pack_chan(uint8_t packet.group_mlx = group_mlx; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.controls, controls, sizeof(float)*8); + mav_array_assign_float(packet.controls, controls, 8); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN); #endif @@ -151,6 +194,20 @@ static inline uint16_t mavlink_msg_set_actuator_control_target_encode_chan(uint8 return mavlink_msg_set_actuator_control_target_pack_chan(system_id, component_id, chan, msg, set_actuator_control_target->time_usec, set_actuator_control_target->group_mlx, set_actuator_control_target->target_system, set_actuator_control_target->target_component, set_actuator_control_target->controls); } +/** + * @brief Encode a set_actuator_control_target struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param set_actuator_control_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_actuator_control_target_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_set_actuator_control_target_t* set_actuator_control_target) +{ + return mavlink_msg_set_actuator_control_target_pack_status(system_id, component_id, _status, msg, set_actuator_control_target->time_usec, set_actuator_control_target->group_mlx, set_actuator_control_target->target_system, set_actuator_control_target->target_component, set_actuator_control_target->controls); +} + /** * @brief Send a set_actuator_control_target message * @param chan MAVLink channel to send the message @@ -179,7 +236,7 @@ static inline void mavlink_msg_set_actuator_control_target_send(mavlink_channel_ packet.group_mlx = group_mlx; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.controls, controls, sizeof(float)*8); + mav_array_assign_float(packet.controls, controls, 8); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET, (const char *)&packet, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_CRC); #endif } @@ -200,7 +257,7 @@ static inline void mavlink_msg_set_actuator_control_target_send_struct(mavlink_c #if MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -222,7 +279,7 @@ static inline void mavlink_msg_set_actuator_control_target_send_buf(mavlink_mess packet->group_mlx = group_mlx; packet->target_system = target_system; packet->target_component = target_component; - mav_array_memcpy(packet->controls, controls, sizeof(float)*8); + mav_array_assign_float(packet->controls, controls, 8); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET, (const char *)packet, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_LEN, MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_set_attitude_target.h b/lib/main/MAVLink/common/mavlink_msg_set_attitude_target.h index df1e851ebe5..e9ed2e044bd 100755 --- a/lib/main/MAVLink/common/mavlink_msg_set_attitude_target.h +++ b/lib/main/MAVLink/common/mavlink_msg_set_attitude_target.h @@ -3,10 +3,10 @@ #define MAVLINK_MSG_ID_SET_ATTITUDE_TARGET 82 - +MAVPACKED( typedef struct __mavlink_set_attitude_target_t { uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ - float q[4]; /*< Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0)*/ + float q[4]; /*< Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) from MAV_FRAME_LOCAL_NED to MAV_FRAME_BODY_FRD*/ float body_roll_rate; /*< [rad/s] Body roll rate*/ float body_pitch_rate; /*< [rad/s] Body pitch rate*/ float body_yaw_rate; /*< [rad/s] Body yaw rate*/ @@ -14,23 +14,25 @@ typedef struct __mavlink_set_attitude_target_t { uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ uint8_t type_mask; /*< Bitmap to indicate which dimensions should be ignored by the vehicle.*/ -} mavlink_set_attitude_target_t; + float thrust_body[3]; /*< 3D thrust setpoint in the body NED frame, normalized to -1 .. 1*/ +}) mavlink_set_attitude_target_t; -#define MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN 39 +#define MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN 51 #define MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_MIN_LEN 39 -#define MAVLINK_MSG_ID_82_LEN 39 +#define MAVLINK_MSG_ID_82_LEN 51 #define MAVLINK_MSG_ID_82_MIN_LEN 39 #define MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC 49 #define MAVLINK_MSG_ID_82_CRC 49 #define MAVLINK_MSG_SET_ATTITUDE_TARGET_FIELD_Q_LEN 4 +#define MAVLINK_MSG_SET_ATTITUDE_TARGET_FIELD_THRUST_BODY_LEN 3 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_SET_ATTITUDE_TARGET { \ 82, \ "SET_ATTITUDE_TARGET", \ - 9, \ + 10, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_set_attitude_target_t, time_boot_ms) }, \ { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_set_attitude_target_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_set_attitude_target_t, target_component) }, \ @@ -40,12 +42,13 @@ typedef struct __mavlink_set_attitude_target_t { { "body_pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_set_attitude_target_t, body_pitch_rate) }, \ { "body_yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_set_attitude_target_t, body_yaw_rate) }, \ { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_set_attitude_target_t, thrust) }, \ + { "thrust_body", NULL, MAVLINK_TYPE_FLOAT, 3, 39, offsetof(mavlink_set_attitude_target_t, thrust_body) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SET_ATTITUDE_TARGET { \ "SET_ATTITUDE_TARGET", \ - 9, \ + 10, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_set_attitude_target_t, time_boot_ms) }, \ { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_set_attitude_target_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_set_attitude_target_t, target_component) }, \ @@ -55,6 +58,7 @@ typedef struct __mavlink_set_attitude_target_t { { "body_pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_set_attitude_target_t, body_pitch_rate) }, \ { "body_yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_set_attitude_target_t, body_yaw_rate) }, \ { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_set_attitude_target_t, thrust) }, \ + { "thrust_body", NULL, MAVLINK_TYPE_FLOAT, 3, 39, offsetof(mavlink_set_attitude_target_t, thrust_body) }, \ } \ } #endif @@ -69,15 +73,16 @@ typedef struct __mavlink_set_attitude_target_t { * @param target_system System ID * @param target_component Component ID * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. - * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) from MAV_FRAME_LOCAL_NED to MAV_FRAME_BODY_FRD * @param body_roll_rate [rad/s] Body roll rate * @param body_pitch_rate [rad/s] Body pitch rate * @param body_yaw_rate [rad/s] Body yaw rate * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + * @param thrust_body 3D thrust setpoint in the body NED frame, normalized to -1 .. 1 * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_attitude_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) + uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust, const float *thrust_body) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN]; @@ -90,6 +95,7 @@ static inline uint16_t mavlink_msg_set_attitude_target_pack(uint8_t system_id, u _mav_put_uint8_t(buf, 37, target_component); _mav_put_uint8_t(buf, 38, type_mask); _mav_put_float_array(buf, 4, q, 4); + _mav_put_float_array(buf, 39, thrust_body, 3); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); #else mavlink_set_attitude_target_t packet; @@ -101,7 +107,8 @@ static inline uint16_t mavlink_msg_set_attitude_target_pack(uint8_t system_id, u packet.target_system = target_system; packet.target_component = target_component; packet.type_mask = type_mask; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); + mav_array_assign_float(packet.thrust_body, thrust_body, 3); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); #endif @@ -109,6 +116,64 @@ static inline uint16_t mavlink_msg_set_attitude_target_pack(uint8_t system_id, u return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); } +/** + * @brief Pack a set_attitude_target message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param target_system System ID + * @param target_component Component ID + * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) from MAV_FRAME_LOCAL_NED to MAV_FRAME_BODY_FRD + * @param body_roll_rate [rad/s] Body roll rate + * @param body_pitch_rate [rad/s] Body pitch rate + * @param body_yaw_rate [rad/s] Body yaw rate + * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + * @param thrust_body 3D thrust setpoint in the body NED frame, normalized to -1 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_attitude_target_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust, const float *thrust_body) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, body_roll_rate); + _mav_put_float(buf, 24, body_pitch_rate); + _mav_put_float(buf, 28, body_yaw_rate); + _mav_put_float(buf, 32, thrust); + _mav_put_uint8_t(buf, 36, target_system); + _mav_put_uint8_t(buf, 37, target_component); + _mav_put_uint8_t(buf, 38, type_mask); + _mav_put_float_array(buf, 4, q, 4); + _mav_put_float_array(buf, 39, thrust_body, 3); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#else + mavlink_set_attitude_target_t packet; + packet.time_boot_ms = time_boot_ms; + packet.body_roll_rate = body_roll_rate; + packet.body_pitch_rate = body_pitch_rate; + packet.body_yaw_rate = body_yaw_rate; + packet.thrust = thrust; + packet.target_system = target_system; + packet.target_component = target_component; + packet.type_mask = type_mask; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.thrust_body, thrust_body, sizeof(float)*3); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_ATTITUDE_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); +#endif +} + /** * @brief Pack a set_attitude_target message on a channel * @param system_id ID of this system @@ -119,16 +184,17 @@ static inline uint16_t mavlink_msg_set_attitude_target_pack(uint8_t system_id, u * @param target_system System ID * @param target_component Component ID * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. - * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) from MAV_FRAME_LOCAL_NED to MAV_FRAME_BODY_FRD * @param body_roll_rate [rad/s] Body roll rate * @param body_pitch_rate [rad/s] Body pitch rate * @param body_yaw_rate [rad/s] Body yaw rate * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + * @param thrust_body 3D thrust setpoint in the body NED frame, normalized to -1 .. 1 * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_attitude_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,uint8_t target_system,uint8_t target_component,uint8_t type_mask,const float *q,float body_roll_rate,float body_pitch_rate,float body_yaw_rate,float thrust) + uint32_t time_boot_ms,uint8_t target_system,uint8_t target_component,uint8_t type_mask,const float *q,float body_roll_rate,float body_pitch_rate,float body_yaw_rate,float thrust,const float *thrust_body) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN]; @@ -141,6 +207,7 @@ static inline uint16_t mavlink_msg_set_attitude_target_pack_chan(uint8_t system_ _mav_put_uint8_t(buf, 37, target_component); _mav_put_uint8_t(buf, 38, type_mask); _mav_put_float_array(buf, 4, q, 4); + _mav_put_float_array(buf, 39, thrust_body, 3); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); #else mavlink_set_attitude_target_t packet; @@ -152,7 +219,8 @@ static inline uint16_t mavlink_msg_set_attitude_target_pack_chan(uint8_t system_ packet.target_system = target_system; packet.target_component = target_component; packet.type_mask = type_mask; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); + mav_array_assign_float(packet.thrust_body, thrust_body, 3); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); #endif @@ -170,7 +238,7 @@ static inline uint16_t mavlink_msg_set_attitude_target_pack_chan(uint8_t system_ */ static inline uint16_t mavlink_msg_set_attitude_target_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_attitude_target_t* set_attitude_target) { - return mavlink_msg_set_attitude_target_pack(system_id, component_id, msg, set_attitude_target->time_boot_ms, set_attitude_target->target_system, set_attitude_target->target_component, set_attitude_target->type_mask, set_attitude_target->q, set_attitude_target->body_roll_rate, set_attitude_target->body_pitch_rate, set_attitude_target->body_yaw_rate, set_attitude_target->thrust); + return mavlink_msg_set_attitude_target_pack(system_id, component_id, msg, set_attitude_target->time_boot_ms, set_attitude_target->target_system, set_attitude_target->target_component, set_attitude_target->type_mask, set_attitude_target->q, set_attitude_target->body_roll_rate, set_attitude_target->body_pitch_rate, set_attitude_target->body_yaw_rate, set_attitude_target->thrust, set_attitude_target->thrust_body); } /** @@ -184,7 +252,21 @@ static inline uint16_t mavlink_msg_set_attitude_target_encode(uint8_t system_id, */ static inline uint16_t mavlink_msg_set_attitude_target_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_attitude_target_t* set_attitude_target) { - return mavlink_msg_set_attitude_target_pack_chan(system_id, component_id, chan, msg, set_attitude_target->time_boot_ms, set_attitude_target->target_system, set_attitude_target->target_component, set_attitude_target->type_mask, set_attitude_target->q, set_attitude_target->body_roll_rate, set_attitude_target->body_pitch_rate, set_attitude_target->body_yaw_rate, set_attitude_target->thrust); + return mavlink_msg_set_attitude_target_pack_chan(system_id, component_id, chan, msg, set_attitude_target->time_boot_ms, set_attitude_target->target_system, set_attitude_target->target_component, set_attitude_target->type_mask, set_attitude_target->q, set_attitude_target->body_roll_rate, set_attitude_target->body_pitch_rate, set_attitude_target->body_yaw_rate, set_attitude_target->thrust, set_attitude_target->thrust_body); +} + +/** + * @brief Encode a set_attitude_target struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param set_attitude_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_attitude_target_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_set_attitude_target_t* set_attitude_target) +{ + return mavlink_msg_set_attitude_target_pack_status(system_id, component_id, _status, msg, set_attitude_target->time_boot_ms, set_attitude_target->target_system, set_attitude_target->target_component, set_attitude_target->type_mask, set_attitude_target->q, set_attitude_target->body_roll_rate, set_attitude_target->body_pitch_rate, set_attitude_target->body_yaw_rate, set_attitude_target->thrust, set_attitude_target->thrust_body); } /** @@ -195,15 +277,16 @@ static inline uint16_t mavlink_msg_set_attitude_target_encode_chan(uint8_t syste * @param target_system System ID * @param target_component Component ID * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. - * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) from MAV_FRAME_LOCAL_NED to MAV_FRAME_BODY_FRD * @param body_roll_rate [rad/s] Body roll rate * @param body_pitch_rate [rad/s] Body pitch rate * @param body_yaw_rate [rad/s] Body yaw rate * @param thrust Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust) + * @param thrust_body 3D thrust setpoint in the body NED frame, normalized to -1 .. 1 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_set_attitude_target_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) +static inline void mavlink_msg_set_attitude_target_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust, const float *thrust_body) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN]; @@ -216,6 +299,7 @@ static inline void mavlink_msg_set_attitude_target_send(mavlink_channel_t chan, _mav_put_uint8_t(buf, 37, target_component); _mav_put_uint8_t(buf, 38, type_mask); _mav_put_float_array(buf, 4, q, 4); + _mav_put_float_array(buf, 39, thrust_body, 3); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); #else mavlink_set_attitude_target_t packet; @@ -227,7 +311,8 @@ static inline void mavlink_msg_set_attitude_target_send(mavlink_channel_t chan, packet.target_system = target_system; packet.target_component = target_component; packet.type_mask = type_mask; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); + mav_array_assign_float(packet.thrust_body, thrust_body, 3); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, (const char *)&packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); #endif } @@ -240,7 +325,7 @@ static inline void mavlink_msg_set_attitude_target_send(mavlink_channel_t chan, static inline void mavlink_msg_set_attitude_target_send_struct(mavlink_channel_t chan, const mavlink_set_attitude_target_t* set_attitude_target) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_set_attitude_target_send(chan, set_attitude_target->time_boot_ms, set_attitude_target->target_system, set_attitude_target->target_component, set_attitude_target->type_mask, set_attitude_target->q, set_attitude_target->body_roll_rate, set_attitude_target->body_pitch_rate, set_attitude_target->body_yaw_rate, set_attitude_target->thrust); + mavlink_msg_set_attitude_target_send(chan, set_attitude_target->time_boot_ms, set_attitude_target->target_system, set_attitude_target->target_component, set_attitude_target->type_mask, set_attitude_target->q, set_attitude_target->body_roll_rate, set_attitude_target->body_pitch_rate, set_attitude_target->body_yaw_rate, set_attitude_target->thrust, set_attitude_target->thrust_body); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, (const char *)set_attitude_target, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); #endif @@ -248,13 +333,13 @@ static inline void mavlink_msg_set_attitude_target_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_set_attitude_target_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust) +static inline void mavlink_msg_set_attitude_target_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t type_mask, const float *q, float body_roll_rate, float body_pitch_rate, float body_yaw_rate, float thrust, const float *thrust_body) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -267,6 +352,7 @@ static inline void mavlink_msg_set_attitude_target_send_buf(mavlink_message_t *m _mav_put_uint8_t(buf, 37, target_component); _mav_put_uint8_t(buf, 38, type_mask); _mav_put_float_array(buf, 4, q, 4); + _mav_put_float_array(buf, 39, thrust_body, 3); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, buf, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); #else mavlink_set_attitude_target_t *packet = (mavlink_set_attitude_target_t *)msgbuf; @@ -278,7 +364,8 @@ static inline void mavlink_msg_set_attitude_target_send_buf(mavlink_message_t *m packet->target_system = target_system; packet->target_component = target_component; packet->type_mask = type_mask; - mav_array_memcpy(packet->q, q, sizeof(float)*4); + mav_array_assign_float(packet->q, q, 4); + mav_array_assign_float(packet->thrust_body, thrust_body, 3); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET, (const char *)packet, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_MIN_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_CRC); #endif } @@ -332,7 +419,7 @@ static inline uint8_t mavlink_msg_set_attitude_target_get_type_mask(const mavlin /** * @brief Get field q from set_attitude_target message * - * @return Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @return Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) from MAV_FRAME_LOCAL_NED to MAV_FRAME_BODY_FRD */ static inline uint16_t mavlink_msg_set_attitude_target_get_q(const mavlink_message_t* msg, float *q) { @@ -379,6 +466,16 @@ static inline float mavlink_msg_set_attitude_target_get_thrust(const mavlink_mes return _MAV_RETURN_float(msg, 32); } +/** + * @brief Get field thrust_body from set_attitude_target message + * + * @return 3D thrust setpoint in the body NED frame, normalized to -1 .. 1 + */ +static inline uint16_t mavlink_msg_set_attitude_target_get_thrust_body(const mavlink_message_t* msg, float *thrust_body) +{ + return _MAV_RETURN_float_array(msg, thrust_body, 3, 39); +} + /** * @brief Decode a set_attitude_target message into a struct * @@ -397,6 +494,7 @@ static inline void mavlink_msg_set_attitude_target_decode(const mavlink_message_ set_attitude_target->target_system = mavlink_msg_set_attitude_target_get_target_system(msg); set_attitude_target->target_component = mavlink_msg_set_attitude_target_get_target_component(msg); set_attitude_target->type_mask = mavlink_msg_set_attitude_target_get_type_mask(msg); + mavlink_msg_set_attitude_target_get_thrust_body(msg, set_attitude_target->thrust_body); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN? msg->len : MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN; memset(set_attitude_target, 0, MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_set_gps_global_origin.h b/lib/main/MAVLink/common/mavlink_msg_set_gps_global_origin.h index 62a2f448f8c..ddee5570358 100755 --- a/lib/main/MAVLink/common/mavlink_msg_set_gps_global_origin.h +++ b/lib/main/MAVLink/common/mavlink_msg_set_gps_global_origin.h @@ -87,6 +87,51 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack(uint8_t system_id, return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); } +/** + * @brief Pack a set_gps_global_origin message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param latitude [degE7] Latitude (WGS84) + * @param longitude [degE7] Longitude (WGS84) + * @param altitude [mm] Altitude (MSL). Positive for up. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_gps_global_origin_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, uint64_t time_usec) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint64_t(buf, 13, time_usec); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#else + mavlink_set_gps_global_origin_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.target_system = target_system; + packet.time_usec = time_usec; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#endif +} + /** * @brief Pack a set_gps_global_origin message on a channel * @param system_id ID of this system @@ -155,6 +200,20 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_encode_chan(uint8_t sys return mavlink_msg_set_gps_global_origin_pack_chan(system_id, component_id, chan, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude, set_gps_global_origin->time_usec); } +/** + * @brief Encode a set_gps_global_origin struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param set_gps_global_origin C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_gps_global_origin_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_set_gps_global_origin_t* set_gps_global_origin) +{ + return mavlink_msg_set_gps_global_origin_pack_status(system_id, component_id, _status, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude, set_gps_global_origin->time_usec); +} + /** * @brief Send a set_gps_global_origin message * @param chan MAVLink channel to send the message @@ -206,7 +265,7 @@ static inline void mavlink_msg_set_gps_global_origin_send_struct(mavlink_channel #if MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_set_home_position.h b/lib/main/MAVLink/common/mavlink_msg_set_home_position.h index ea7d2d0f266..e7c22e96c56 100755 --- a/lib/main/MAVLink/common/mavlink_msg_set_home_position.h +++ b/lib/main/MAVLink/common/mavlink_msg_set_home_position.h @@ -8,9 +8,9 @@ typedef struct __mavlink_set_home_position_t { int32_t latitude; /*< [degE7] Latitude (WGS84)*/ int32_t longitude; /*< [degE7] Longitude (WGS84)*/ int32_t altitude; /*< [mm] Altitude (MSL). Positive for up.*/ - float x; /*< [m] Local X position of this position in the local coordinate frame*/ - float y; /*< [m] Local Y position of this position in the local coordinate frame*/ - float z; /*< [m] Local Z position of this position in the local coordinate frame*/ + float x; /*< [m] Local X position of this position in the local coordinate frame (NED)*/ + float y; /*< [m] Local Y position of this position in the local coordinate frame (NED)*/ + float z; /*< [m] Local Z position of this position in the local coordinate frame (NED: positive "down")*/ float q[4]; /*< World to surface normal and heading transformation of the takeoff position. Used to indicate the heading and slope of the ground*/ float approach_x; /*< [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone.*/ float approach_y; /*< [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone.*/ @@ -78,9 +78,9 @@ typedef struct __mavlink_set_home_position_t { * @param latitude [degE7] Latitude (WGS84) * @param longitude [degE7] Longitude (WGS84) * @param altitude [mm] Altitude (MSL). Positive for up. - * @param x [m] Local X position of this position in the local coordinate frame - * @param y [m] Local Y position of this position in the local coordinate frame - * @param z [m] Local Z position of this position in the local coordinate frame + * @param x [m] Local X position of this position in the local coordinate frame (NED) + * @param y [m] Local Y position of this position in the local coordinate frame (NED) + * @param z [m] Local Z position of this position in the local coordinate frame (NED: positive "down") * @param q World to surface normal and heading transformation of the takeoff position. Used to indicate the heading and slope of the ground * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. @@ -119,7 +119,7 @@ static inline uint16_t mavlink_msg_set_home_position_pack(uint8_t system_id, uin packet.approach_z = approach_z; packet.target_system = target_system; packet.time_usec = time_usec; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN); #endif @@ -127,6 +127,70 @@ static inline uint16_t mavlink_msg_set_home_position_pack(uint8_t system_id, uin return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_CRC); } +/** + * @brief Pack a set_home_position message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID. + * @param latitude [degE7] Latitude (WGS84) + * @param longitude [degE7] Longitude (WGS84) + * @param altitude [mm] Altitude (MSL). Positive for up. + * @param x [m] Local X position of this position in the local coordinate frame (NED) + * @param y [m] Local Y position of this position in the local coordinate frame (NED) + * @param z [m] Local Z position of this position in the local coordinate frame (NED: positive "down") + * @param q World to surface normal and heading transformation of the takeoff position. Used to indicate the heading and slope of the ground + * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + * @param approach_z [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_home_position_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z, uint64_t time_usec) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_HOME_POSITION_LEN]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_float(buf, 12, x); + _mav_put_float(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_float(buf, 40, approach_x); + _mav_put_float(buf, 44, approach_y); + _mav_put_float(buf, 48, approach_z); + _mav_put_uint8_t(buf, 52, target_system); + _mav_put_uint64_t(buf, 53, time_usec); + _mav_put_float_array(buf, 24, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN); +#else + mavlink_set_home_position_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.x = x; + packet.y = y; + packet.z = z; + packet.approach_x = approach_x; + packet.approach_y = approach_y; + packet.approach_z = approach_z; + packet.target_system = target_system; + packet.time_usec = time_usec; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_HOME_POSITION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SET_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SET_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN); +#endif +} + /** * @brief Pack a set_home_position message on a channel * @param system_id ID of this system @@ -137,9 +201,9 @@ static inline uint16_t mavlink_msg_set_home_position_pack(uint8_t system_id, uin * @param latitude [degE7] Latitude (WGS84) * @param longitude [degE7] Longitude (WGS84) * @param altitude [mm] Altitude (MSL). Positive for up. - * @param x [m] Local X position of this position in the local coordinate frame - * @param y [m] Local Y position of this position in the local coordinate frame - * @param z [m] Local Z position of this position in the local coordinate frame + * @param x [m] Local X position of this position in the local coordinate frame (NED) + * @param y [m] Local Y position of this position in the local coordinate frame (NED) + * @param z [m] Local Z position of this position in the local coordinate frame (NED: positive "down") * @param q World to surface normal and heading transformation of the takeoff position. Used to indicate the heading and slope of the ground * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. @@ -179,7 +243,7 @@ static inline uint16_t mavlink_msg_set_home_position_pack_chan(uint8_t system_id packet.approach_z = approach_z; packet.target_system = target_system; packet.time_usec = time_usec; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN); #endif @@ -214,6 +278,20 @@ static inline uint16_t mavlink_msg_set_home_position_encode_chan(uint8_t system_ return mavlink_msg_set_home_position_pack_chan(system_id, component_id, chan, msg, set_home_position->target_system, set_home_position->latitude, set_home_position->longitude, set_home_position->altitude, set_home_position->x, set_home_position->y, set_home_position->z, set_home_position->q, set_home_position->approach_x, set_home_position->approach_y, set_home_position->approach_z, set_home_position->time_usec); } +/** + * @brief Encode a set_home_position struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param set_home_position C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_home_position_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_set_home_position_t* set_home_position) +{ + return mavlink_msg_set_home_position_pack_status(system_id, component_id, _status, msg, set_home_position->target_system, set_home_position->latitude, set_home_position->longitude, set_home_position->altitude, set_home_position->x, set_home_position->y, set_home_position->z, set_home_position->q, set_home_position->approach_x, set_home_position->approach_y, set_home_position->approach_z, set_home_position->time_usec); +} + /** * @brief Send a set_home_position message * @param chan MAVLink channel to send the message @@ -222,9 +300,9 @@ static inline uint16_t mavlink_msg_set_home_position_encode_chan(uint8_t system_ * @param latitude [degE7] Latitude (WGS84) * @param longitude [degE7] Longitude (WGS84) * @param altitude [mm] Altitude (MSL). Positive for up. - * @param x [m] Local X position of this position in the local coordinate frame - * @param y [m] Local Y position of this position in the local coordinate frame - * @param z [m] Local Z position of this position in the local coordinate frame + * @param x [m] Local X position of this position in the local coordinate frame (NED) + * @param y [m] Local Y position of this position in the local coordinate frame (NED) + * @param z [m] Local Z position of this position in the local coordinate frame (NED: positive "down") * @param q World to surface normal and heading transformation of the takeoff position. Used to indicate the heading and slope of the ground * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. @@ -263,7 +341,7 @@ static inline void mavlink_msg_set_home_position_send(mavlink_channel_t chan, ui packet.approach_z = approach_z; packet.target_system = target_system; packet.time_usec = time_usec; - mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_assign_float(packet.q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_HOME_POSITION, (const char *)&packet, MAVLINK_MSG_ID_SET_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_CRC); #endif } @@ -284,7 +362,7 @@ static inline void mavlink_msg_set_home_position_send_struct(mavlink_channel_t c #if MAVLINK_MSG_ID_SET_HOME_POSITION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -320,7 +398,7 @@ static inline void mavlink_msg_set_home_position_send_buf(mavlink_message_t *msg packet->approach_z = approach_z; packet->target_system = target_system; packet->time_usec = time_usec; - mav_array_memcpy(packet->q, q, sizeof(float)*4); + mav_array_assign_float(packet->q, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_HOME_POSITION, (const char *)packet, MAVLINK_MSG_ID_SET_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_CRC); #endif } @@ -374,7 +452,7 @@ static inline int32_t mavlink_msg_set_home_position_get_altitude(const mavlink_m /** * @brief Get field x from set_home_position message * - * @return [m] Local X position of this position in the local coordinate frame + * @return [m] Local X position of this position in the local coordinate frame (NED) */ static inline float mavlink_msg_set_home_position_get_x(const mavlink_message_t* msg) { @@ -384,7 +462,7 @@ static inline float mavlink_msg_set_home_position_get_x(const mavlink_message_t* /** * @brief Get field y from set_home_position message * - * @return [m] Local Y position of this position in the local coordinate frame + * @return [m] Local Y position of this position in the local coordinate frame (NED) */ static inline float mavlink_msg_set_home_position_get_y(const mavlink_message_t* msg) { @@ -394,7 +472,7 @@ static inline float mavlink_msg_set_home_position_get_y(const mavlink_message_t* /** * @brief Get field z from set_home_position message * - * @return [m] Local Z position of this position in the local coordinate frame + * @return [m] Local Z position of this position in the local coordinate frame (NED: positive "down") */ static inline float mavlink_msg_set_home_position_get_z(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_set_mode.h b/lib/main/MAVLink/common/mavlink_msg_set_mode.h index d2d0874525c..613f92b5514 100755 --- a/lib/main/MAVLink/common/mavlink_msg_set_mode.h +++ b/lib/main/MAVLink/common/mavlink_msg_set_mode.h @@ -75,6 +75,45 @@ static inline uint16_t mavlink_msg_set_mode_pack(uint8_t system_id, uint8_t comp return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MODE_MIN_LEN, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); } +/** + * @brief Pack a set_mode message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system The system setting the mode + * @param base_mode The new base mode. + * @param custom_mode The new autopilot-specific mode. This field can be ignored by an autopilot. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_mode_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t base_mode, uint32_t custom_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_MODE_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, base_mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_MODE_LEN); +#else + mavlink_set_mode_t packet; + packet.custom_mode = custom_mode; + packet.target_system = target_system; + packet.base_mode = base_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_MODE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_MODE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SET_MODE_MIN_LEN, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SET_MODE_MIN_LEN, MAVLINK_MSG_ID_SET_MODE_LEN); +#endif +} + /** * @brief Pack a set_mode message on a channel * @param system_id ID of this system @@ -137,6 +176,20 @@ static inline uint16_t mavlink_msg_set_mode_encode_chan(uint8_t system_id, uint8 return mavlink_msg_set_mode_pack_chan(system_id, component_id, chan, msg, set_mode->target_system, set_mode->base_mode, set_mode->custom_mode); } +/** + * @brief Encode a set_mode struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param set_mode C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_mode_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_set_mode_t* set_mode) +{ + return mavlink_msg_set_mode_pack_status(system_id, component_id, _status, msg, set_mode->target_system, set_mode->base_mode, set_mode->custom_mode); +} + /** * @brief Send a set_mode message * @param chan MAVLink channel to send the message @@ -182,7 +235,7 @@ static inline void mavlink_msg_set_mode_send_struct(mavlink_channel_t chan, cons #if MAVLINK_MSG_ID_SET_MODE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_set_position_target_global_int.h b/lib/main/MAVLink/common/mavlink_msg_set_position_target_global_int.h index 3a1523078c5..29c4c70157a 100755 --- a/lib/main/MAVLink/common/mavlink_msg_set_position_target_global_int.h +++ b/lib/main/MAVLink/common/mavlink_msg_set_position_target_global_int.h @@ -6,8 +6,8 @@ typedef struct __mavlink_set_position_target_global_int_t { uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot). The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency.*/ - int32_t lat_int; /*< [degE7] X Position in WGS84 frame*/ - int32_t lon_int; /*< [degE7] Y Position in WGS84 frame*/ + int32_t lat_int; /*< [degE7] Latitude in WGS84 frame*/ + int32_t lon_int; /*< [degE7] Longitude in WGS84 frame*/ float alt; /*< [m] Altitude (MSL, Relative to home, or AGL - depending on frame)*/ float vx; /*< [m/s] X velocity in NED frame*/ float vy; /*< [m/s] Y velocity in NED frame*/ @@ -20,7 +20,7 @@ typedef struct __mavlink_set_position_target_global_int_t { uint16_t type_mask; /*< Bitmap to indicate which dimensions should be ignored by the vehicle.*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ - uint8_t coordinate_frame; /*< Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11*/ + uint8_t coordinate_frame; /*< Valid options are: MAV_FRAME_GLOBAL = 0, MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, MAV_FRAME_GLOBAL_TERRAIN_ALT = 10 (MAV_FRAME_GLOBAL_INT, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT are allowed synonyms, but have been deprecated)*/ } mavlink_set_position_target_global_int_t; #define MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN 53 @@ -89,10 +89,10 @@ typedef struct __mavlink_set_position_target_global_int_t { * @param time_boot_ms [ms] Timestamp (time since system boot). The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. * @param target_system System ID * @param target_component Component ID - * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL = 0, MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, MAV_FRAME_GLOBAL_TERRAIN_ALT = 10 (MAV_FRAME_GLOBAL_INT, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT are allowed synonyms, but have been deprecated) * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. - * @param lat_int [degE7] X Position in WGS84 frame - * @param lon_int [degE7] Y Position in WGS84 frame + * @param lat_int [degE7] Latitude in WGS84 frame + * @param lon_int [degE7] Longitude in WGS84 frame * @param alt [m] Altitude (MSL, Relative to home, or AGL - depending on frame) * @param vx [m/s] X velocity in NED frame * @param vy [m/s] Y velocity in NED frame @@ -153,6 +153,84 @@ static inline uint16_t mavlink_msg_set_position_target_global_int_pack(uint8_t s return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_MIN_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_CRC); } +/** + * @brief Pack a set_position_target_global_int message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. + * @param target_system System ID + * @param target_component Component ID + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL = 0, MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, MAV_FRAME_GLOBAL_TERRAIN_ALT = 10 (MAV_FRAME_GLOBAL_INT, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT are allowed synonyms, but have been deprecated) + * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. + * @param lat_int [degE7] Latitude in WGS84 frame + * @param lon_int [degE7] Longitude in WGS84 frame + * @param alt [m] Altitude (MSL, Relative to home, or AGL - depending on frame) + * @param vx [m/s] X velocity in NED frame + * @param vy [m/s] Y velocity in NED frame + * @param vz [m/s] Z velocity in NED frame + * @param afx [m/s/s] X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy [m/s/s] Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz [m/s/s] Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw [rad] yaw setpoint + * @param yaw_rate [rad/s] yaw rate setpoint + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_position_target_global_int_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t type_mask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_int); + _mav_put_int32_t(buf, 8, lon_int); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#else + mavlink_set_position_target_global_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_int = lat_int; + packet.lon_int = lon_int; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.target_system = target_system; + packet.target_component = target_component; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_MIN_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_MIN_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN); +#endif +} + /** * @brief Pack a set_position_target_global_int message on a channel * @param system_id ID of this system @@ -162,10 +240,10 @@ static inline uint16_t mavlink_msg_set_position_target_global_int_pack(uint8_t s * @param time_boot_ms [ms] Timestamp (time since system boot). The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. * @param target_system System ID * @param target_component Component ID - * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL = 0, MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, MAV_FRAME_GLOBAL_TERRAIN_ALT = 10 (MAV_FRAME_GLOBAL_INT, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT are allowed synonyms, but have been deprecated) * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. - * @param lat_int [degE7] X Position in WGS84 frame - * @param lon_int [degE7] Y Position in WGS84 frame + * @param lat_int [degE7] Latitude in WGS84 frame + * @param lon_int [degE7] Longitude in WGS84 frame * @param alt [m] Altitude (MSL, Relative to home, or AGL - depending on frame) * @param vx [m/s] X velocity in NED frame * @param vy [m/s] Y velocity in NED frame @@ -254,6 +332,20 @@ static inline uint16_t mavlink_msg_set_position_target_global_int_encode_chan(ui return mavlink_msg_set_position_target_global_int_pack_chan(system_id, component_id, chan, msg, set_position_target_global_int->time_boot_ms, set_position_target_global_int->target_system, set_position_target_global_int->target_component, set_position_target_global_int->coordinate_frame, set_position_target_global_int->type_mask, set_position_target_global_int->lat_int, set_position_target_global_int->lon_int, set_position_target_global_int->alt, set_position_target_global_int->vx, set_position_target_global_int->vy, set_position_target_global_int->vz, set_position_target_global_int->afx, set_position_target_global_int->afy, set_position_target_global_int->afz, set_position_target_global_int->yaw, set_position_target_global_int->yaw_rate); } +/** + * @brief Encode a set_position_target_global_int struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param set_position_target_global_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_position_target_global_int_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_set_position_target_global_int_t* set_position_target_global_int) +{ + return mavlink_msg_set_position_target_global_int_pack_status(system_id, component_id, _status, msg, set_position_target_global_int->time_boot_ms, set_position_target_global_int->target_system, set_position_target_global_int->target_component, set_position_target_global_int->coordinate_frame, set_position_target_global_int->type_mask, set_position_target_global_int->lat_int, set_position_target_global_int->lon_int, set_position_target_global_int->alt, set_position_target_global_int->vx, set_position_target_global_int->vy, set_position_target_global_int->vz, set_position_target_global_int->afx, set_position_target_global_int->afy, set_position_target_global_int->afz, set_position_target_global_int->yaw, set_position_target_global_int->yaw_rate); +} + /** * @brief Send a set_position_target_global_int message * @param chan MAVLink channel to send the message @@ -261,10 +353,10 @@ static inline uint16_t mavlink_msg_set_position_target_global_int_encode_chan(ui * @param time_boot_ms [ms] Timestamp (time since system boot). The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. * @param target_system System ID * @param target_component Component ID - * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @param coordinate_frame Valid options are: MAV_FRAME_GLOBAL = 0, MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, MAV_FRAME_GLOBAL_TERRAIN_ALT = 10 (MAV_FRAME_GLOBAL_INT, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT are allowed synonyms, but have been deprecated) * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. - * @param lat_int [degE7] X Position in WGS84 frame - * @param lon_int [degE7] Y Position in WGS84 frame + * @param lat_int [degE7] Latitude in WGS84 frame + * @param lon_int [degE7] Longitude in WGS84 frame * @param alt [m] Altitude (MSL, Relative to home, or AGL - depending on frame) * @param vx [m/s] X velocity in NED frame * @param vy [m/s] Y velocity in NED frame @@ -338,7 +430,7 @@ static inline void mavlink_msg_set_position_target_global_int_send_struct(mavlin #if MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -428,7 +520,7 @@ static inline uint8_t mavlink_msg_set_position_target_global_int_get_target_comp /** * @brief Get field coordinate_frame from set_position_target_global_int message * - * @return Valid options are: MAV_FRAME_GLOBAL_INT = 5, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT = 6, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11 + * @return Valid options are: MAV_FRAME_GLOBAL = 0, MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, MAV_FRAME_GLOBAL_TERRAIN_ALT = 10 (MAV_FRAME_GLOBAL_INT, MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, MAV_FRAME_GLOBAL_TERRAIN_ALT_INT are allowed synonyms, but have been deprecated) */ static inline uint8_t mavlink_msg_set_position_target_global_int_get_coordinate_frame(const mavlink_message_t* msg) { @@ -448,7 +540,7 @@ static inline uint16_t mavlink_msg_set_position_target_global_int_get_type_mask( /** * @brief Get field lat_int from set_position_target_global_int message * - * @return [degE7] X Position in WGS84 frame + * @return [degE7] Latitude in WGS84 frame */ static inline int32_t mavlink_msg_set_position_target_global_int_get_lat_int(const mavlink_message_t* msg) { @@ -458,7 +550,7 @@ static inline int32_t mavlink_msg_set_position_target_global_int_get_lat_int(con /** * @brief Get field lon_int from set_position_target_global_int message * - * @return [degE7] Y Position in WGS84 frame + * @return [degE7] Longitude in WGS84 frame */ static inline int32_t mavlink_msg_set_position_target_global_int_get_lon_int(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_set_position_target_local_ned.h b/lib/main/MAVLink/common/mavlink_msg_set_position_target_local_ned.h index 4e16b2ab34c..3c7256d459e 100755 --- a/lib/main/MAVLink/common/mavlink_msg_set_position_target_local_ned.h +++ b/lib/main/MAVLink/common/mavlink_msg_set_position_target_local_ned.h @@ -153,6 +153,84 @@ static inline uint16_t mavlink_msg_set_position_target_local_ned_pack(uint8_t sy return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_MIN_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_CRC); } +/** + * @brief Pack a set_position_target_local_ned message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param target_system System ID + * @param target_component Component ID + * @param coordinate_frame Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9 + * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. + * @param x [m] X Position in NED frame + * @param y [m] Y Position in NED frame + * @param z [m] Z Position in NED frame (note, altitude is negative in NED) + * @param vx [m/s] X velocity in NED frame + * @param vy [m/s] Y velocity in NED frame + * @param vz [m/s] Z velocity in NED frame + * @param afx [m/s/s] X acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afy [m/s/s] Y acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param afz [m/s/s] Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N + * @param yaw [rad] yaw setpoint + * @param yaw_rate [rad/s] yaw rate setpoint + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_position_target_local_ned_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, uint16_t type_mask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + _mav_put_float(buf, 28, afx); + _mav_put_float(buf, 32, afy); + _mav_put_float(buf, 36, afz); + _mav_put_float(buf, 40, yaw); + _mav_put_float(buf, 44, yaw_rate); + _mav_put_uint16_t(buf, 48, type_mask); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, coordinate_frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#else + mavlink_set_position_target_local_ned_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.afx = afx; + packet.afy = afy; + packet.afz = afz; + packet.yaw = yaw; + packet.yaw_rate = yaw_rate; + packet.type_mask = type_mask; + packet.target_system = target_system; + packet.target_component = target_component; + packet.coordinate_frame = coordinate_frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_MIN_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_MIN_LEN, MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN); +#endif +} + /** * @brief Pack a set_position_target_local_ned message on a channel * @param system_id ID of this system @@ -254,6 +332,20 @@ static inline uint16_t mavlink_msg_set_position_target_local_ned_encode_chan(uin return mavlink_msg_set_position_target_local_ned_pack_chan(system_id, component_id, chan, msg, set_position_target_local_ned->time_boot_ms, set_position_target_local_ned->target_system, set_position_target_local_ned->target_component, set_position_target_local_ned->coordinate_frame, set_position_target_local_ned->type_mask, set_position_target_local_ned->x, set_position_target_local_ned->y, set_position_target_local_ned->z, set_position_target_local_ned->vx, set_position_target_local_ned->vy, set_position_target_local_ned->vz, set_position_target_local_ned->afx, set_position_target_local_ned->afy, set_position_target_local_ned->afz, set_position_target_local_ned->yaw, set_position_target_local_ned->yaw_rate); } +/** + * @brief Encode a set_position_target_local_ned struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param set_position_target_local_ned C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_position_target_local_ned_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_set_position_target_local_ned_t* set_position_target_local_ned) +{ + return mavlink_msg_set_position_target_local_ned_pack_status(system_id, component_id, _status, msg, set_position_target_local_ned->time_boot_ms, set_position_target_local_ned->target_system, set_position_target_local_ned->target_component, set_position_target_local_ned->coordinate_frame, set_position_target_local_ned->type_mask, set_position_target_local_ned->x, set_position_target_local_ned->y, set_position_target_local_ned->z, set_position_target_local_ned->vx, set_position_target_local_ned->vy, set_position_target_local_ned->vz, set_position_target_local_ned->afx, set_position_target_local_ned->afy, set_position_target_local_ned->afz, set_position_target_local_ned->yaw, set_position_target_local_ned->yaw_rate); +} + /** * @brief Send a set_position_target_local_ned message * @param chan MAVLink channel to send the message @@ -338,7 +430,7 @@ static inline void mavlink_msg_set_position_target_local_ned_send_struct(mavlink #if MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_setup_signing.h b/lib/main/MAVLink/common/mavlink_msg_setup_signing.h index e9bab0cbaa6..d415fd6bdc4 100644 --- a/lib/main/MAVLink/common/mavlink_msg_setup_signing.h +++ b/lib/main/MAVLink/common/mavlink_msg_setup_signing.h @@ -71,7 +71,7 @@ static inline uint16_t mavlink_msg_setup_signing_pack(uint8_t system_id, uint8_t packet.initial_timestamp = initial_timestamp; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.secret_key, secret_key, sizeof(uint8_t)*32); + mav_array_assign_uint8_t(packet.secret_key, secret_key, 32); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETUP_SIGNING_LEN); #endif @@ -79,6 +79,46 @@ static inline uint16_t mavlink_msg_setup_signing_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_CRC); } +/** + * @brief Pack a setup_signing message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system system id of the target + * @param target_component component ID of the target + * @param secret_key signing key + * @param initial_timestamp initial timestamp + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_setup_signing_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *secret_key, uint64_t initial_timestamp) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SETUP_SIGNING_LEN]; + _mav_put_uint64_t(buf, 0, initial_timestamp); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); + _mav_put_uint8_t_array(buf, 10, secret_key, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETUP_SIGNING_LEN); +#else + mavlink_setup_signing_t packet; + packet.initial_timestamp = initial_timestamp; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.secret_key, secret_key, sizeof(uint8_t)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETUP_SIGNING_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SETUP_SIGNING; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN); +#endif +} + /** * @brief Pack a setup_signing message on a channel * @param system_id ID of this system @@ -107,7 +147,7 @@ static inline uint16_t mavlink_msg_setup_signing_pack_chan(uint8_t system_id, ui packet.initial_timestamp = initial_timestamp; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.secret_key, secret_key, sizeof(uint8_t)*32); + mav_array_assign_uint8_t(packet.secret_key, secret_key, 32); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETUP_SIGNING_LEN); #endif @@ -142,6 +182,20 @@ static inline uint16_t mavlink_msg_setup_signing_encode_chan(uint8_t system_id, return mavlink_msg_setup_signing_pack_chan(system_id, component_id, chan, msg, setup_signing->target_system, setup_signing->target_component, setup_signing->secret_key, setup_signing->initial_timestamp); } +/** + * @brief Encode a setup_signing struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param setup_signing C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_setup_signing_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_setup_signing_t* setup_signing) +{ + return mavlink_msg_setup_signing_pack_status(system_id, component_id, _status, msg, setup_signing->target_system, setup_signing->target_component, setup_signing->secret_key, setup_signing->initial_timestamp); +} + /** * @brief Send a setup_signing message * @param chan MAVLink channel to send the message @@ -167,7 +221,7 @@ static inline void mavlink_msg_setup_signing_send(mavlink_channel_t chan, uint8_ packet.initial_timestamp = initial_timestamp; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.secret_key, secret_key, sizeof(uint8_t)*32); + mav_array_assign_uint8_t(packet.secret_key, secret_key, 32); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETUP_SIGNING, (const char *)&packet, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_CRC); #endif } @@ -188,7 +242,7 @@ static inline void mavlink_msg_setup_signing_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_SETUP_SIGNING_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -208,7 +262,7 @@ static inline void mavlink_msg_setup_signing_send_buf(mavlink_message_t *msgbuf, packet->initial_timestamp = initial_timestamp; packet->target_system = target_system; packet->target_component = target_component; - mav_array_memcpy(packet->secret_key, secret_key, sizeof(uint8_t)*32); + mav_array_assign_uint8_t(packet->secret_key, secret_key, 32); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETUP_SIGNING, (const char *)packet, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_sim_state.h b/lib/main/MAVLink/common/mavlink_msg_sim_state.h index b128b8d54bc..c49735f65dd 100755 --- a/lib/main/MAVLink/common/mavlink_msg_sim_state.h +++ b/lib/main/MAVLink/common/mavlink_msg_sim_state.h @@ -9,28 +9,30 @@ typedef struct __mavlink_sim_state_t { float q2; /*< True attitude quaternion component 2, x (0 in null-rotation)*/ float q3; /*< True attitude quaternion component 3, y (0 in null-rotation)*/ float q4; /*< True attitude quaternion component 4, z (0 in null-rotation)*/ - float roll; /*< Attitude roll expressed as Euler angles, not recommended except for human-readable outputs*/ - float pitch; /*< Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs*/ - float yaw; /*< Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs*/ + float roll; /*< [rad] Attitude roll expressed as Euler angles, not recommended except for human-readable outputs*/ + float pitch; /*< [rad] Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs*/ + float yaw; /*< [rad] Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs*/ float xacc; /*< [m/s/s] X acceleration*/ float yacc; /*< [m/s/s] Y acceleration*/ float zacc; /*< [m/s/s] Z acceleration*/ float xgyro; /*< [rad/s] Angular speed around X axis*/ float ygyro; /*< [rad/s] Angular speed around Y axis*/ float zgyro; /*< [rad/s] Angular speed around Z axis*/ - float lat; /*< [deg] Latitude*/ - float lon; /*< [deg] Longitude*/ + float lat; /*< [deg] Latitude (lower precision). Both this and the lat_int field should be set.*/ + float lon; /*< [deg] Longitude (lower precision). Both this and the lon_int field should be set.*/ float alt; /*< [m] Altitude*/ float std_dev_horz; /*< Horizontal position standard deviation*/ float std_dev_vert; /*< Vertical position standard deviation*/ float vn; /*< [m/s] True velocity in north direction in earth-fixed NED frame*/ float ve; /*< [m/s] True velocity in east direction in earth-fixed NED frame*/ float vd; /*< [m/s] True velocity in down direction in earth-fixed NED frame*/ + int32_t lat_int; /*< [degE7] Latitude (higher precision). If 0, recipients should use the lat field value (otherwise this field is preferred).*/ + int32_t lon_int; /*< [degE7] Longitude (higher precision). If 0, recipients should use the lon field value (otherwise this field is preferred).*/ } mavlink_sim_state_t; -#define MAVLINK_MSG_ID_SIM_STATE_LEN 84 +#define MAVLINK_MSG_ID_SIM_STATE_LEN 92 #define MAVLINK_MSG_ID_SIM_STATE_MIN_LEN 84 -#define MAVLINK_MSG_ID_108_LEN 84 +#define MAVLINK_MSG_ID_108_LEN 92 #define MAVLINK_MSG_ID_108_MIN_LEN 84 #define MAVLINK_MSG_ID_SIM_STATE_CRC 32 @@ -42,7 +44,7 @@ typedef struct __mavlink_sim_state_t { #define MAVLINK_MESSAGE_INFO_SIM_STATE { \ 108, \ "SIM_STATE", \ - 21, \ + 23, \ { { "q1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_sim_state_t, q1) }, \ { "q2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_sim_state_t, q2) }, \ { "q3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_sim_state_t, q3) }, \ @@ -64,12 +66,14 @@ typedef struct __mavlink_sim_state_t { { "vn", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_sim_state_t, vn) }, \ { "ve", NULL, MAVLINK_TYPE_FLOAT, 0, 76, offsetof(mavlink_sim_state_t, ve) }, \ { "vd", NULL, MAVLINK_TYPE_FLOAT, 0, 80, offsetof(mavlink_sim_state_t, vd) }, \ + { "lat_int", NULL, MAVLINK_TYPE_INT32_T, 0, 84, offsetof(mavlink_sim_state_t, lat_int) }, \ + { "lon_int", NULL, MAVLINK_TYPE_INT32_T, 0, 88, offsetof(mavlink_sim_state_t, lon_int) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SIM_STATE { \ "SIM_STATE", \ - 21, \ + 23, \ { { "q1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_sim_state_t, q1) }, \ { "q2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_sim_state_t, q2) }, \ { "q3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_sim_state_t, q3) }, \ @@ -91,6 +95,8 @@ typedef struct __mavlink_sim_state_t { { "vn", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_sim_state_t, vn) }, \ { "ve", NULL, MAVLINK_TYPE_FLOAT, 0, 76, offsetof(mavlink_sim_state_t, ve) }, \ { "vd", NULL, MAVLINK_TYPE_FLOAT, 0, 80, offsetof(mavlink_sim_state_t, vd) }, \ + { "lat_int", NULL, MAVLINK_TYPE_INT32_T, 0, 84, offsetof(mavlink_sim_state_t, lat_int) }, \ + { "lon_int", NULL, MAVLINK_TYPE_INT32_T, 0, 88, offsetof(mavlink_sim_state_t, lon_int) }, \ } \ } #endif @@ -105,27 +111,29 @@ typedef struct __mavlink_sim_state_t { * @param q2 True attitude quaternion component 2, x (0 in null-rotation) * @param q3 True attitude quaternion component 3, y (0 in null-rotation) * @param q4 True attitude quaternion component 4, z (0 in null-rotation) - * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs - * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs - * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs + * @param roll [rad] Attitude roll expressed as Euler angles, not recommended except for human-readable outputs + * @param pitch [rad] Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs + * @param yaw [rad] Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs * @param xacc [m/s/s] X acceleration * @param yacc [m/s/s] Y acceleration * @param zacc [m/s/s] Z acceleration * @param xgyro [rad/s] Angular speed around X axis * @param ygyro [rad/s] Angular speed around Y axis * @param zgyro [rad/s] Angular speed around Z axis - * @param lat [deg] Latitude - * @param lon [deg] Longitude + * @param lat [deg] Latitude (lower precision). Both this and the lat_int field should be set. + * @param lon [deg] Longitude (lower precision). Both this and the lon_int field should be set. * @param alt [m] Altitude * @param std_dev_horz Horizontal position standard deviation * @param std_dev_vert Vertical position standard deviation * @param vn [m/s] True velocity in north direction in earth-fixed NED frame * @param ve [m/s] True velocity in east direction in earth-fixed NED frame * @param vd [m/s] True velocity in down direction in earth-fixed NED frame + * @param lat_int [degE7] Latitude (higher precision). If 0, recipients should use the lat field value (otherwise this field is preferred). + * @param lon_int [degE7] Longitude (higher precision). If 0, recipients should use the lon field value (otherwise this field is preferred). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_sim_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd) + float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd, int32_t lat_int, int32_t lon_int) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SIM_STATE_LEN]; @@ -150,6 +158,8 @@ static inline uint16_t mavlink_msg_sim_state_pack(uint8_t system_id, uint8_t com _mav_put_float(buf, 72, vn); _mav_put_float(buf, 76, ve); _mav_put_float(buf, 80, vd); + _mav_put_int32_t(buf, 84, lat_int); + _mav_put_int32_t(buf, 88, lon_int); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIM_STATE_LEN); #else @@ -175,6 +185,8 @@ static inline uint16_t mavlink_msg_sim_state_pack(uint8_t system_id, uint8_t com packet.vn = vn; packet.ve = ve; packet.vd = vd; + packet.lat_int = lat_int; + packet.lon_int = lon_int; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIM_STATE_LEN); #endif @@ -183,6 +195,105 @@ static inline uint16_t mavlink_msg_sim_state_pack(uint8_t system_id, uint8_t com return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SIM_STATE_MIN_LEN, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); } +/** + * @brief Pack a sim_state message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param q1 True attitude quaternion component 1, w (1 in null-rotation) + * @param q2 True attitude quaternion component 2, x (0 in null-rotation) + * @param q3 True attitude quaternion component 3, y (0 in null-rotation) + * @param q4 True attitude quaternion component 4, z (0 in null-rotation) + * @param roll [rad] Attitude roll expressed as Euler angles, not recommended except for human-readable outputs + * @param pitch [rad] Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs + * @param yaw [rad] Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs + * @param xacc [m/s/s] X acceleration + * @param yacc [m/s/s] Y acceleration + * @param zacc [m/s/s] Z acceleration + * @param xgyro [rad/s] Angular speed around X axis + * @param ygyro [rad/s] Angular speed around Y axis + * @param zgyro [rad/s] Angular speed around Z axis + * @param lat [deg] Latitude (lower precision). Both this and the lat_int field should be set. + * @param lon [deg] Longitude (lower precision). Both this and the lon_int field should be set. + * @param alt [m] Altitude + * @param std_dev_horz Horizontal position standard deviation + * @param std_dev_vert Vertical position standard deviation + * @param vn [m/s] True velocity in north direction in earth-fixed NED frame + * @param ve [m/s] True velocity in east direction in earth-fixed NED frame + * @param vd [m/s] True velocity in down direction in earth-fixed NED frame + * @param lat_int [degE7] Latitude (higher precision). If 0, recipients should use the lat field value (otherwise this field is preferred). + * @param lon_int [degE7] Longitude (higher precision). If 0, recipients should use the lon field value (otherwise this field is preferred). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_sim_state_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd, int32_t lat_int, int32_t lon_int) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SIM_STATE_LEN]; + _mav_put_float(buf, 0, q1); + _mav_put_float(buf, 4, q2); + _mav_put_float(buf, 8, q3); + _mav_put_float(buf, 12, q4); + _mav_put_float(buf, 16, roll); + _mav_put_float(buf, 20, pitch); + _mav_put_float(buf, 24, yaw); + _mav_put_float(buf, 28, xacc); + _mav_put_float(buf, 32, yacc); + _mav_put_float(buf, 36, zacc); + _mav_put_float(buf, 40, xgyro); + _mav_put_float(buf, 44, ygyro); + _mav_put_float(buf, 48, zgyro); + _mav_put_float(buf, 52, lat); + _mav_put_float(buf, 56, lon); + _mav_put_float(buf, 60, alt); + _mav_put_float(buf, 64, std_dev_horz); + _mav_put_float(buf, 68, std_dev_vert); + _mav_put_float(buf, 72, vn); + _mav_put_float(buf, 76, ve); + _mav_put_float(buf, 80, vd); + _mav_put_int32_t(buf, 84, lat_int); + _mav_put_int32_t(buf, 88, lon_int); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIM_STATE_LEN); +#else + mavlink_sim_state_t packet; + packet.q1 = q1; + packet.q2 = q2; + packet.q3 = q3; + packet.q4 = q4; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.std_dev_horz = std_dev_horz; + packet.std_dev_vert = std_dev_vert; + packet.vn = vn; + packet.ve = ve; + packet.vd = vd; + packet.lat_int = lat_int; + packet.lon_int = lon_int; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIM_STATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SIM_STATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SIM_STATE_MIN_LEN, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SIM_STATE_MIN_LEN, MAVLINK_MSG_ID_SIM_STATE_LEN); +#endif +} + /** * @brief Pack a sim_state message on a channel * @param system_id ID of this system @@ -193,28 +304,30 @@ static inline uint16_t mavlink_msg_sim_state_pack(uint8_t system_id, uint8_t com * @param q2 True attitude quaternion component 2, x (0 in null-rotation) * @param q3 True attitude quaternion component 3, y (0 in null-rotation) * @param q4 True attitude quaternion component 4, z (0 in null-rotation) - * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs - * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs - * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs + * @param roll [rad] Attitude roll expressed as Euler angles, not recommended except for human-readable outputs + * @param pitch [rad] Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs + * @param yaw [rad] Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs * @param xacc [m/s/s] X acceleration * @param yacc [m/s/s] Y acceleration * @param zacc [m/s/s] Z acceleration * @param xgyro [rad/s] Angular speed around X axis * @param ygyro [rad/s] Angular speed around Y axis * @param zgyro [rad/s] Angular speed around Z axis - * @param lat [deg] Latitude - * @param lon [deg] Longitude + * @param lat [deg] Latitude (lower precision). Both this and the lat_int field should be set. + * @param lon [deg] Longitude (lower precision). Both this and the lon_int field should be set. * @param alt [m] Altitude * @param std_dev_horz Horizontal position standard deviation * @param std_dev_vert Vertical position standard deviation * @param vn [m/s] True velocity in north direction in earth-fixed NED frame * @param ve [m/s] True velocity in east direction in earth-fixed NED frame * @param vd [m/s] True velocity in down direction in earth-fixed NED frame + * @param lat_int [degE7] Latitude (higher precision). If 0, recipients should use the lat field value (otherwise this field is preferred). + * @param lon_int [degE7] Longitude (higher precision). If 0, recipients should use the lon field value (otherwise this field is preferred). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_sim_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - float q1,float q2,float q3,float q4,float roll,float pitch,float yaw,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float lat,float lon,float alt,float std_dev_horz,float std_dev_vert,float vn,float ve,float vd) + float q1,float q2,float q3,float q4,float roll,float pitch,float yaw,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float lat,float lon,float alt,float std_dev_horz,float std_dev_vert,float vn,float ve,float vd,int32_t lat_int,int32_t lon_int) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SIM_STATE_LEN]; @@ -239,6 +352,8 @@ static inline uint16_t mavlink_msg_sim_state_pack_chan(uint8_t system_id, uint8_ _mav_put_float(buf, 72, vn); _mav_put_float(buf, 76, ve); _mav_put_float(buf, 80, vd); + _mav_put_int32_t(buf, 84, lat_int); + _mav_put_int32_t(buf, 88, lon_int); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIM_STATE_LEN); #else @@ -264,6 +379,8 @@ static inline uint16_t mavlink_msg_sim_state_pack_chan(uint8_t system_id, uint8_ packet.vn = vn; packet.ve = ve; packet.vd = vd; + packet.lat_int = lat_int; + packet.lon_int = lon_int; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SIM_STATE_LEN); #endif @@ -282,7 +399,7 @@ static inline uint16_t mavlink_msg_sim_state_pack_chan(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_sim_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sim_state_t* sim_state) { - return mavlink_msg_sim_state_pack(system_id, component_id, msg, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd); + return mavlink_msg_sim_state_pack(system_id, component_id, msg, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd, sim_state->lat_int, sim_state->lon_int); } /** @@ -296,7 +413,21 @@ static inline uint16_t mavlink_msg_sim_state_encode(uint8_t system_id, uint8_t c */ static inline uint16_t mavlink_msg_sim_state_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sim_state_t* sim_state) { - return mavlink_msg_sim_state_pack_chan(system_id, component_id, chan, msg, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd); + return mavlink_msg_sim_state_pack_chan(system_id, component_id, chan, msg, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd, sim_state->lat_int, sim_state->lon_int); +} + +/** + * @brief Encode a sim_state struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param sim_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sim_state_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_sim_state_t* sim_state) +{ + return mavlink_msg_sim_state_pack_status(system_id, component_id, _status, msg, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd, sim_state->lat_int, sim_state->lon_int); } /** @@ -307,27 +438,29 @@ static inline uint16_t mavlink_msg_sim_state_encode_chan(uint8_t system_id, uint * @param q2 True attitude quaternion component 2, x (0 in null-rotation) * @param q3 True attitude quaternion component 3, y (0 in null-rotation) * @param q4 True attitude quaternion component 4, z (0 in null-rotation) - * @param roll Attitude roll expressed as Euler angles, not recommended except for human-readable outputs - * @param pitch Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs - * @param yaw Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs + * @param roll [rad] Attitude roll expressed as Euler angles, not recommended except for human-readable outputs + * @param pitch [rad] Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs + * @param yaw [rad] Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs * @param xacc [m/s/s] X acceleration * @param yacc [m/s/s] Y acceleration * @param zacc [m/s/s] Z acceleration * @param xgyro [rad/s] Angular speed around X axis * @param ygyro [rad/s] Angular speed around Y axis * @param zgyro [rad/s] Angular speed around Z axis - * @param lat [deg] Latitude - * @param lon [deg] Longitude + * @param lat [deg] Latitude (lower precision). Both this and the lat_int field should be set. + * @param lon [deg] Longitude (lower precision). Both this and the lon_int field should be set. * @param alt [m] Altitude * @param std_dev_horz Horizontal position standard deviation * @param std_dev_vert Vertical position standard deviation * @param vn [m/s] True velocity in north direction in earth-fixed NED frame * @param ve [m/s] True velocity in east direction in earth-fixed NED frame * @param vd [m/s] True velocity in down direction in earth-fixed NED frame + * @param lat_int [degE7] Latitude (higher precision). If 0, recipients should use the lat field value (otherwise this field is preferred). + * @param lon_int [degE7] Longitude (higher precision). If 0, recipients should use the lon field value (otherwise this field is preferred). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_sim_state_send(mavlink_channel_t chan, float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd) +static inline void mavlink_msg_sim_state_send(mavlink_channel_t chan, float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd, int32_t lat_int, int32_t lon_int) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SIM_STATE_LEN]; @@ -352,6 +485,8 @@ static inline void mavlink_msg_sim_state_send(mavlink_channel_t chan, float q1, _mav_put_float(buf, 72, vn); _mav_put_float(buf, 76, ve); _mav_put_float(buf, 80, vd); + _mav_put_int32_t(buf, 84, lat_int); + _mav_put_int32_t(buf, 88, lon_int); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_MIN_LEN, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); #else @@ -377,6 +512,8 @@ static inline void mavlink_msg_sim_state_send(mavlink_channel_t chan, float q1, packet.vn = vn; packet.ve = ve; packet.vd = vd; + packet.lat_int = lat_int; + packet.lon_int = lon_int; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)&packet, MAVLINK_MSG_ID_SIM_STATE_MIN_LEN, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); #endif @@ -390,7 +527,7 @@ static inline void mavlink_msg_sim_state_send(mavlink_channel_t chan, float q1, static inline void mavlink_msg_sim_state_send_struct(mavlink_channel_t chan, const mavlink_sim_state_t* sim_state) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_sim_state_send(chan, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd); + mavlink_msg_sim_state_send(chan, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd, sim_state->lat_int, sim_state->lon_int); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)sim_state, MAVLINK_MSG_ID_SIM_STATE_MIN_LEN, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); #endif @@ -398,13 +535,13 @@ static inline void mavlink_msg_sim_state_send_struct(mavlink_channel_t chan, con #if MAVLINK_MSG_ID_SIM_STATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_sim_state_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd) +static inline void mavlink_msg_sim_state_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd, int32_t lat_int, int32_t lon_int) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -429,6 +566,8 @@ static inline void mavlink_msg_sim_state_send_buf(mavlink_message_t *msgbuf, mav _mav_put_float(buf, 72, vn); _mav_put_float(buf, 76, ve); _mav_put_float(buf, 80, vd); + _mav_put_int32_t(buf, 84, lat_int); + _mav_put_int32_t(buf, 88, lon_int); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_MIN_LEN, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); #else @@ -454,6 +593,8 @@ static inline void mavlink_msg_sim_state_send_buf(mavlink_message_t *msgbuf, mav packet->vn = vn; packet->ve = ve; packet->vd = vd; + packet->lat_int = lat_int; + packet->lon_int = lon_int; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)packet, MAVLINK_MSG_ID_SIM_STATE_MIN_LEN, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); #endif @@ -508,7 +649,7 @@ static inline float mavlink_msg_sim_state_get_q4(const mavlink_message_t* msg) /** * @brief Get field roll from sim_state message * - * @return Attitude roll expressed as Euler angles, not recommended except for human-readable outputs + * @return [rad] Attitude roll expressed as Euler angles, not recommended except for human-readable outputs */ static inline float mavlink_msg_sim_state_get_roll(const mavlink_message_t* msg) { @@ -518,7 +659,7 @@ static inline float mavlink_msg_sim_state_get_roll(const mavlink_message_t* msg) /** * @brief Get field pitch from sim_state message * - * @return Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs + * @return [rad] Attitude pitch expressed as Euler angles, not recommended except for human-readable outputs */ static inline float mavlink_msg_sim_state_get_pitch(const mavlink_message_t* msg) { @@ -528,7 +669,7 @@ static inline float mavlink_msg_sim_state_get_pitch(const mavlink_message_t* msg /** * @brief Get field yaw from sim_state message * - * @return Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs + * @return [rad] Attitude yaw expressed as Euler angles, not recommended except for human-readable outputs */ static inline float mavlink_msg_sim_state_get_yaw(const mavlink_message_t* msg) { @@ -598,7 +739,7 @@ static inline float mavlink_msg_sim_state_get_zgyro(const mavlink_message_t* msg /** * @brief Get field lat from sim_state message * - * @return [deg] Latitude + * @return [deg] Latitude (lower precision). Both this and the lat_int field should be set. */ static inline float mavlink_msg_sim_state_get_lat(const mavlink_message_t* msg) { @@ -608,7 +749,7 @@ static inline float mavlink_msg_sim_state_get_lat(const mavlink_message_t* msg) /** * @brief Get field lon from sim_state message * - * @return [deg] Longitude + * @return [deg] Longitude (lower precision). Both this and the lon_int field should be set. */ static inline float mavlink_msg_sim_state_get_lon(const mavlink_message_t* msg) { @@ -675,6 +816,26 @@ static inline float mavlink_msg_sim_state_get_vd(const mavlink_message_t* msg) return _MAV_RETURN_float(msg, 80); } +/** + * @brief Get field lat_int from sim_state message + * + * @return [degE7] Latitude (higher precision). If 0, recipients should use the lat field value (otherwise this field is preferred). + */ +static inline int32_t mavlink_msg_sim_state_get_lat_int(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 84); +} + +/** + * @brief Get field lon_int from sim_state message + * + * @return [degE7] Longitude (higher precision). If 0, recipients should use the lon field value (otherwise this field is preferred). + */ +static inline int32_t mavlink_msg_sim_state_get_lon_int(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 88); +} + /** * @brief Decode a sim_state message into a struct * @@ -705,6 +866,8 @@ static inline void mavlink_msg_sim_state_decode(const mavlink_message_t* msg, ma sim_state->vn = mavlink_msg_sim_state_get_vn(msg); sim_state->ve = mavlink_msg_sim_state_get_ve(msg); sim_state->vd = mavlink_msg_sim_state_get_vd(msg); + sim_state->lat_int = mavlink_msg_sim_state_get_lat_int(msg); + sim_state->lon_int = mavlink_msg_sim_state_get_lon_int(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SIM_STATE_LEN? msg->len : MAVLINK_MSG_ID_SIM_STATE_LEN; memset(sim_state, 0, MAVLINK_MSG_ID_SIM_STATE_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_smart_battery_info.h b/lib/main/MAVLink/common/mavlink_msg_smart_battery_info.h index 174c197f31a..94810e7dc6b 100644 --- a/lib/main/MAVLink/common/mavlink_msg_smart_battery_info.h +++ b/lib/main/MAVLink/common/mavlink_msg_smart_battery_info.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_SMART_BATTERY_INFO 370 - +MAVPACKED( typedef struct __mavlink_smart_battery_info_t { int32_t capacity_full_specification; /*< [mAh] Capacity when full according to manufacturer, -1: field not provided.*/ int32_t capacity_full; /*< [mAh] Capacity when full (accounting for battery degradation), -1: field not provided.*/ @@ -16,12 +16,17 @@ typedef struct __mavlink_smart_battery_info_t { uint8_t battery_function; /*< Function of the battery*/ uint8_t type; /*< Type (chemistry) of the battery*/ char serial_number[16]; /*< Serial number in ASCII characters, 0 terminated. All 0: field not provided.*/ - char device_name[50]; /*< Static device name. Encode as manufacturer and product names separated using an underscore.*/ -} mavlink_smart_battery_info_t; - -#define MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN 87 + char device_name[50]; /*< Static device name in ASCII characters, 0 terminated. All 0: field not provided. Encode as manufacturer name then product name separated using an underscore.*/ + uint16_t charging_maximum_voltage; /*< [mV] Maximum per-cell voltage when charged. 0: field not provided.*/ + uint8_t cells_in_series; /*< Number of battery cells in series. 0: field not provided.*/ + uint32_t discharge_maximum_current; /*< [mA] Maximum pack discharge current. 0: field not provided.*/ + uint32_t discharge_maximum_burst_current; /*< [mA] Maximum pack discharge burst current. 0: field not provided.*/ + char manufacture_date[11]; /*< Manufacture date (DD/MM/YYYY) in ASCII characters, 0 terminated. All 0: field not provided.*/ +}) mavlink_smart_battery_info_t; + +#define MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN 109 #define MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN 87 -#define MAVLINK_MSG_ID_370_LEN 87 +#define MAVLINK_MSG_ID_370_LEN 109 #define MAVLINK_MSG_ID_370_MIN_LEN 87 #define MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC 75 @@ -29,12 +34,13 @@ typedef struct __mavlink_smart_battery_info_t { #define MAVLINK_MSG_SMART_BATTERY_INFO_FIELD_SERIAL_NUMBER_LEN 16 #define MAVLINK_MSG_SMART_BATTERY_INFO_FIELD_DEVICE_NAME_LEN 50 +#define MAVLINK_MSG_SMART_BATTERY_INFO_FIELD_MANUFACTURE_DATE_LEN 11 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_SMART_BATTERY_INFO { \ 370, \ "SMART_BATTERY_INFO", \ - 12, \ + 17, \ { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_smart_battery_info_t, id) }, \ { "battery_function", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_smart_battery_info_t, battery_function) }, \ { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_smart_battery_info_t, type) }, \ @@ -47,12 +53,17 @@ typedef struct __mavlink_smart_battery_info_t { { "discharge_minimum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_smart_battery_info_t, discharge_minimum_voltage) }, \ { "charging_minimum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_smart_battery_info_t, charging_minimum_voltage) }, \ { "resting_minimum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_smart_battery_info_t, resting_minimum_voltage) }, \ + { "charging_maximum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 87, offsetof(mavlink_smart_battery_info_t, charging_maximum_voltage) }, \ + { "cells_in_series", NULL, MAVLINK_TYPE_UINT8_T, 0, 89, offsetof(mavlink_smart_battery_info_t, cells_in_series) }, \ + { "discharge_maximum_current", NULL, MAVLINK_TYPE_UINT32_T, 0, 90, offsetof(mavlink_smart_battery_info_t, discharge_maximum_current) }, \ + { "discharge_maximum_burst_current", NULL, MAVLINK_TYPE_UINT32_T, 0, 94, offsetof(mavlink_smart_battery_info_t, discharge_maximum_burst_current) }, \ + { "manufacture_date", NULL, MAVLINK_TYPE_CHAR, 11, 98, offsetof(mavlink_smart_battery_info_t, manufacture_date) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SMART_BATTERY_INFO { \ "SMART_BATTERY_INFO", \ - 12, \ + 17, \ { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_smart_battery_info_t, id) }, \ { "battery_function", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_smart_battery_info_t, battery_function) }, \ { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_smart_battery_info_t, type) }, \ @@ -65,6 +76,11 @@ typedef struct __mavlink_smart_battery_info_t { { "discharge_minimum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_smart_battery_info_t, discharge_minimum_voltage) }, \ { "charging_minimum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_smart_battery_info_t, charging_minimum_voltage) }, \ { "resting_minimum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_smart_battery_info_t, resting_minimum_voltage) }, \ + { "charging_maximum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 87, offsetof(mavlink_smart_battery_info_t, charging_maximum_voltage) }, \ + { "cells_in_series", NULL, MAVLINK_TYPE_UINT8_T, 0, 89, offsetof(mavlink_smart_battery_info_t, cells_in_series) }, \ + { "discharge_maximum_current", NULL, MAVLINK_TYPE_UINT32_T, 0, 90, offsetof(mavlink_smart_battery_info_t, discharge_maximum_current) }, \ + { "discharge_maximum_burst_current", NULL, MAVLINK_TYPE_UINT32_T, 0, 94, offsetof(mavlink_smart_battery_info_t, discharge_maximum_burst_current) }, \ + { "manufacture_date", NULL, MAVLINK_TYPE_CHAR, 11, 98, offsetof(mavlink_smart_battery_info_t, manufacture_date) }, \ } \ } #endif @@ -82,15 +98,20 @@ typedef struct __mavlink_smart_battery_info_t { * @param capacity_full [mAh] Capacity when full (accounting for battery degradation), -1: field not provided. * @param cycle_count Charge/discharge cycle count. UINT16_MAX: field not provided. * @param serial_number Serial number in ASCII characters, 0 terminated. All 0: field not provided. - * @param device_name Static device name. Encode as manufacturer and product names separated using an underscore. + * @param device_name Static device name in ASCII characters, 0 terminated. All 0: field not provided. Encode as manufacturer name then product name separated using an underscore. * @param weight [g] Battery weight. 0: field not provided. * @param discharge_minimum_voltage [mV] Minimum per-cell voltage when discharging. If not supplied set to UINT16_MAX value. * @param charging_minimum_voltage [mV] Minimum per-cell voltage when charging. If not supplied set to UINT16_MAX value. * @param resting_minimum_voltage [mV] Minimum per-cell voltage when resting. If not supplied set to UINT16_MAX value. + * @param charging_maximum_voltage [mV] Maximum per-cell voltage when charged. 0: field not provided. + * @param cells_in_series Number of battery cells in series. 0: field not provided. + * @param discharge_maximum_current [mA] Maximum pack discharge current. 0: field not provided. + * @param discharge_maximum_burst_current [mA] Maximum pack discharge burst current. 0: field not provided. + * @param manufacture_date Manufacture date (DD/MM/YYYY) in ASCII characters, 0 terminated. All 0: field not provided. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_smart_battery_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t id, uint8_t battery_function, uint8_t type, int32_t capacity_full_specification, int32_t capacity_full, uint16_t cycle_count, const char *serial_number, const char *device_name, uint16_t weight, uint16_t discharge_minimum_voltage, uint16_t charging_minimum_voltage, uint16_t resting_minimum_voltage) + uint8_t id, uint8_t battery_function, uint8_t type, int32_t capacity_full_specification, int32_t capacity_full, uint16_t cycle_count, const char *serial_number, const char *device_name, uint16_t weight, uint16_t discharge_minimum_voltage, uint16_t charging_minimum_voltage, uint16_t resting_minimum_voltage, uint16_t charging_maximum_voltage, uint8_t cells_in_series, uint32_t discharge_maximum_current, uint32_t discharge_maximum_burst_current, const char *manufacture_date) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN]; @@ -104,8 +125,13 @@ static inline uint16_t mavlink_msg_smart_battery_info_pack(uint8_t system_id, ui _mav_put_uint8_t(buf, 18, id); _mav_put_uint8_t(buf, 19, battery_function); _mav_put_uint8_t(buf, 20, type); + _mav_put_uint16_t(buf, 87, charging_maximum_voltage); + _mav_put_uint8_t(buf, 89, cells_in_series); + _mav_put_uint32_t(buf, 90, discharge_maximum_current); + _mav_put_uint32_t(buf, 94, discharge_maximum_burst_current); _mav_put_char_array(buf, 21, serial_number, 16); _mav_put_char_array(buf, 37, device_name, 50); + _mav_put_char_array(buf, 98, manufacture_date, 11); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN); #else mavlink_smart_battery_info_t packet; @@ -119,13 +145,97 @@ static inline uint16_t mavlink_msg_smart_battery_info_pack(uint8_t system_id, ui packet.id = id; packet.battery_function = battery_function; packet.type = type; + packet.charging_maximum_voltage = charging_maximum_voltage; + packet.cells_in_series = cells_in_series; + packet.discharge_maximum_current = discharge_maximum_current; + packet.discharge_maximum_burst_current = discharge_maximum_burst_current; + mav_array_assign_char(packet.serial_number, serial_number, 16); + mav_array_assign_char(packet.device_name, device_name, 50); + mav_array_assign_char(packet.manufacture_date, manufacture_date, 11); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SMART_BATTERY_INFO; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); +} + +/** + * @brief Pack a smart_battery_info message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param id Battery ID + * @param battery_function Function of the battery + * @param type Type (chemistry) of the battery + * @param capacity_full_specification [mAh] Capacity when full according to manufacturer, -1: field not provided. + * @param capacity_full [mAh] Capacity when full (accounting for battery degradation), -1: field not provided. + * @param cycle_count Charge/discharge cycle count. UINT16_MAX: field not provided. + * @param serial_number Serial number in ASCII characters, 0 terminated. All 0: field not provided. + * @param device_name Static device name in ASCII characters, 0 terminated. All 0: field not provided. Encode as manufacturer name then product name separated using an underscore. + * @param weight [g] Battery weight. 0: field not provided. + * @param discharge_minimum_voltage [mV] Minimum per-cell voltage when discharging. If not supplied set to UINT16_MAX value. + * @param charging_minimum_voltage [mV] Minimum per-cell voltage when charging. If not supplied set to UINT16_MAX value. + * @param resting_minimum_voltage [mV] Minimum per-cell voltage when resting. If not supplied set to UINT16_MAX value. + * @param charging_maximum_voltage [mV] Maximum per-cell voltage when charged. 0: field not provided. + * @param cells_in_series Number of battery cells in series. 0: field not provided. + * @param discharge_maximum_current [mA] Maximum pack discharge current. 0: field not provided. + * @param discharge_maximum_burst_current [mA] Maximum pack discharge burst current. 0: field not provided. + * @param manufacture_date Manufacture date (DD/MM/YYYY) in ASCII characters, 0 terminated. All 0: field not provided. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_smart_battery_info_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t id, uint8_t battery_function, uint8_t type, int32_t capacity_full_specification, int32_t capacity_full, uint16_t cycle_count, const char *serial_number, const char *device_name, uint16_t weight, uint16_t discharge_minimum_voltage, uint16_t charging_minimum_voltage, uint16_t resting_minimum_voltage, uint16_t charging_maximum_voltage, uint8_t cells_in_series, uint32_t discharge_maximum_current, uint32_t discharge_maximum_burst_current, const char *manufacture_date) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN]; + _mav_put_int32_t(buf, 0, capacity_full_specification); + _mav_put_int32_t(buf, 4, capacity_full); + _mav_put_uint16_t(buf, 8, cycle_count); + _mav_put_uint16_t(buf, 10, weight); + _mav_put_uint16_t(buf, 12, discharge_minimum_voltage); + _mav_put_uint16_t(buf, 14, charging_minimum_voltage); + _mav_put_uint16_t(buf, 16, resting_minimum_voltage); + _mav_put_uint8_t(buf, 18, id); + _mav_put_uint8_t(buf, 19, battery_function); + _mav_put_uint8_t(buf, 20, type); + _mav_put_uint16_t(buf, 87, charging_maximum_voltage); + _mav_put_uint8_t(buf, 89, cells_in_series); + _mav_put_uint32_t(buf, 90, discharge_maximum_current); + _mav_put_uint32_t(buf, 94, discharge_maximum_burst_current); + _mav_put_char_array(buf, 21, serial_number, 16); + _mav_put_char_array(buf, 37, device_name, 50); + _mav_put_char_array(buf, 98, manufacture_date, 11); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN); +#else + mavlink_smart_battery_info_t packet; + packet.capacity_full_specification = capacity_full_specification; + packet.capacity_full = capacity_full; + packet.cycle_count = cycle_count; + packet.weight = weight; + packet.discharge_minimum_voltage = discharge_minimum_voltage; + packet.charging_minimum_voltage = charging_minimum_voltage; + packet.resting_minimum_voltage = resting_minimum_voltage; + packet.id = id; + packet.battery_function = battery_function; + packet.type = type; + packet.charging_maximum_voltage = charging_maximum_voltage; + packet.cells_in_series = cells_in_series; + packet.discharge_maximum_current = discharge_maximum_current; + packet.discharge_maximum_burst_current = discharge_maximum_burst_current; mav_array_memcpy(packet.serial_number, serial_number, sizeof(char)*16); mav_array_memcpy(packet.device_name, device_name, sizeof(char)*50); + mav_array_memcpy(packet.manufacture_date, manufacture_date, sizeof(char)*11); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN); #endif msg->msgid = MAVLINK_MSG_ID_SMART_BATTERY_INFO; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN); +#endif } /** @@ -141,16 +251,21 @@ static inline uint16_t mavlink_msg_smart_battery_info_pack(uint8_t system_id, ui * @param capacity_full [mAh] Capacity when full (accounting for battery degradation), -1: field not provided. * @param cycle_count Charge/discharge cycle count. UINT16_MAX: field not provided. * @param serial_number Serial number in ASCII characters, 0 terminated. All 0: field not provided. - * @param device_name Static device name. Encode as manufacturer and product names separated using an underscore. + * @param device_name Static device name in ASCII characters, 0 terminated. All 0: field not provided. Encode as manufacturer name then product name separated using an underscore. * @param weight [g] Battery weight. 0: field not provided. * @param discharge_minimum_voltage [mV] Minimum per-cell voltage when discharging. If not supplied set to UINT16_MAX value. * @param charging_minimum_voltage [mV] Minimum per-cell voltage when charging. If not supplied set to UINT16_MAX value. * @param resting_minimum_voltage [mV] Minimum per-cell voltage when resting. If not supplied set to UINT16_MAX value. + * @param charging_maximum_voltage [mV] Maximum per-cell voltage when charged. 0: field not provided. + * @param cells_in_series Number of battery cells in series. 0: field not provided. + * @param discharge_maximum_current [mA] Maximum pack discharge current. 0: field not provided. + * @param discharge_maximum_burst_current [mA] Maximum pack discharge burst current. 0: field not provided. + * @param manufacture_date Manufacture date (DD/MM/YYYY) in ASCII characters, 0 terminated. All 0: field not provided. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_smart_battery_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t id,uint8_t battery_function,uint8_t type,int32_t capacity_full_specification,int32_t capacity_full,uint16_t cycle_count,const char *serial_number,const char *device_name,uint16_t weight,uint16_t discharge_minimum_voltage,uint16_t charging_minimum_voltage,uint16_t resting_minimum_voltage) + uint8_t id,uint8_t battery_function,uint8_t type,int32_t capacity_full_specification,int32_t capacity_full,uint16_t cycle_count,const char *serial_number,const char *device_name,uint16_t weight,uint16_t discharge_minimum_voltage,uint16_t charging_minimum_voltage,uint16_t resting_minimum_voltage,uint16_t charging_maximum_voltage,uint8_t cells_in_series,uint32_t discharge_maximum_current,uint32_t discharge_maximum_burst_current,const char *manufacture_date) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN]; @@ -164,8 +279,13 @@ static inline uint16_t mavlink_msg_smart_battery_info_pack_chan(uint8_t system_i _mav_put_uint8_t(buf, 18, id); _mav_put_uint8_t(buf, 19, battery_function); _mav_put_uint8_t(buf, 20, type); + _mav_put_uint16_t(buf, 87, charging_maximum_voltage); + _mav_put_uint8_t(buf, 89, cells_in_series); + _mav_put_uint32_t(buf, 90, discharge_maximum_current); + _mav_put_uint32_t(buf, 94, discharge_maximum_burst_current); _mav_put_char_array(buf, 21, serial_number, 16); _mav_put_char_array(buf, 37, device_name, 50); + _mav_put_char_array(buf, 98, manufacture_date, 11); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN); #else mavlink_smart_battery_info_t packet; @@ -179,8 +299,13 @@ static inline uint16_t mavlink_msg_smart_battery_info_pack_chan(uint8_t system_i packet.id = id; packet.battery_function = battery_function; packet.type = type; - mav_array_memcpy(packet.serial_number, serial_number, sizeof(char)*16); - mav_array_memcpy(packet.device_name, device_name, sizeof(char)*50); + packet.charging_maximum_voltage = charging_maximum_voltage; + packet.cells_in_series = cells_in_series; + packet.discharge_maximum_current = discharge_maximum_current; + packet.discharge_maximum_burst_current = discharge_maximum_burst_current; + mav_array_assign_char(packet.serial_number, serial_number, 16); + mav_array_assign_char(packet.device_name, device_name, 50); + mav_array_assign_char(packet.manufacture_date, manufacture_date, 11); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN); #endif @@ -198,7 +323,7 @@ static inline uint16_t mavlink_msg_smart_battery_info_pack_chan(uint8_t system_i */ static inline uint16_t mavlink_msg_smart_battery_info_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_smart_battery_info_t* smart_battery_info) { - return mavlink_msg_smart_battery_info_pack(system_id, component_id, msg, smart_battery_info->id, smart_battery_info->battery_function, smart_battery_info->type, smart_battery_info->capacity_full_specification, smart_battery_info->capacity_full, smart_battery_info->cycle_count, smart_battery_info->serial_number, smart_battery_info->device_name, smart_battery_info->weight, smart_battery_info->discharge_minimum_voltage, smart_battery_info->charging_minimum_voltage, smart_battery_info->resting_minimum_voltage); + return mavlink_msg_smart_battery_info_pack(system_id, component_id, msg, smart_battery_info->id, smart_battery_info->battery_function, smart_battery_info->type, smart_battery_info->capacity_full_specification, smart_battery_info->capacity_full, smart_battery_info->cycle_count, smart_battery_info->serial_number, smart_battery_info->device_name, smart_battery_info->weight, smart_battery_info->discharge_minimum_voltage, smart_battery_info->charging_minimum_voltage, smart_battery_info->resting_minimum_voltage, smart_battery_info->charging_maximum_voltage, smart_battery_info->cells_in_series, smart_battery_info->discharge_maximum_current, smart_battery_info->discharge_maximum_burst_current, smart_battery_info->manufacture_date); } /** @@ -212,7 +337,21 @@ static inline uint16_t mavlink_msg_smart_battery_info_encode(uint8_t system_id, */ static inline uint16_t mavlink_msg_smart_battery_info_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_smart_battery_info_t* smart_battery_info) { - return mavlink_msg_smart_battery_info_pack_chan(system_id, component_id, chan, msg, smart_battery_info->id, smart_battery_info->battery_function, smart_battery_info->type, smart_battery_info->capacity_full_specification, smart_battery_info->capacity_full, smart_battery_info->cycle_count, smart_battery_info->serial_number, smart_battery_info->device_name, smart_battery_info->weight, smart_battery_info->discharge_minimum_voltage, smart_battery_info->charging_minimum_voltage, smart_battery_info->resting_minimum_voltage); + return mavlink_msg_smart_battery_info_pack_chan(system_id, component_id, chan, msg, smart_battery_info->id, smart_battery_info->battery_function, smart_battery_info->type, smart_battery_info->capacity_full_specification, smart_battery_info->capacity_full, smart_battery_info->cycle_count, smart_battery_info->serial_number, smart_battery_info->device_name, smart_battery_info->weight, smart_battery_info->discharge_minimum_voltage, smart_battery_info->charging_minimum_voltage, smart_battery_info->resting_minimum_voltage, smart_battery_info->charging_maximum_voltage, smart_battery_info->cells_in_series, smart_battery_info->discharge_maximum_current, smart_battery_info->discharge_maximum_burst_current, smart_battery_info->manufacture_date); +} + +/** + * @brief Encode a smart_battery_info struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param smart_battery_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_smart_battery_info_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_smart_battery_info_t* smart_battery_info) +{ + return mavlink_msg_smart_battery_info_pack_status(system_id, component_id, _status, msg, smart_battery_info->id, smart_battery_info->battery_function, smart_battery_info->type, smart_battery_info->capacity_full_specification, smart_battery_info->capacity_full, smart_battery_info->cycle_count, smart_battery_info->serial_number, smart_battery_info->device_name, smart_battery_info->weight, smart_battery_info->discharge_minimum_voltage, smart_battery_info->charging_minimum_voltage, smart_battery_info->resting_minimum_voltage, smart_battery_info->charging_maximum_voltage, smart_battery_info->cells_in_series, smart_battery_info->discharge_maximum_current, smart_battery_info->discharge_maximum_burst_current, smart_battery_info->manufacture_date); } /** @@ -226,15 +365,20 @@ static inline uint16_t mavlink_msg_smart_battery_info_encode_chan(uint8_t system * @param capacity_full [mAh] Capacity when full (accounting for battery degradation), -1: field not provided. * @param cycle_count Charge/discharge cycle count. UINT16_MAX: field not provided. * @param serial_number Serial number in ASCII characters, 0 terminated. All 0: field not provided. - * @param device_name Static device name. Encode as manufacturer and product names separated using an underscore. + * @param device_name Static device name in ASCII characters, 0 terminated. All 0: field not provided. Encode as manufacturer name then product name separated using an underscore. * @param weight [g] Battery weight. 0: field not provided. * @param discharge_minimum_voltage [mV] Minimum per-cell voltage when discharging. If not supplied set to UINT16_MAX value. * @param charging_minimum_voltage [mV] Minimum per-cell voltage when charging. If not supplied set to UINT16_MAX value. * @param resting_minimum_voltage [mV] Minimum per-cell voltage when resting. If not supplied set to UINT16_MAX value. + * @param charging_maximum_voltage [mV] Maximum per-cell voltage when charged. 0: field not provided. + * @param cells_in_series Number of battery cells in series. 0: field not provided. + * @param discharge_maximum_current [mA] Maximum pack discharge current. 0: field not provided. + * @param discharge_maximum_burst_current [mA] Maximum pack discharge burst current. 0: field not provided. + * @param manufacture_date Manufacture date (DD/MM/YYYY) in ASCII characters, 0 terminated. All 0: field not provided. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_smart_battery_info_send(mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int32_t capacity_full_specification, int32_t capacity_full, uint16_t cycle_count, const char *serial_number, const char *device_name, uint16_t weight, uint16_t discharge_minimum_voltage, uint16_t charging_minimum_voltage, uint16_t resting_minimum_voltage) +static inline void mavlink_msg_smart_battery_info_send(mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int32_t capacity_full_specification, int32_t capacity_full, uint16_t cycle_count, const char *serial_number, const char *device_name, uint16_t weight, uint16_t discharge_minimum_voltage, uint16_t charging_minimum_voltage, uint16_t resting_minimum_voltage, uint16_t charging_maximum_voltage, uint8_t cells_in_series, uint32_t discharge_maximum_current, uint32_t discharge_maximum_burst_current, const char *manufacture_date) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN]; @@ -248,8 +392,13 @@ static inline void mavlink_msg_smart_battery_info_send(mavlink_channel_t chan, u _mav_put_uint8_t(buf, 18, id); _mav_put_uint8_t(buf, 19, battery_function); _mav_put_uint8_t(buf, 20, type); + _mav_put_uint16_t(buf, 87, charging_maximum_voltage); + _mav_put_uint8_t(buf, 89, cells_in_series); + _mav_put_uint32_t(buf, 90, discharge_maximum_current); + _mav_put_uint32_t(buf, 94, discharge_maximum_burst_current); _mav_put_char_array(buf, 21, serial_number, 16); _mav_put_char_array(buf, 37, device_name, 50); + _mav_put_char_array(buf, 98, manufacture_date, 11); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMART_BATTERY_INFO, buf, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); #else mavlink_smart_battery_info_t packet; @@ -263,8 +412,13 @@ static inline void mavlink_msg_smart_battery_info_send(mavlink_channel_t chan, u packet.id = id; packet.battery_function = battery_function; packet.type = type; - mav_array_memcpy(packet.serial_number, serial_number, sizeof(char)*16); - mav_array_memcpy(packet.device_name, device_name, sizeof(char)*50); + packet.charging_maximum_voltage = charging_maximum_voltage; + packet.cells_in_series = cells_in_series; + packet.discharge_maximum_current = discharge_maximum_current; + packet.discharge_maximum_burst_current = discharge_maximum_burst_current; + mav_array_assign_char(packet.serial_number, serial_number, 16); + mav_array_assign_char(packet.device_name, device_name, 50); + mav_array_assign_char(packet.manufacture_date, manufacture_date, 11); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMART_BATTERY_INFO, (const char *)&packet, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); #endif } @@ -277,7 +431,7 @@ static inline void mavlink_msg_smart_battery_info_send(mavlink_channel_t chan, u static inline void mavlink_msg_smart_battery_info_send_struct(mavlink_channel_t chan, const mavlink_smart_battery_info_t* smart_battery_info) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_smart_battery_info_send(chan, smart_battery_info->id, smart_battery_info->battery_function, smart_battery_info->type, smart_battery_info->capacity_full_specification, smart_battery_info->capacity_full, smart_battery_info->cycle_count, smart_battery_info->serial_number, smart_battery_info->device_name, smart_battery_info->weight, smart_battery_info->discharge_minimum_voltage, smart_battery_info->charging_minimum_voltage, smart_battery_info->resting_minimum_voltage); + mavlink_msg_smart_battery_info_send(chan, smart_battery_info->id, smart_battery_info->battery_function, smart_battery_info->type, smart_battery_info->capacity_full_specification, smart_battery_info->capacity_full, smart_battery_info->cycle_count, smart_battery_info->serial_number, smart_battery_info->device_name, smart_battery_info->weight, smart_battery_info->discharge_minimum_voltage, smart_battery_info->charging_minimum_voltage, smart_battery_info->resting_minimum_voltage, smart_battery_info->charging_maximum_voltage, smart_battery_info->cells_in_series, smart_battery_info->discharge_maximum_current, smart_battery_info->discharge_maximum_burst_current, smart_battery_info->manufacture_date); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMART_BATTERY_INFO, (const char *)smart_battery_info, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); #endif @@ -285,13 +439,13 @@ static inline void mavlink_msg_smart_battery_info_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_smart_battery_info_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int32_t capacity_full_specification, int32_t capacity_full, uint16_t cycle_count, const char *serial_number, const char *device_name, uint16_t weight, uint16_t discharge_minimum_voltage, uint16_t charging_minimum_voltage, uint16_t resting_minimum_voltage) +static inline void mavlink_msg_smart_battery_info_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int32_t capacity_full_specification, int32_t capacity_full, uint16_t cycle_count, const char *serial_number, const char *device_name, uint16_t weight, uint16_t discharge_minimum_voltage, uint16_t charging_minimum_voltage, uint16_t resting_minimum_voltage, uint16_t charging_maximum_voltage, uint8_t cells_in_series, uint32_t discharge_maximum_current, uint32_t discharge_maximum_burst_current, const char *manufacture_date) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -305,8 +459,13 @@ static inline void mavlink_msg_smart_battery_info_send_buf(mavlink_message_t *ms _mav_put_uint8_t(buf, 18, id); _mav_put_uint8_t(buf, 19, battery_function); _mav_put_uint8_t(buf, 20, type); + _mav_put_uint16_t(buf, 87, charging_maximum_voltage); + _mav_put_uint8_t(buf, 89, cells_in_series); + _mav_put_uint32_t(buf, 90, discharge_maximum_current); + _mav_put_uint32_t(buf, 94, discharge_maximum_burst_current); _mav_put_char_array(buf, 21, serial_number, 16); _mav_put_char_array(buf, 37, device_name, 50); + _mav_put_char_array(buf, 98, manufacture_date, 11); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMART_BATTERY_INFO, buf, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); #else mavlink_smart_battery_info_t *packet = (mavlink_smart_battery_info_t *)msgbuf; @@ -320,8 +479,13 @@ static inline void mavlink_msg_smart_battery_info_send_buf(mavlink_message_t *ms packet->id = id; packet->battery_function = battery_function; packet->type = type; - mav_array_memcpy(packet->serial_number, serial_number, sizeof(char)*16); - mav_array_memcpy(packet->device_name, device_name, sizeof(char)*50); + packet->charging_maximum_voltage = charging_maximum_voltage; + packet->cells_in_series = cells_in_series; + packet->discharge_maximum_current = discharge_maximum_current; + packet->discharge_maximum_burst_current = discharge_maximum_burst_current; + mav_array_assign_char(packet->serial_number, serial_number, 16); + mav_array_assign_char(packet->device_name, device_name, 50); + mav_array_assign_char(packet->manufacture_date, manufacture_date, 11); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMART_BATTERY_INFO, (const char *)packet, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); #endif } @@ -405,7 +569,7 @@ static inline uint16_t mavlink_msg_smart_battery_info_get_serial_number(const ma /** * @brief Get field device_name from smart_battery_info message * - * @return Static device name. Encode as manufacturer and product names separated using an underscore. + * @return Static device name in ASCII characters, 0 terminated. All 0: field not provided. Encode as manufacturer name then product name separated using an underscore. */ static inline uint16_t mavlink_msg_smart_battery_info_get_device_name(const mavlink_message_t* msg, char *device_name) { @@ -452,6 +616,56 @@ static inline uint16_t mavlink_msg_smart_battery_info_get_resting_minimum_voltag return _MAV_RETURN_uint16_t(msg, 16); } +/** + * @brief Get field charging_maximum_voltage from smart_battery_info message + * + * @return [mV] Maximum per-cell voltage when charged. 0: field not provided. + */ +static inline uint16_t mavlink_msg_smart_battery_info_get_charging_maximum_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 87); +} + +/** + * @brief Get field cells_in_series from smart_battery_info message + * + * @return Number of battery cells in series. 0: field not provided. + */ +static inline uint8_t mavlink_msg_smart_battery_info_get_cells_in_series(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 89); +} + +/** + * @brief Get field discharge_maximum_current from smart_battery_info message + * + * @return [mA] Maximum pack discharge current. 0: field not provided. + */ +static inline uint32_t mavlink_msg_smart_battery_info_get_discharge_maximum_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 90); +} + +/** + * @brief Get field discharge_maximum_burst_current from smart_battery_info message + * + * @return [mA] Maximum pack discharge burst current. 0: field not provided. + */ +static inline uint32_t mavlink_msg_smart_battery_info_get_discharge_maximum_burst_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 94); +} + +/** + * @brief Get field manufacture_date from smart_battery_info message + * + * @return Manufacture date (DD/MM/YYYY) in ASCII characters, 0 terminated. All 0: field not provided. + */ +static inline uint16_t mavlink_msg_smart_battery_info_get_manufacture_date(const mavlink_message_t* msg, char *manufacture_date) +{ + return _MAV_RETURN_char_array(msg, manufacture_date, 11, 98); +} + /** * @brief Decode a smart_battery_info message into a struct * @@ -473,6 +687,11 @@ static inline void mavlink_msg_smart_battery_info_decode(const mavlink_message_t smart_battery_info->type = mavlink_msg_smart_battery_info_get_type(msg); mavlink_msg_smart_battery_info_get_serial_number(msg, smart_battery_info->serial_number); mavlink_msg_smart_battery_info_get_device_name(msg, smart_battery_info->device_name); + smart_battery_info->charging_maximum_voltage = mavlink_msg_smart_battery_info_get_charging_maximum_voltage(msg); + smart_battery_info->cells_in_series = mavlink_msg_smart_battery_info_get_cells_in_series(msg); + smart_battery_info->discharge_maximum_current = mavlink_msg_smart_battery_info_get_discharge_maximum_current(msg); + smart_battery_info->discharge_maximum_burst_current = mavlink_msg_smart_battery_info_get_discharge_maximum_burst_current(msg); + mavlink_msg_smart_battery_info_get_manufacture_date(msg, smart_battery_info->manufacture_date); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN? msg->len : MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN; memset(smart_battery_info, 0, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_statustext.h b/lib/main/MAVLink/common/mavlink_msg_statustext.h index 28e0c0b0fd8..57facb8a5d2 100755 --- a/lib/main/MAVLink/common/mavlink_msg_statustext.h +++ b/lib/main/MAVLink/common/mavlink_msg_statustext.h @@ -71,7 +71,7 @@ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t co packet.severity = severity; packet.id = id; packet.chunk_seq = chunk_seq; - mav_array_memcpy(packet.text, text, sizeof(char)*50); + mav_array_assign_char(packet.text, text, 50); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); #endif @@ -79,6 +79,46 @@ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t co return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); } +/** + * @brief Pack a statustext message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param severity Severity of status. Relies on the definitions within RFC-5424. + * @param text Status text message, without null termination character + * @param id Unique (opaque) identifier for this statustext message. May be used to reassemble a logical long-statustext message from a sequence of chunks. A value of zero indicates this is the only chunk in the sequence and the message can be emitted immediately. + * @param chunk_seq This chunk's sequence number; indexing is from zero. Any null character in the text field is taken to mean this was the last chunk. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_statustext_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t severity, const char *text, uint16_t id, uint8_t chunk_seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; + _mav_put_uint8_t(buf, 0, severity); + _mav_put_uint16_t(buf, 51, id); + _mav_put_uint8_t(buf, 53, chunk_seq); + _mav_put_char_array(buf, 1, text, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); +#else + mavlink_statustext_t packet; + packet.severity = severity; + packet.id = id; + packet.chunk_seq = chunk_seq; + mav_array_memcpy(packet.text, text, sizeof(char)*50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN); +#endif +} + /** * @brief Pack a statustext message on a channel * @param system_id ID of this system @@ -107,7 +147,7 @@ static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8 packet.severity = severity; packet.id = id; packet.chunk_seq = chunk_seq; - mav_array_memcpy(packet.text, text, sizeof(char)*50); + mav_array_assign_char(packet.text, text, 50); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); #endif @@ -142,6 +182,20 @@ static inline uint16_t mavlink_msg_statustext_encode_chan(uint8_t system_id, uin return mavlink_msg_statustext_pack_chan(system_id, component_id, chan, msg, statustext->severity, statustext->text, statustext->id, statustext->chunk_seq); } +/** + * @brief Encode a statustext struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param statustext C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_statustext_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_statustext_t* statustext) +{ + return mavlink_msg_statustext_pack_status(system_id, component_id, _status, msg, statustext->severity, statustext->text, statustext->id, statustext->chunk_seq); +} + /** * @brief Send a statustext message * @param chan MAVLink channel to send the message @@ -167,7 +221,7 @@ static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t s packet.severity = severity; packet.id = id; packet.chunk_seq = chunk_seq; - mav_array_memcpy(packet.text, text, sizeof(char)*50); + mav_array_assign_char(packet.text, text, 50); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)&packet, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); #endif } @@ -188,7 +242,7 @@ static inline void mavlink_msg_statustext_send_struct(mavlink_channel_t chan, co #if MAVLINK_MSG_ID_STATUSTEXT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -208,7 +262,7 @@ static inline void mavlink_msg_statustext_send_buf(mavlink_message_t *msgbuf, ma packet->severity = severity; packet->id = id; packet->chunk_seq = chunk_seq; - mav_array_memcpy(packet->text, text, sizeof(char)*50); + mav_array_assign_char(packet->text, text, 50); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)packet, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_storage_information.h b/lib/main/MAVLink/common/mavlink_msg_storage_information.h index 76d8afd57b0..59db68826b5 100644 --- a/lib/main/MAVLink/common/mavlink_msg_storage_information.h +++ b/lib/main/MAVLink/common/mavlink_msg_storage_information.h @@ -16,11 +16,15 @@ typedef struct __mavlink_storage_information_t { uint8_t status; /*< Status of storage*/ uint8_t type; /*< Type of storage*/ char name[32]; /*< Textual storage name to be used in UI (microSD 1, Internal Memory, etc.) This is a NULL terminated string. If it is exactly 32 characters long, add a terminating NULL. If this string is empty, the generic type is shown to the user.*/ + uint8_t storage_usage; /*< Flags indicating whether this instance is preferred storage for photos, videos, etc. + Note: Implementations should initially set the flags on the system-default storage id used for saving media (if possible/supported). + This setting can then be overridden using MAV_CMD_SET_STORAGE_USAGE. + If the media usage flags are not set, a GCS may assume storage ID 1 is the default storage for all media types.*/ } mavlink_storage_information_t; -#define MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN 60 +#define MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN 61 #define MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN 27 -#define MAVLINK_MSG_ID_261_LEN 60 +#define MAVLINK_MSG_ID_261_LEN 61 #define MAVLINK_MSG_ID_261_MIN_LEN 27 #define MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC 179 @@ -32,7 +36,7 @@ typedef struct __mavlink_storage_information_t { #define MAVLINK_MESSAGE_INFO_STORAGE_INFORMATION { \ 261, \ "STORAGE_INFORMATION", \ - 11, \ + 12, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_storage_information_t, time_boot_ms) }, \ { "storage_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_storage_information_t, storage_id) }, \ { "storage_count", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_storage_information_t, storage_count) }, \ @@ -44,12 +48,13 @@ typedef struct __mavlink_storage_information_t { { "write_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_storage_information_t, write_speed) }, \ { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_storage_information_t, type) }, \ { "name", NULL, MAVLINK_TYPE_CHAR, 32, 28, offsetof(mavlink_storage_information_t, name) }, \ + { "storage_usage", NULL, MAVLINK_TYPE_UINT8_T, 0, 60, offsetof(mavlink_storage_information_t, storage_usage) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_STORAGE_INFORMATION { \ "STORAGE_INFORMATION", \ - 11, \ + 12, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_storage_information_t, time_boot_ms) }, \ { "storage_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_storage_information_t, storage_id) }, \ { "storage_count", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_storage_information_t, storage_count) }, \ @@ -61,6 +66,7 @@ typedef struct __mavlink_storage_information_t { { "write_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_storage_information_t, write_speed) }, \ { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_storage_information_t, type) }, \ { "name", NULL, MAVLINK_TYPE_CHAR, 32, 28, offsetof(mavlink_storage_information_t, name) }, \ + { "storage_usage", NULL, MAVLINK_TYPE_UINT8_T, 0, 60, offsetof(mavlink_storage_information_t, storage_usage) }, \ } \ } #endif @@ -82,10 +88,14 @@ typedef struct __mavlink_storage_information_t { * @param write_speed [MiB/s] Write speed. * @param type Type of storage * @param name Textual storage name to be used in UI (microSD 1, Internal Memory, etc.) This is a NULL terminated string. If it is exactly 32 characters long, add a terminating NULL. If this string is empty, the generic type is shown to the user. + * @param storage_usage Flags indicating whether this instance is preferred storage for photos, videos, etc. + Note: Implementations should initially set the flags on the system-default storage id used for saving media (if possible/supported). + This setting can then be overridden using MAV_CMD_SET_STORAGE_USAGE. + If the media usage flags are not set, a GCS may assume storage ID 1 is the default storage for all media types. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_storage_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, uint8_t storage_id, uint8_t storage_count, uint8_t status, float total_capacity, float used_capacity, float available_capacity, float read_speed, float write_speed, uint8_t type, const char *name) + uint32_t time_boot_ms, uint8_t storage_id, uint8_t storage_count, uint8_t status, float total_capacity, float used_capacity, float available_capacity, float read_speed, float write_speed, uint8_t type, const char *name, uint8_t storage_usage) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN]; @@ -99,6 +109,7 @@ static inline uint16_t mavlink_msg_storage_information_pack(uint8_t system_id, u _mav_put_uint8_t(buf, 25, storage_count); _mav_put_uint8_t(buf, 26, status); _mav_put_uint8_t(buf, 27, type); + _mav_put_uint8_t(buf, 60, storage_usage); _mav_put_char_array(buf, 28, name, 32); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN); #else @@ -113,7 +124,8 @@ static inline uint16_t mavlink_msg_storage_information_pack(uint8_t system_id, u packet.storage_count = storage_count; packet.status = status; packet.type = type; - mav_array_memcpy(packet.name, name, sizeof(char)*32); + packet.storage_usage = storage_usage; + mav_array_assign_char(packet.name, name, 32); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN); #endif @@ -121,6 +133,73 @@ static inline uint16_t mavlink_msg_storage_information_pack(uint8_t system_id, u return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); } +/** + * @brief Pack a storage_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param storage_id Storage ID (1 for first, 2 for second, etc.) + * @param storage_count Number of storage devices + * @param status Status of storage + * @param total_capacity [MiB] Total capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param used_capacity [MiB] Used capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param available_capacity [MiB] Available storage capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param read_speed [MiB/s] Read speed. + * @param write_speed [MiB/s] Write speed. + * @param type Type of storage + * @param name Textual storage name to be used in UI (microSD 1, Internal Memory, etc.) This is a NULL terminated string. If it is exactly 32 characters long, add a terminating NULL. If this string is empty, the generic type is shown to the user. + * @param storage_usage Flags indicating whether this instance is preferred storage for photos, videos, etc. + Note: Implementations should initially set the flags on the system-default storage id used for saving media (if possible/supported). + This setting can then be overridden using MAV_CMD_SET_STORAGE_USAGE. + If the media usage flags are not set, a GCS may assume storage ID 1 is the default storage for all media types. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_storage_information_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t storage_id, uint8_t storage_count, uint8_t status, float total_capacity, float used_capacity, float available_capacity, float read_speed, float write_speed, uint8_t type, const char *name, uint8_t storage_usage) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, total_capacity); + _mav_put_float(buf, 8, used_capacity); + _mav_put_float(buf, 12, available_capacity); + _mav_put_float(buf, 16, read_speed); + _mav_put_float(buf, 20, write_speed); + _mav_put_uint8_t(buf, 24, storage_id); + _mav_put_uint8_t(buf, 25, storage_count); + _mav_put_uint8_t(buf, 26, status); + _mav_put_uint8_t(buf, 27, type); + _mav_put_uint8_t(buf, 60, storage_usage); + _mav_put_char_array(buf, 28, name, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN); +#else + mavlink_storage_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.total_capacity = total_capacity; + packet.used_capacity = used_capacity; + packet.available_capacity = available_capacity; + packet.read_speed = read_speed; + packet.write_speed = write_speed; + packet.storage_id = storage_id; + packet.storage_count = storage_count; + packet.status = status; + packet.type = type; + packet.storage_usage = storage_usage; + mav_array_memcpy(packet.name, name, sizeof(char)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_STORAGE_INFORMATION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN); +#endif +} + /** * @brief Pack a storage_information message on a channel * @param system_id ID of this system @@ -138,11 +217,15 @@ static inline uint16_t mavlink_msg_storage_information_pack(uint8_t system_id, u * @param write_speed [MiB/s] Write speed. * @param type Type of storage * @param name Textual storage name to be used in UI (microSD 1, Internal Memory, etc.) This is a NULL terminated string. If it is exactly 32 characters long, add a terminating NULL. If this string is empty, the generic type is shown to the user. + * @param storage_usage Flags indicating whether this instance is preferred storage for photos, videos, etc. + Note: Implementations should initially set the flags on the system-default storage id used for saving media (if possible/supported). + This setting can then be overridden using MAV_CMD_SET_STORAGE_USAGE. + If the media usage flags are not set, a GCS may assume storage ID 1 is the default storage for all media types. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_storage_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,uint8_t storage_id,uint8_t storage_count,uint8_t status,float total_capacity,float used_capacity,float available_capacity,float read_speed,float write_speed,uint8_t type,const char *name) + uint32_t time_boot_ms,uint8_t storage_id,uint8_t storage_count,uint8_t status,float total_capacity,float used_capacity,float available_capacity,float read_speed,float write_speed,uint8_t type,const char *name,uint8_t storage_usage) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN]; @@ -156,6 +239,7 @@ static inline uint16_t mavlink_msg_storage_information_pack_chan(uint8_t system_ _mav_put_uint8_t(buf, 25, storage_count); _mav_put_uint8_t(buf, 26, status); _mav_put_uint8_t(buf, 27, type); + _mav_put_uint8_t(buf, 60, storage_usage); _mav_put_char_array(buf, 28, name, 32); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN); #else @@ -170,7 +254,8 @@ static inline uint16_t mavlink_msg_storage_information_pack_chan(uint8_t system_ packet.storage_count = storage_count; packet.status = status; packet.type = type; - mav_array_memcpy(packet.name, name, sizeof(char)*32); + packet.storage_usage = storage_usage; + mav_array_assign_char(packet.name, name, 32); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN); #endif @@ -188,7 +273,7 @@ static inline uint16_t mavlink_msg_storage_information_pack_chan(uint8_t system_ */ static inline uint16_t mavlink_msg_storage_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_storage_information_t* storage_information) { - return mavlink_msg_storage_information_pack(system_id, component_id, msg, storage_information->time_boot_ms, storage_information->storage_id, storage_information->storage_count, storage_information->status, storage_information->total_capacity, storage_information->used_capacity, storage_information->available_capacity, storage_information->read_speed, storage_information->write_speed, storage_information->type, storage_information->name); + return mavlink_msg_storage_information_pack(system_id, component_id, msg, storage_information->time_boot_ms, storage_information->storage_id, storage_information->storage_count, storage_information->status, storage_information->total_capacity, storage_information->used_capacity, storage_information->available_capacity, storage_information->read_speed, storage_information->write_speed, storage_information->type, storage_information->name, storage_information->storage_usage); } /** @@ -202,7 +287,21 @@ static inline uint16_t mavlink_msg_storage_information_encode(uint8_t system_id, */ static inline uint16_t mavlink_msg_storage_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_storage_information_t* storage_information) { - return mavlink_msg_storage_information_pack_chan(system_id, component_id, chan, msg, storage_information->time_boot_ms, storage_information->storage_id, storage_information->storage_count, storage_information->status, storage_information->total_capacity, storage_information->used_capacity, storage_information->available_capacity, storage_information->read_speed, storage_information->write_speed, storage_information->type, storage_information->name); + return mavlink_msg_storage_information_pack_chan(system_id, component_id, chan, msg, storage_information->time_boot_ms, storage_information->storage_id, storage_information->storage_count, storage_information->status, storage_information->total_capacity, storage_information->used_capacity, storage_information->available_capacity, storage_information->read_speed, storage_information->write_speed, storage_information->type, storage_information->name, storage_information->storage_usage); +} + +/** + * @brief Encode a storage_information struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param storage_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_storage_information_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_storage_information_t* storage_information) +{ + return mavlink_msg_storage_information_pack_status(system_id, component_id, _status, msg, storage_information->time_boot_ms, storage_information->storage_id, storage_information->storage_count, storage_information->status, storage_information->total_capacity, storage_information->used_capacity, storage_information->available_capacity, storage_information->read_speed, storage_information->write_speed, storage_information->type, storage_information->name, storage_information->storage_usage); } /** @@ -220,10 +319,14 @@ static inline uint16_t mavlink_msg_storage_information_encode_chan(uint8_t syste * @param write_speed [MiB/s] Write speed. * @param type Type of storage * @param name Textual storage name to be used in UI (microSD 1, Internal Memory, etc.) This is a NULL terminated string. If it is exactly 32 characters long, add a terminating NULL. If this string is empty, the generic type is shown to the user. + * @param storage_usage Flags indicating whether this instance is preferred storage for photos, videos, etc. + Note: Implementations should initially set the flags on the system-default storage id used for saving media (if possible/supported). + This setting can then be overridden using MAV_CMD_SET_STORAGE_USAGE. + If the media usage flags are not set, a GCS may assume storage ID 1 is the default storage for all media types. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_storage_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t storage_id, uint8_t storage_count, uint8_t status, float total_capacity, float used_capacity, float available_capacity, float read_speed, float write_speed, uint8_t type, const char *name) +static inline void mavlink_msg_storage_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t storage_id, uint8_t storage_count, uint8_t status, float total_capacity, float used_capacity, float available_capacity, float read_speed, float write_speed, uint8_t type, const char *name, uint8_t storage_usage) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN]; @@ -237,6 +340,7 @@ static inline void mavlink_msg_storage_information_send(mavlink_channel_t chan, _mav_put_uint8_t(buf, 25, storage_count); _mav_put_uint8_t(buf, 26, status); _mav_put_uint8_t(buf, 27, type); + _mav_put_uint8_t(buf, 60, storage_usage); _mav_put_char_array(buf, 28, name, 32); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STORAGE_INFORMATION, buf, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); #else @@ -251,7 +355,8 @@ static inline void mavlink_msg_storage_information_send(mavlink_channel_t chan, packet.storage_count = storage_count; packet.status = status; packet.type = type; - mav_array_memcpy(packet.name, name, sizeof(char)*32); + packet.storage_usage = storage_usage; + mav_array_assign_char(packet.name, name, 32); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STORAGE_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); #endif } @@ -264,7 +369,7 @@ static inline void mavlink_msg_storage_information_send(mavlink_channel_t chan, static inline void mavlink_msg_storage_information_send_struct(mavlink_channel_t chan, const mavlink_storage_information_t* storage_information) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_storage_information_send(chan, storage_information->time_boot_ms, storage_information->storage_id, storage_information->storage_count, storage_information->status, storage_information->total_capacity, storage_information->used_capacity, storage_information->available_capacity, storage_information->read_speed, storage_information->write_speed, storage_information->type, storage_information->name); + mavlink_msg_storage_information_send(chan, storage_information->time_boot_ms, storage_information->storage_id, storage_information->storage_count, storage_information->status, storage_information->total_capacity, storage_information->used_capacity, storage_information->available_capacity, storage_information->read_speed, storage_information->write_speed, storage_information->type, storage_information->name, storage_information->storage_usage); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STORAGE_INFORMATION, (const char *)storage_information, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); #endif @@ -272,13 +377,13 @@ static inline void mavlink_msg_storage_information_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_storage_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t storage_id, uint8_t storage_count, uint8_t status, float total_capacity, float used_capacity, float available_capacity, float read_speed, float write_speed, uint8_t type, const char *name) +static inline void mavlink_msg_storage_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t storage_id, uint8_t storage_count, uint8_t status, float total_capacity, float used_capacity, float available_capacity, float read_speed, float write_speed, uint8_t type, const char *name, uint8_t storage_usage) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -292,6 +397,7 @@ static inline void mavlink_msg_storage_information_send_buf(mavlink_message_t *m _mav_put_uint8_t(buf, 25, storage_count); _mav_put_uint8_t(buf, 26, status); _mav_put_uint8_t(buf, 27, type); + _mav_put_uint8_t(buf, 60, storage_usage); _mav_put_char_array(buf, 28, name, 32); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STORAGE_INFORMATION, buf, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); #else @@ -306,7 +412,8 @@ static inline void mavlink_msg_storage_information_send_buf(mavlink_message_t *m packet->storage_count = storage_count; packet->status = status; packet->type = type; - mav_array_memcpy(packet->name, name, sizeof(char)*32); + packet->storage_usage = storage_usage; + mav_array_assign_char(packet->name, name, 32); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STORAGE_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); #endif } @@ -427,6 +534,19 @@ static inline uint16_t mavlink_msg_storage_information_get_name(const mavlink_me return _MAV_RETURN_char_array(msg, name, 32, 28); } +/** + * @brief Get field storage_usage from storage_information message + * + * @return Flags indicating whether this instance is preferred storage for photos, videos, etc. + Note: Implementations should initially set the flags on the system-default storage id used for saving media (if possible/supported). + This setting can then be overridden using MAV_CMD_SET_STORAGE_USAGE. + If the media usage flags are not set, a GCS may assume storage ID 1 is the default storage for all media types. + */ +static inline uint8_t mavlink_msg_storage_information_get_storage_usage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 60); +} + /** * @brief Decode a storage_information message into a struct * @@ -447,6 +567,7 @@ static inline void mavlink_msg_storage_information_decode(const mavlink_message_ storage_information->status = mavlink_msg_storage_information_get_status(msg); storage_information->type = mavlink_msg_storage_information_get_type(msg); mavlink_msg_storage_information_get_name(msg, storage_information->name); + storage_information->storage_usage = mavlink_msg_storage_information_get_storage_usage(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN; memset(storage_information, 0, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_supported_tunes.h b/lib/main/MAVLink/common/mavlink_msg_supported_tunes.h index a8fb9a3abf8..801414a7e9a 100644 --- a/lib/main/MAVLink/common/mavlink_msg_supported_tunes.h +++ b/lib/main/MAVLink/common/mavlink_msg_supported_tunes.h @@ -75,6 +75,45 @@ static inline uint16_t mavlink_msg_supported_tunes_pack(uint8_t system_id, uint8 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_CRC); } +/** + * @brief Pack a supported_tunes message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param format Bitfield of supported tune formats. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_supported_tunes_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t format) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN]; + _mav_put_uint32_t(buf, 0, format); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN); +#else + mavlink_supported_tunes_t packet; + packet.format = format; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SUPPORTED_TUNES; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN); +#endif +} + /** * @brief Pack a supported_tunes message on a channel * @param system_id ID of this system @@ -137,6 +176,20 @@ static inline uint16_t mavlink_msg_supported_tunes_encode_chan(uint8_t system_id return mavlink_msg_supported_tunes_pack_chan(system_id, component_id, chan, msg, supported_tunes->target_system, supported_tunes->target_component, supported_tunes->format); } +/** + * @brief Encode a supported_tunes struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param supported_tunes C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_supported_tunes_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_supported_tunes_t* supported_tunes) +{ + return mavlink_msg_supported_tunes_pack_status(system_id, component_id, _status, msg, supported_tunes->target_system, supported_tunes->target_component, supported_tunes->format); +} + /** * @brief Send a supported_tunes message * @param chan MAVLink channel to send the message @@ -182,7 +235,7 @@ static inline void mavlink_msg_supported_tunes_send_struct(mavlink_channel_t cha #if MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_sys_status.h b/lib/main/MAVLink/common/mavlink_msg_sys_status.h index 301302a14c9..75fc3bb6460 100755 --- a/lib/main/MAVLink/common/mavlink_msg_sys_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_sys_status.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_SYS_STATUS 1 - +MAVPACKED( typedef struct __mavlink_sys_status_t { uint32_t onboard_control_sensors_present; /*< Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present.*/ uint32_t onboard_control_sensors_enabled; /*< Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled.*/ @@ -18,11 +18,14 @@ typedef struct __mavlink_sys_status_t { uint16_t errors_count3; /*< Autopilot-specific errors*/ uint16_t errors_count4; /*< Autopilot-specific errors*/ int8_t battery_remaining; /*< [%] Battery energy remaining, -1: Battery remaining energy not sent by autopilot*/ -} mavlink_sys_status_t; + uint32_t onboard_control_sensors_present_extended; /*< Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present.*/ + uint32_t onboard_control_sensors_enabled_extended; /*< Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled.*/ + uint32_t onboard_control_sensors_health_extended; /*< Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy.*/ +}) mavlink_sys_status_t; -#define MAVLINK_MSG_ID_SYS_STATUS_LEN 31 +#define MAVLINK_MSG_ID_SYS_STATUS_LEN 43 #define MAVLINK_MSG_ID_SYS_STATUS_MIN_LEN 31 -#define MAVLINK_MSG_ID_1_LEN 31 +#define MAVLINK_MSG_ID_1_LEN 43 #define MAVLINK_MSG_ID_1_MIN_LEN 31 #define MAVLINK_MSG_ID_SYS_STATUS_CRC 124 @@ -34,7 +37,7 @@ typedef struct __mavlink_sys_status_t { #define MAVLINK_MESSAGE_INFO_SYS_STATUS { \ 1, \ "SYS_STATUS", \ - 13, \ + 16, \ { { "onboard_control_sensors_present", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_sys_status_t, onboard_control_sensors_present) }, \ { "onboard_control_sensors_enabled", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_sys_status_t, onboard_control_sensors_enabled) }, \ { "onboard_control_sensors_health", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_sys_status_t, onboard_control_sensors_health) }, \ @@ -48,12 +51,15 @@ typedef struct __mavlink_sys_status_t { { "errors_count2", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_sys_status_t, errors_count2) }, \ { "errors_count3", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_sys_status_t, errors_count3) }, \ { "errors_count4", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_sys_status_t, errors_count4) }, \ + { "onboard_control_sensors_present_extended", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 31, offsetof(mavlink_sys_status_t, onboard_control_sensors_present_extended) }, \ + { "onboard_control_sensors_enabled_extended", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 35, offsetof(mavlink_sys_status_t, onboard_control_sensors_enabled_extended) }, \ + { "onboard_control_sensors_health_extended", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 39, offsetof(mavlink_sys_status_t, onboard_control_sensors_health_extended) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SYS_STATUS { \ "SYS_STATUS", \ - 13, \ + 16, \ { { "onboard_control_sensors_present", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_sys_status_t, onboard_control_sensors_present) }, \ { "onboard_control_sensors_enabled", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_sys_status_t, onboard_control_sensors_enabled) }, \ { "onboard_control_sensors_health", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_sys_status_t, onboard_control_sensors_health) }, \ @@ -67,6 +73,9 @@ typedef struct __mavlink_sys_status_t { { "errors_count2", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_sys_status_t, errors_count2) }, \ { "errors_count3", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_sys_status_t, errors_count3) }, \ { "errors_count4", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_sys_status_t, errors_count4) }, \ + { "onboard_control_sensors_present_extended", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 31, offsetof(mavlink_sys_status_t, onboard_control_sensors_present_extended) }, \ + { "onboard_control_sensors_enabled_extended", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 35, offsetof(mavlink_sys_status_t, onboard_control_sensors_enabled_extended) }, \ + { "onboard_control_sensors_health_extended", "0x%04x", MAVLINK_TYPE_UINT32_T, 0, 39, offsetof(mavlink_sys_status_t, onboard_control_sensors_health_extended) }, \ } \ } #endif @@ -90,10 +99,13 @@ typedef struct __mavlink_sys_status_t { * @param errors_count2 Autopilot-specific errors * @param errors_count3 Autopilot-specific errors * @param errors_count4 Autopilot-specific errors + * @param onboard_control_sensors_present_extended Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. + * @param onboard_control_sensors_enabled_extended Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. + * @param onboard_control_sensors_health_extended Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4) + uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4, uint32_t onboard_control_sensors_present_extended, uint32_t onboard_control_sensors_enabled_extended, uint32_t onboard_control_sensors_health_extended) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SYS_STATUS_LEN]; @@ -110,6 +122,9 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co _mav_put_uint16_t(buf, 26, errors_count3); _mav_put_uint16_t(buf, 28, errors_count4); _mav_put_int8_t(buf, 30, battery_remaining); + _mav_put_uint32_t(buf, 31, onboard_control_sensors_present_extended); + _mav_put_uint32_t(buf, 35, onboard_control_sensors_enabled_extended); + _mav_put_uint32_t(buf, 39, onboard_control_sensors_health_extended); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_STATUS_LEN); #else @@ -127,6 +142,9 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co packet.errors_count3 = errors_count3; packet.errors_count4 = errors_count4; packet.battery_remaining = battery_remaining; + packet.onboard_control_sensors_present_extended = onboard_control_sensors_present_extended; + packet.onboard_control_sensors_enabled_extended = onboard_control_sensors_enabled_extended; + packet.onboard_control_sensors_health_extended = onboard_control_sensors_health_extended; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_STATUS_LEN); #endif @@ -135,6 +153,84 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_STATUS_MIN_LEN, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); } +/** + * @brief Pack a sys_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param onboard_control_sensors_present Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. + * @param onboard_control_sensors_enabled Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. + * @param onboard_control_sensors_health Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy. + * @param load [d%] Maximum usage in percent of the mainloop time. Values: [0-1000] - should always be below 1000 + * @param voltage_battery [mV] Battery voltage, UINT16_MAX: Voltage not sent by autopilot + * @param current_battery [cA] Battery current, -1: Current not sent by autopilot + * @param battery_remaining [%] Battery energy remaining, -1: Battery remaining energy not sent by autopilot + * @param drop_rate_comm [c%] Communication drop rate, (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + * @param errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + * @param errors_count1 Autopilot-specific errors + * @param errors_count2 Autopilot-specific errors + * @param errors_count3 Autopilot-specific errors + * @param errors_count4 Autopilot-specific errors + * @param onboard_control_sensors_present_extended Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. + * @param onboard_control_sensors_enabled_extended Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. + * @param onboard_control_sensors_health_extended Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_sys_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4, uint32_t onboard_control_sensors_present_extended, uint32_t onboard_control_sensors_enabled_extended, uint32_t onboard_control_sensors_health_extended) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SYS_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, onboard_control_sensors_present); + _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled); + _mav_put_uint32_t(buf, 8, onboard_control_sensors_health); + _mav_put_uint16_t(buf, 12, load); + _mav_put_uint16_t(buf, 14, voltage_battery); + _mav_put_int16_t(buf, 16, current_battery); + _mav_put_uint16_t(buf, 18, drop_rate_comm); + _mav_put_uint16_t(buf, 20, errors_comm); + _mav_put_uint16_t(buf, 22, errors_count1); + _mav_put_uint16_t(buf, 24, errors_count2); + _mav_put_uint16_t(buf, 26, errors_count3); + _mav_put_uint16_t(buf, 28, errors_count4); + _mav_put_int8_t(buf, 30, battery_remaining); + _mav_put_uint32_t(buf, 31, onboard_control_sensors_present_extended); + _mav_put_uint32_t(buf, 35, onboard_control_sensors_enabled_extended); + _mav_put_uint32_t(buf, 39, onboard_control_sensors_health_extended); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#else + mavlink_sys_status_t packet; + packet.onboard_control_sensors_present = onboard_control_sensors_present; + packet.onboard_control_sensors_enabled = onboard_control_sensors_enabled; + packet.onboard_control_sensors_health = onboard_control_sensors_health; + packet.load = load; + packet.voltage_battery = voltage_battery; + packet.current_battery = current_battery; + packet.drop_rate_comm = drop_rate_comm; + packet.errors_comm = errors_comm; + packet.errors_count1 = errors_count1; + packet.errors_count2 = errors_count2; + packet.errors_count3 = errors_count3; + packet.errors_count4 = errors_count4; + packet.battery_remaining = battery_remaining; + packet.onboard_control_sensors_present_extended = onboard_control_sensors_present_extended; + packet.onboard_control_sensors_enabled_extended = onboard_control_sensors_enabled_extended; + packet.onboard_control_sensors_health_extended = onboard_control_sensors_health_extended; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SYS_STATUS_MIN_LEN, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SYS_STATUS_MIN_LEN, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#endif +} + /** * @brief Pack a sys_status message on a channel * @param system_id ID of this system @@ -154,11 +250,14 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co * @param errors_count2 Autopilot-specific errors * @param errors_count3 Autopilot-specific errors * @param errors_count4 Autopilot-specific errors + * @param onboard_control_sensors_present_extended Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. + * @param onboard_control_sensors_enabled_extended Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. + * @param onboard_control_sensors_health_extended Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t onboard_control_sensors_present,uint32_t onboard_control_sensors_enabled,uint32_t onboard_control_sensors_health,uint16_t load,uint16_t voltage_battery,int16_t current_battery,int8_t battery_remaining,uint16_t drop_rate_comm,uint16_t errors_comm,uint16_t errors_count1,uint16_t errors_count2,uint16_t errors_count3,uint16_t errors_count4) + uint32_t onboard_control_sensors_present,uint32_t onboard_control_sensors_enabled,uint32_t onboard_control_sensors_health,uint16_t load,uint16_t voltage_battery,int16_t current_battery,int8_t battery_remaining,uint16_t drop_rate_comm,uint16_t errors_comm,uint16_t errors_count1,uint16_t errors_count2,uint16_t errors_count3,uint16_t errors_count4,uint32_t onboard_control_sensors_present_extended,uint32_t onboard_control_sensors_enabled_extended,uint32_t onboard_control_sensors_health_extended) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SYS_STATUS_LEN]; @@ -175,6 +274,9 @@ static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8 _mav_put_uint16_t(buf, 26, errors_count3); _mav_put_uint16_t(buf, 28, errors_count4); _mav_put_int8_t(buf, 30, battery_remaining); + _mav_put_uint32_t(buf, 31, onboard_control_sensors_present_extended); + _mav_put_uint32_t(buf, 35, onboard_control_sensors_enabled_extended); + _mav_put_uint32_t(buf, 39, onboard_control_sensors_health_extended); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_STATUS_LEN); #else @@ -192,6 +294,9 @@ static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8 packet.errors_count3 = errors_count3; packet.errors_count4 = errors_count4; packet.battery_remaining = battery_remaining; + packet.onboard_control_sensors_present_extended = onboard_control_sensors_present_extended; + packet.onboard_control_sensors_enabled_extended = onboard_control_sensors_enabled_extended; + packet.onboard_control_sensors_health_extended = onboard_control_sensors_health_extended; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_STATUS_LEN); #endif @@ -210,7 +315,7 @@ static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status) { - return mavlink_msg_sys_status_pack(system_id, component_id, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4); + return mavlink_msg_sys_status_pack(system_id, component_id, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4, sys_status->onboard_control_sensors_present_extended, sys_status->onboard_control_sensors_enabled_extended, sys_status->onboard_control_sensors_health_extended); } /** @@ -224,7 +329,21 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_sys_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status) { - return mavlink_msg_sys_status_pack_chan(system_id, component_id, chan, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4); + return mavlink_msg_sys_status_pack_chan(system_id, component_id, chan, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4, sys_status->onboard_control_sensors_present_extended, sys_status->onboard_control_sensors_enabled_extended, sys_status->onboard_control_sensors_health_extended); +} + +/** + * @brief Encode a sys_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param sys_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sys_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status) +{ + return mavlink_msg_sys_status_pack_status(system_id, component_id, _status, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4, sys_status->onboard_control_sensors_present_extended, sys_status->onboard_control_sensors_enabled_extended, sys_status->onboard_control_sensors_health_extended); } /** @@ -244,10 +363,13 @@ static inline uint16_t mavlink_msg_sys_status_encode_chan(uint8_t system_id, uin * @param errors_count2 Autopilot-specific errors * @param errors_count3 Autopilot-specific errors * @param errors_count4 Autopilot-specific errors + * @param onboard_control_sensors_present_extended Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. + * @param onboard_control_sensors_enabled_extended Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. + * @param onboard_control_sensors_health_extended Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4) +static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4, uint32_t onboard_control_sensors_present_extended, uint32_t onboard_control_sensors_enabled_extended, uint32_t onboard_control_sensors_health_extended) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SYS_STATUS_LEN]; @@ -264,6 +386,9 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t _mav_put_uint16_t(buf, 26, errors_count3); _mav_put_uint16_t(buf, 28, errors_count4); _mav_put_int8_t(buf, 30, battery_remaining); + _mav_put_uint32_t(buf, 31, onboard_control_sensors_present_extended); + _mav_put_uint32_t(buf, 35, onboard_control_sensors_enabled_extended); + _mav_put_uint32_t(buf, 39, onboard_control_sensors_health_extended); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_MIN_LEN, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); #else @@ -281,6 +406,9 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t packet.errors_count3 = errors_count3; packet.errors_count4 = errors_count4; packet.battery_remaining = battery_remaining; + packet.onboard_control_sensors_present_extended = onboard_control_sensors_present_extended; + packet.onboard_control_sensors_enabled_extended = onboard_control_sensors_enabled_extended; + packet.onboard_control_sensors_health_extended = onboard_control_sensors_health_extended; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)&packet, MAVLINK_MSG_ID_SYS_STATUS_MIN_LEN, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); #endif @@ -294,7 +422,7 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t static inline void mavlink_msg_sys_status_send_struct(mavlink_channel_t chan, const mavlink_sys_status_t* sys_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_sys_status_send(chan, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4); + mavlink_msg_sys_status_send(chan, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4, sys_status->onboard_control_sensors_present_extended, sys_status->onboard_control_sensors_enabled_extended, sys_status->onboard_control_sensors_health_extended); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)sys_status, MAVLINK_MSG_ID_SYS_STATUS_MIN_LEN, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); #endif @@ -302,13 +430,13 @@ static inline void mavlink_msg_sys_status_send_struct(mavlink_channel_t chan, co #if MAVLINK_MSG_ID_SYS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_sys_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4) +static inline void mavlink_msg_sys_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4, uint32_t onboard_control_sensors_present_extended, uint32_t onboard_control_sensors_enabled_extended, uint32_t onboard_control_sensors_health_extended) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -325,6 +453,9 @@ static inline void mavlink_msg_sys_status_send_buf(mavlink_message_t *msgbuf, ma _mav_put_uint16_t(buf, 26, errors_count3); _mav_put_uint16_t(buf, 28, errors_count4); _mav_put_int8_t(buf, 30, battery_remaining); + _mav_put_uint32_t(buf, 31, onboard_control_sensors_present_extended); + _mav_put_uint32_t(buf, 35, onboard_control_sensors_enabled_extended); + _mav_put_uint32_t(buf, 39, onboard_control_sensors_health_extended); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_MIN_LEN, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); #else @@ -342,6 +473,9 @@ static inline void mavlink_msg_sys_status_send_buf(mavlink_message_t *msgbuf, ma packet->errors_count3 = errors_count3; packet->errors_count4 = errors_count4; packet->battery_remaining = battery_remaining; + packet->onboard_control_sensors_present_extended = onboard_control_sensors_present_extended; + packet->onboard_control_sensors_enabled_extended = onboard_control_sensors_enabled_extended; + packet->onboard_control_sensors_health_extended = onboard_control_sensors_health_extended; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)packet, MAVLINK_MSG_ID_SYS_STATUS_MIN_LEN, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); #endif @@ -483,6 +617,36 @@ static inline uint16_t mavlink_msg_sys_status_get_errors_count4(const mavlink_me return _MAV_RETURN_uint16_t(msg, 28); } +/** + * @brief Get field onboard_control_sensors_present_extended from sys_status message + * + * @return Bitmap showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. + */ +static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_present_extended(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 31); +} + +/** + * @brief Get field onboard_control_sensors_enabled_extended from sys_status message + * + * @return Bitmap showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. + */ +static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_enabled_extended(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 35); +} + +/** + * @brief Get field onboard_control_sensors_health_extended from sys_status message + * + * @return Bitmap showing which onboard controllers and sensors have an error (or are operational). Value of 0: error. Value of 1: healthy. + */ +static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_health_extended(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 39); +} + /** * @brief Decode a sys_status message into a struct * @@ -505,6 +669,9 @@ static inline void mavlink_msg_sys_status_decode(const mavlink_message_t* msg, m sys_status->errors_count3 = mavlink_msg_sys_status_get_errors_count3(msg); sys_status->errors_count4 = mavlink_msg_sys_status_get_errors_count4(msg); sys_status->battery_remaining = mavlink_msg_sys_status_get_battery_remaining(msg); + sys_status->onboard_control_sensors_present_extended = mavlink_msg_sys_status_get_onboard_control_sensors_present_extended(msg); + sys_status->onboard_control_sensors_enabled_extended = mavlink_msg_sys_status_get_onboard_control_sensors_enabled_extended(msg); + sys_status->onboard_control_sensors_health_extended = mavlink_msg_sys_status_get_onboard_control_sensors_health_extended(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SYS_STATUS_LEN? msg->len : MAVLINK_MSG_ID_SYS_STATUS_LEN; memset(sys_status, 0, MAVLINK_MSG_ID_SYS_STATUS_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_system_time.h b/lib/main/MAVLink/common/mavlink_msg_system_time.h index a3848c74190..1c57e692a5a 100755 --- a/lib/main/MAVLink/common/mavlink_msg_system_time.h +++ b/lib/main/MAVLink/common/mavlink_msg_system_time.h @@ -69,6 +69,42 @@ static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t c return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYSTEM_TIME_MIN_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); } +/** + * @brief Pack a system_time message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_unix_usec [us] Timestamp (UNIX epoch time). + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_system_time_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_unix_usec, uint32_t time_boot_ms) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SYSTEM_TIME_LEN]; + _mav_put_uint64_t(buf, 0, time_unix_usec); + _mav_put_uint32_t(buf, 8, time_boot_ms); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#else + mavlink_system_time_t packet; + packet.time_unix_usec = time_unix_usec; + packet.time_boot_ms = time_boot_ms; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SYSTEM_TIME_MIN_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_SYSTEM_TIME_MIN_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#endif +} + /** * @brief Pack a system_time message on a channel * @param system_id ID of this system @@ -128,6 +164,20 @@ static inline uint16_t mavlink_msg_system_time_encode_chan(uint8_t system_id, ui return mavlink_msg_system_time_pack_chan(system_id, component_id, chan, msg, system_time->time_unix_usec, system_time->time_boot_ms); } +/** + * @brief Encode a system_time struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param system_time C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_system_time_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_system_time_t* system_time) +{ + return mavlink_msg_system_time_pack_status(system_id, component_id, _status, msg, system_time->time_unix_usec, system_time->time_boot_ms); +} + /** * @brief Send a system_time message * @param chan MAVLink channel to send the message @@ -170,7 +220,7 @@ static inline void mavlink_msg_system_time_send_struct(mavlink_channel_t chan, c #if MAVLINK_MSG_ID_SYSTEM_TIME_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_terrain_check.h b/lib/main/MAVLink/common/mavlink_msg_terrain_check.h index 74afd52eeae..d1e4b35e746 100755 --- a/lib/main/MAVLink/common/mavlink_msg_terrain_check.h +++ b/lib/main/MAVLink/common/mavlink_msg_terrain_check.h @@ -69,6 +69,42 @@ static inline uint16_t mavlink_msg_terrain_check_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_CHECK_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); } +/** + * @brief Pack a terrain_check message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param lat [degE7] Latitude + * @param lon [degE7] Longitude + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_check_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + int32_t lat, int32_t lon) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_CHECK_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#else + mavlink_terrain_check_t packet; + packet.lat = lat; + packet.lon = lon; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_CHECK; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TERRAIN_CHECK_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TERRAIN_CHECK_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_CHECK_LEN); +#endif +} + /** * @brief Pack a terrain_check message on a channel * @param system_id ID of this system @@ -128,6 +164,20 @@ static inline uint16_t mavlink_msg_terrain_check_encode_chan(uint8_t system_id, return mavlink_msg_terrain_check_pack_chan(system_id, component_id, chan, msg, terrain_check->lat, terrain_check->lon); } +/** + * @brief Encode a terrain_check struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param terrain_check C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_check_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_terrain_check_t* terrain_check) +{ + return mavlink_msg_terrain_check_pack_status(system_id, component_id, _status, msg, terrain_check->lat, terrain_check->lon); +} + /** * @brief Send a terrain_check message * @param chan MAVLink channel to send the message @@ -170,7 +220,7 @@ static inline void mavlink_msg_terrain_check_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_TERRAIN_CHECK_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_terrain_data.h b/lib/main/MAVLink/common/mavlink_msg_terrain_data.h index 3099502cab1..610c94dbaa3 100755 --- a/lib/main/MAVLink/common/mavlink_msg_terrain_data.h +++ b/lib/main/MAVLink/common/mavlink_msg_terrain_data.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_terrain_data_pack(uint8_t system_id, uint8_t packet.lon = lon; packet.grid_spacing = grid_spacing; packet.gridbit = gridbit; - mav_array_memcpy(packet.data, data, sizeof(int16_t)*16); + mav_array_assign_int16_t(packet.data, data, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); #endif @@ -85,6 +85,49 @@ static inline uint16_t mavlink_msg_terrain_data_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_DATA_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC); } +/** + * @brief Pack a terrain_data message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param lat [degE7] Latitude of SW corner of first grid + * @param lon [degE7] Longitude of SW corner of first grid + * @param grid_spacing [m] Grid spacing + * @param gridbit bit within the terrain request mask + * @param data [m] Terrain data MSL + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_data_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + int32_t lat, int32_t lon, uint16_t grid_spacing, uint8_t gridbit, const int16_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_DATA_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_uint16_t(buf, 8, grid_spacing); + _mav_put_uint8_t(buf, 42, gridbit); + _mav_put_int16_t_array(buf, 10, data, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#else + mavlink_terrain_data_t packet; + packet.lat = lat; + packet.lon = lon; + packet.grid_spacing = grid_spacing; + packet.gridbit = gridbit; + mav_array_memcpy(packet.data, data, sizeof(int16_t)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_DATA; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TERRAIN_DATA_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TERRAIN_DATA_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); +#endif +} + /** * @brief Pack a terrain_data message on a channel * @param system_id ID of this system @@ -116,7 +159,7 @@ static inline uint16_t mavlink_msg_terrain_data_pack_chan(uint8_t system_id, uin packet.lon = lon; packet.grid_spacing = grid_spacing; packet.gridbit = gridbit; - mav_array_memcpy(packet.data, data, sizeof(int16_t)*16); + mav_array_assign_int16_t(packet.data, data, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_DATA_LEN); #endif @@ -151,6 +194,20 @@ static inline uint16_t mavlink_msg_terrain_data_encode_chan(uint8_t system_id, u return mavlink_msg_terrain_data_pack_chan(system_id, component_id, chan, msg, terrain_data->lat, terrain_data->lon, terrain_data->grid_spacing, terrain_data->gridbit, terrain_data->data); } +/** + * @brief Encode a terrain_data struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param terrain_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_data_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_terrain_data_t* terrain_data) +{ + return mavlink_msg_terrain_data_pack_status(system_id, component_id, _status, msg, terrain_data->lat, terrain_data->lon, terrain_data->grid_spacing, terrain_data->gridbit, terrain_data->data); +} + /** * @brief Send a terrain_data message * @param chan MAVLink channel to send the message @@ -179,7 +236,7 @@ static inline void mavlink_msg_terrain_data_send(mavlink_channel_t chan, int32_t packet.lon = lon; packet.grid_spacing = grid_spacing; packet.gridbit = gridbit; - mav_array_memcpy(packet.data, data, sizeof(int16_t)*16); + mav_array_assign_int16_t(packet.data, data, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, (const char *)&packet, MAVLINK_MSG_ID_TERRAIN_DATA_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC); #endif } @@ -200,7 +257,7 @@ static inline void mavlink_msg_terrain_data_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_TERRAIN_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -222,7 +279,7 @@ static inline void mavlink_msg_terrain_data_send_buf(mavlink_message_t *msgbuf, packet->lon = lon; packet->grid_spacing = grid_spacing; packet->gridbit = gridbit; - mav_array_memcpy(packet->data, data, sizeof(int16_t)*16); + mav_array_assign_int16_t(packet->data, data, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TERRAIN_DATA, (const char *)packet, MAVLINK_MSG_ID_TERRAIN_DATA_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_LEN, MAVLINK_MSG_ID_TERRAIN_DATA_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_terrain_report.h b/lib/main/MAVLink/common/mavlink_msg_terrain_report.h index 8757a75a273..b09a95bbbd7 100755 --- a/lib/main/MAVLink/common/mavlink_msg_terrain_report.h +++ b/lib/main/MAVLink/common/mavlink_msg_terrain_report.h @@ -99,6 +99,57 @@ static inline uint16_t mavlink_msg_terrain_report_pack(uint8_t system_id, uint8_ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_REPORT_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN, MAVLINK_MSG_ID_TERRAIN_REPORT_CRC); } +/** + * @brief Pack a terrain_report message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param lat [degE7] Latitude + * @param lon [degE7] Longitude + * @param spacing grid spacing (zero if terrain at this location unavailable) + * @param terrain_height [m] Terrain height MSL + * @param current_height [m] Current vehicle height above lat/lon terrain height + * @param pending Number of 4x4 terrain blocks waiting to be received or read from disk + * @param loaded Number of 4x4 terrain blocks in memory + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_report_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + int32_t lat, int32_t lon, uint16_t spacing, float terrain_height, float current_height, uint16_t pending, uint16_t loaded) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_REPORT_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_float(buf, 8, terrain_height); + _mav_put_float(buf, 12, current_height); + _mav_put_uint16_t(buf, 16, spacing); + _mav_put_uint16_t(buf, 18, pending); + _mav_put_uint16_t(buf, 20, loaded); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#else + mavlink_terrain_report_t packet; + packet.lat = lat; + packet.lon = lon; + packet.terrain_height = terrain_height; + packet.current_height = current_height; + packet.spacing = spacing; + packet.pending = pending; + packet.loaded = loaded; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_REPORT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TERRAIN_REPORT_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN, MAVLINK_MSG_ID_TERRAIN_REPORT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TERRAIN_REPORT_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_REPORT_LEN); +#endif +} + /** * @brief Pack a terrain_report message on a channel * @param system_id ID of this system @@ -173,6 +224,20 @@ static inline uint16_t mavlink_msg_terrain_report_encode_chan(uint8_t system_id, return mavlink_msg_terrain_report_pack_chan(system_id, component_id, chan, msg, terrain_report->lat, terrain_report->lon, terrain_report->spacing, terrain_report->terrain_height, terrain_report->current_height, terrain_report->pending, terrain_report->loaded); } +/** + * @brief Encode a terrain_report struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param terrain_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_report_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_terrain_report_t* terrain_report) +{ + return mavlink_msg_terrain_report_pack_status(system_id, component_id, _status, msg, terrain_report->lat, terrain_report->lon, terrain_report->spacing, terrain_report->terrain_height, terrain_report->current_height, terrain_report->pending, terrain_report->loaded); +} + /** * @brief Send a terrain_report message * @param chan MAVLink channel to send the message @@ -230,7 +295,7 @@ static inline void mavlink_msg_terrain_report_send_struct(mavlink_channel_t chan #if MAVLINK_MSG_ID_TERRAIN_REPORT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_terrain_request.h b/lib/main/MAVLink/common/mavlink_msg_terrain_request.h index 9889edd7d0a..9712e1ca230 100755 --- a/lib/main/MAVLink/common/mavlink_msg_terrain_request.h +++ b/lib/main/MAVLink/common/mavlink_msg_terrain_request.h @@ -81,6 +81,48 @@ static inline uint16_t mavlink_msg_terrain_request_pack(uint8_t system_id, uint8 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TERRAIN_REQUEST_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN, MAVLINK_MSG_ID_TERRAIN_REQUEST_CRC); } +/** + * @brief Pack a terrain_request message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param lat [degE7] Latitude of SW corner of first grid + * @param lon [degE7] Longitude of SW corner of first grid + * @param grid_spacing [m] Grid spacing + * @param mask Bitmask of requested 4x4 grids (row major 8x7 array of grids, 56 bits) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_terrain_request_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + int32_t lat, int32_t lon, uint16_t grid_spacing, uint64_t mask) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN]; + _mav_put_uint64_t(buf, 0, mask); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_uint16_t(buf, 16, grid_spacing); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#else + mavlink_terrain_request_t packet; + packet.mask = mask; + packet.lat = lat; + packet.lon = lon; + packet.grid_spacing = grid_spacing; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TERRAIN_REQUEST; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TERRAIN_REQUEST_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN, MAVLINK_MSG_ID_TERRAIN_REQUEST_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TERRAIN_REQUEST_MIN_LEN, MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN); +#endif +} + /** * @brief Pack a terrain_request message on a channel * @param system_id ID of this system @@ -146,6 +188,20 @@ static inline uint16_t mavlink_msg_terrain_request_encode_chan(uint8_t system_id return mavlink_msg_terrain_request_pack_chan(system_id, component_id, chan, msg, terrain_request->lat, terrain_request->lon, terrain_request->grid_spacing, terrain_request->mask); } +/** + * @brief Encode a terrain_request struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param terrain_request C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_terrain_request_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_terrain_request_t* terrain_request) +{ + return mavlink_msg_terrain_request_pack_status(system_id, component_id, _status, msg, terrain_request->lat, terrain_request->lon, terrain_request->grid_spacing, terrain_request->mask); +} + /** * @brief Send a terrain_request message * @param chan MAVLink channel to send the message @@ -194,7 +250,7 @@ static inline void mavlink_msg_terrain_request_send_struct(mavlink_channel_t cha #if MAVLINK_MSG_ID_TERRAIN_REQUEST_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_time_estimate_to_target.h b/lib/main/MAVLink/common/mavlink_msg_time_estimate_to_target.h index 29d03d59468..95295fba10b 100644 --- a/lib/main/MAVLink/common/mavlink_msg_time_estimate_to_target.h +++ b/lib/main/MAVLink/common/mavlink_msg_time_estimate_to_target.h @@ -87,6 +87,51 @@ static inline uint16_t mavlink_msg_time_estimate_to_target_pack(uint8_t system_i return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_CRC); } +/** + * @brief Pack a time_estimate_to_target message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param safe_return [s] Estimated time to complete the vehicle's configured "safe return" action from its current position (e.g. RTL, Smart RTL, etc.). -1 indicates that the vehicle is landed, or that no time estimate available. + * @param land [s] Estimated time for vehicle to complete the LAND action from its current position. -1 indicates that the vehicle is landed, or that no time estimate available. + * @param mission_next_item [s] Estimated time for reaching/completing the currently active mission item. -1 means no time estimate available. + * @param mission_end [s] Estimated time for completing the current mission. -1 means no mission active and/or no estimate available. + * @param commanded_action [s] Estimated time for completing the current commanded action (i.e. Go To, Takeoff, Land, etc.). -1 means no action active and/or no estimate available. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_time_estimate_to_target_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + int32_t safe_return, int32_t land, int32_t mission_next_item, int32_t mission_end, int32_t commanded_action) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN]; + _mav_put_int32_t(buf, 0, safe_return); + _mav_put_int32_t(buf, 4, land); + _mav_put_int32_t(buf, 8, mission_next_item); + _mav_put_int32_t(buf, 12, mission_end); + _mav_put_int32_t(buf, 16, commanded_action); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN); +#else + mavlink_time_estimate_to_target_t packet; + packet.safe_return = safe_return; + packet.land = land; + packet.mission_next_item = mission_next_item; + packet.mission_end = mission_end; + packet.commanded_action = commanded_action; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN); +#endif +} + /** * @brief Pack a time_estimate_to_target message on a channel * @param system_id ID of this system @@ -155,6 +200,20 @@ static inline uint16_t mavlink_msg_time_estimate_to_target_encode_chan(uint8_t s return mavlink_msg_time_estimate_to_target_pack_chan(system_id, component_id, chan, msg, time_estimate_to_target->safe_return, time_estimate_to_target->land, time_estimate_to_target->mission_next_item, time_estimate_to_target->mission_end, time_estimate_to_target->commanded_action); } +/** + * @brief Encode a time_estimate_to_target struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param time_estimate_to_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_time_estimate_to_target_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_time_estimate_to_target_t* time_estimate_to_target) +{ + return mavlink_msg_time_estimate_to_target_pack_status(system_id, component_id, _status, msg, time_estimate_to_target->safe_return, time_estimate_to_target->land, time_estimate_to_target->mission_next_item, time_estimate_to_target->mission_end, time_estimate_to_target->commanded_action); +} + /** * @brief Send a time_estimate_to_target message * @param chan MAVLink channel to send the message @@ -206,7 +265,7 @@ static inline void mavlink_msg_time_estimate_to_target_send_struct(mavlink_chann #if MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_timesync.h b/lib/main/MAVLink/common/mavlink_msg_timesync.h index a9de47dd457..1a73dfaf150 100755 --- a/lib/main/MAVLink/common/mavlink_msg_timesync.h +++ b/lib/main/MAVLink/common/mavlink_msg_timesync.h @@ -5,13 +5,15 @@ typedef struct __mavlink_timesync_t { - int64_t tc1; /*< Time sync timestamp 1*/ - int64_t ts1; /*< Time sync timestamp 2*/ + int64_t tc1; /*< [ns] Time sync timestamp 1. Syncing: 0. Responding: Timestamp of responding component.*/ + int64_t ts1; /*< [ns] Time sync timestamp 2. Timestamp of syncing component (mirrored in response).*/ + uint8_t target_system; /*< Target system id. Request: 0 (broadcast) or id of specific system. Response must contain system id of the requesting component.*/ + uint8_t target_component; /*< Target component id. Request: 0 (broadcast) or id of specific component. Response must contain component id of the requesting component.*/ } mavlink_timesync_t; -#define MAVLINK_MSG_ID_TIMESYNC_LEN 16 +#define MAVLINK_MSG_ID_TIMESYNC_LEN 18 #define MAVLINK_MSG_ID_TIMESYNC_MIN_LEN 16 -#define MAVLINK_MSG_ID_111_LEN 16 +#define MAVLINK_MSG_ID_111_LEN 18 #define MAVLINK_MSG_ID_111_MIN_LEN 16 #define MAVLINK_MSG_ID_TIMESYNC_CRC 34 @@ -23,17 +25,21 @@ typedef struct __mavlink_timesync_t { #define MAVLINK_MESSAGE_INFO_TIMESYNC { \ 111, \ "TIMESYNC", \ - 2, \ + 4, \ { { "tc1", NULL, MAVLINK_TYPE_INT64_T, 0, 0, offsetof(mavlink_timesync_t, tc1) }, \ { "ts1", NULL, MAVLINK_TYPE_INT64_T, 0, 8, offsetof(mavlink_timesync_t, ts1) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_timesync_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_timesync_t, target_component) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_TIMESYNC { \ "TIMESYNC", \ - 2, \ + 4, \ { { "tc1", NULL, MAVLINK_TYPE_INT64_T, 0, 0, offsetof(mavlink_timesync_t, tc1) }, \ { "ts1", NULL, MAVLINK_TYPE_INT64_T, 0, 8, offsetof(mavlink_timesync_t, ts1) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_timesync_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_timesync_t, target_component) }, \ } \ } #endif @@ -44,23 +50,29 @@ typedef struct __mavlink_timesync_t { * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param tc1 Time sync timestamp 1 - * @param ts1 Time sync timestamp 2 + * @param tc1 [ns] Time sync timestamp 1. Syncing: 0. Responding: Timestamp of responding component. + * @param ts1 [ns] Time sync timestamp 2. Timestamp of syncing component (mirrored in response). + * @param target_system Target system id. Request: 0 (broadcast) or id of specific system. Response must contain system id of the requesting component. + * @param target_component Target component id. Request: 0 (broadcast) or id of specific component. Response must contain component id of the requesting component. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_timesync_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - int64_t tc1, int64_t ts1) + int64_t tc1, int64_t ts1, uint8_t target_system, uint8_t target_component) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_TIMESYNC_LEN]; _mav_put_int64_t(buf, 0, tc1); _mav_put_int64_t(buf, 8, ts1); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TIMESYNC_LEN); #else mavlink_timesync_t packet; packet.tc1 = tc1; packet.ts1 = ts1; + packet.target_system = target_system; + packet.target_component = target_component; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TIMESYNC_LEN); #endif @@ -69,30 +81,78 @@ static inline uint16_t mavlink_msg_timesync_pack(uint8_t system_id, uint8_t comp return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); } +/** + * @brief Pack a timesync message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param tc1 [ns] Time sync timestamp 1. Syncing: 0. Responding: Timestamp of responding component. + * @param ts1 [ns] Time sync timestamp 2. Timestamp of syncing component (mirrored in response). + * @param target_system Target system id. Request: 0 (broadcast) or id of specific system. Response must contain system id of the requesting component. + * @param target_component Target component id. Request: 0 (broadcast) or id of specific component. Response must contain component id of the requesting component. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_timesync_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + int64_t tc1, int64_t ts1, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TIMESYNC_LEN]; + _mav_put_int64_t(buf, 0, tc1); + _mav_put_int64_t(buf, 8, ts1); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TIMESYNC_LEN); +#else + mavlink_timesync_t packet; + packet.tc1 = tc1; + packet.ts1 = ts1; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TIMESYNC_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TIMESYNC; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN); +#endif +} + /** * @brief Pack a timesync message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into - * @param tc1 Time sync timestamp 1 - * @param ts1 Time sync timestamp 2 + * @param tc1 [ns] Time sync timestamp 1. Syncing: 0. Responding: Timestamp of responding component. + * @param ts1 [ns] Time sync timestamp 2. Timestamp of syncing component (mirrored in response). + * @param target_system Target system id. Request: 0 (broadcast) or id of specific system. Response must contain system id of the requesting component. + * @param target_component Target component id. Request: 0 (broadcast) or id of specific component. Response must contain component id of the requesting component. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_timesync_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - int64_t tc1,int64_t ts1) + int64_t tc1,int64_t ts1,uint8_t target_system,uint8_t target_component) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_TIMESYNC_LEN]; _mav_put_int64_t(buf, 0, tc1); _mav_put_int64_t(buf, 8, ts1); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TIMESYNC_LEN); #else mavlink_timesync_t packet; packet.tc1 = tc1; packet.ts1 = ts1; + packet.target_system = target_system; + packet.target_component = target_component; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TIMESYNC_LEN); #endif @@ -111,7 +171,7 @@ static inline uint16_t mavlink_msg_timesync_pack_chan(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_timesync_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_timesync_t* timesync) { - return mavlink_msg_timesync_pack(system_id, component_id, msg, timesync->tc1, timesync->ts1); + return mavlink_msg_timesync_pack(system_id, component_id, msg, timesync->tc1, timesync->ts1, timesync->target_system, timesync->target_component); } /** @@ -125,30 +185,50 @@ static inline uint16_t mavlink_msg_timesync_encode(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_timesync_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_timesync_t* timesync) { - return mavlink_msg_timesync_pack_chan(system_id, component_id, chan, msg, timesync->tc1, timesync->ts1); + return mavlink_msg_timesync_pack_chan(system_id, component_id, chan, msg, timesync->tc1, timesync->ts1, timesync->target_system, timesync->target_component); +} + +/** + * @brief Encode a timesync struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param timesync C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_timesync_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_timesync_t* timesync) +{ + return mavlink_msg_timesync_pack_status(system_id, component_id, _status, msg, timesync->tc1, timesync->ts1, timesync->target_system, timesync->target_component); } /** * @brief Send a timesync message * @param chan MAVLink channel to send the message * - * @param tc1 Time sync timestamp 1 - * @param ts1 Time sync timestamp 2 + * @param tc1 [ns] Time sync timestamp 1. Syncing: 0. Responding: Timestamp of responding component. + * @param ts1 [ns] Time sync timestamp 2. Timestamp of syncing component (mirrored in response). + * @param target_system Target system id. Request: 0 (broadcast) or id of specific system. Response must contain system id of the requesting component. + * @param target_component Target component id. Request: 0 (broadcast) or id of specific component. Response must contain component id of the requesting component. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_timesync_send(mavlink_channel_t chan, int64_t tc1, int64_t ts1) +static inline void mavlink_msg_timesync_send(mavlink_channel_t chan, int64_t tc1, int64_t ts1, uint8_t target_system, uint8_t target_component) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_TIMESYNC_LEN]; _mav_put_int64_t(buf, 0, tc1); _mav_put_int64_t(buf, 8, ts1); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, buf, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); #else mavlink_timesync_t packet; packet.tc1 = tc1; packet.ts1 = ts1; + packet.target_system = target_system; + packet.target_component = target_component; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, (const char *)&packet, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); #endif @@ -162,7 +242,7 @@ static inline void mavlink_msg_timesync_send(mavlink_channel_t chan, int64_t tc1 static inline void mavlink_msg_timesync_send_struct(mavlink_channel_t chan, const mavlink_timesync_t* timesync) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_timesync_send(chan, timesync->tc1, timesync->ts1); + mavlink_msg_timesync_send(chan, timesync->tc1, timesync->ts1, timesync->target_system, timesync->target_component); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, (const char *)timesync, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); #endif @@ -170,24 +250,28 @@ static inline void mavlink_msg_timesync_send_struct(mavlink_channel_t chan, cons #if MAVLINK_MSG_ID_TIMESYNC_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_timesync_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int64_t tc1, int64_t ts1) +static inline void mavlink_msg_timesync_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int64_t tc1, int64_t ts1, uint8_t target_system, uint8_t target_component) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_int64_t(buf, 0, tc1); _mav_put_int64_t(buf, 8, ts1); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, buf, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); #else mavlink_timesync_t *packet = (mavlink_timesync_t *)msgbuf; packet->tc1 = tc1; packet->ts1 = ts1; + packet->target_system = target_system; + packet->target_component = target_component; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIMESYNC, (const char *)packet, MAVLINK_MSG_ID_TIMESYNC_MIN_LEN, MAVLINK_MSG_ID_TIMESYNC_LEN, MAVLINK_MSG_ID_TIMESYNC_CRC); #endif @@ -202,7 +286,7 @@ static inline void mavlink_msg_timesync_send_buf(mavlink_message_t *msgbuf, mavl /** * @brief Get field tc1 from timesync message * - * @return Time sync timestamp 1 + * @return [ns] Time sync timestamp 1. Syncing: 0. Responding: Timestamp of responding component. */ static inline int64_t mavlink_msg_timesync_get_tc1(const mavlink_message_t* msg) { @@ -212,13 +296,33 @@ static inline int64_t mavlink_msg_timesync_get_tc1(const mavlink_message_t* msg) /** * @brief Get field ts1 from timesync message * - * @return Time sync timestamp 2 + * @return [ns] Time sync timestamp 2. Timestamp of syncing component (mirrored in response). */ static inline int64_t mavlink_msg_timesync_get_ts1(const mavlink_message_t* msg) { return _MAV_RETURN_int64_t(msg, 8); } +/** + * @brief Get field target_system from timesync message + * + * @return Target system id. Request: 0 (broadcast) or id of specific system. Response must contain system id of the requesting component. + */ +static inline uint8_t mavlink_msg_timesync_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 16); +} + +/** + * @brief Get field target_component from timesync message + * + * @return Target component id. Request: 0 (broadcast) or id of specific component. Response must contain component id of the requesting component. + */ +static inline uint8_t mavlink_msg_timesync_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 17); +} + /** * @brief Decode a timesync message into a struct * @@ -230,6 +334,8 @@ static inline void mavlink_msg_timesync_decode(const mavlink_message_t* msg, mav #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS timesync->tc1 = mavlink_msg_timesync_get_tc1(msg); timesync->ts1 = mavlink_msg_timesync_get_ts1(msg); + timesync->target_system = mavlink_msg_timesync_get_target_system(msg); + timesync->target_component = mavlink_msg_timesync_get_target_component(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_TIMESYNC_LEN? msg->len : MAVLINK_MSG_ID_TIMESYNC_LEN; memset(timesync, 0, MAVLINK_MSG_ID_TIMESYNC_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_bezier.h b/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_bezier.h index c535fd44162..a6a57b29338 100644 --- a/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_bezier.h +++ b/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_bezier.h @@ -75,6 +75,51 @@ typedef struct __mavlink_trajectory_representation_bezier_t { static inline uint16_t mavlink_msg_trajectory_representation_bezier_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_usec, uint8_t valid_points, const float *pos_x, const float *pos_y, const float *pos_z, const float *delta, const float *pos_yaw) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 108, valid_points); + _mav_put_float_array(buf, 8, pos_x, 5); + _mav_put_float_array(buf, 28, pos_y, 5); + _mav_put_float_array(buf, 48, pos_z, 5); + _mav_put_float_array(buf, 68, delta, 5); + _mav_put_float_array(buf, 88, pos_yaw, 5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN); +#else + mavlink_trajectory_representation_bezier_t packet; + packet.time_usec = time_usec; + packet.valid_points = valid_points; + mav_array_assign_float(packet.pos_x, pos_x, 5); + mav_array_assign_float(packet.pos_y, pos_y, 5); + mav_array_assign_float(packet.pos_z, pos_z, 5); + mav_array_assign_float(packet.delta, delta, 5); + mav_array_assign_float(packet.pos_yaw, pos_yaw, 5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); +} + +/** + * @brief Pack a trajectory_representation_bezier message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param valid_points Number of valid control points (up-to 5 points are possible) + * @param pos_x [m] X-coordinate of bezier control points. Set to NaN if not being used + * @param pos_y [m] Y-coordinate of bezier control points. Set to NaN if not being used + * @param pos_z [m] Z-coordinate of bezier control points. Set to NaN if not being used + * @param delta [s] Bezier time horizon. Set to NaN if velocity/acceleration should not be incorporated + * @param pos_yaw [rad] Yaw. Set to NaN for unchanged + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint8_t valid_points, const float *pos_x, const float *pos_y, const float *pos_z, const float *delta, const float *pos_yaw) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN]; _mav_put_uint64_t(buf, 0, time_usec); @@ -98,7 +143,11 @@ static inline uint16_t mavlink_msg_trajectory_representation_bezier_pack(uint8_t #endif msg->msgid = MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN); +#endif } /** @@ -134,11 +183,11 @@ static inline uint16_t mavlink_msg_trajectory_representation_bezier_pack_chan(ui mavlink_trajectory_representation_bezier_t packet; packet.time_usec = time_usec; packet.valid_points = valid_points; - mav_array_memcpy(packet.pos_x, pos_x, sizeof(float)*5); - mav_array_memcpy(packet.pos_y, pos_y, sizeof(float)*5); - mav_array_memcpy(packet.pos_z, pos_z, sizeof(float)*5); - mav_array_memcpy(packet.delta, delta, sizeof(float)*5); - mav_array_memcpy(packet.pos_yaw, pos_yaw, sizeof(float)*5); + mav_array_assign_float(packet.pos_x, pos_x, 5); + mav_array_assign_float(packet.pos_y, pos_y, 5); + mav_array_assign_float(packet.pos_z, pos_z, 5); + mav_array_assign_float(packet.delta, delta, 5); + mav_array_assign_float(packet.pos_yaw, pos_yaw, 5); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN); #endif @@ -173,6 +222,20 @@ static inline uint16_t mavlink_msg_trajectory_representation_bezier_encode_chan( return mavlink_msg_trajectory_representation_bezier_pack_chan(system_id, component_id, chan, msg, trajectory_representation_bezier->time_usec, trajectory_representation_bezier->valid_points, trajectory_representation_bezier->pos_x, trajectory_representation_bezier->pos_y, trajectory_representation_bezier->pos_z, trajectory_representation_bezier->delta, trajectory_representation_bezier->pos_yaw); } +/** + * @brief Encode a trajectory_representation_bezier struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param trajectory_representation_bezier C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_trajectory_representation_bezier_t* trajectory_representation_bezier) +{ + return mavlink_msg_trajectory_representation_bezier_pack_status(system_id, component_id, _status, msg, trajectory_representation_bezier->time_usec, trajectory_representation_bezier->valid_points, trajectory_representation_bezier->pos_x, trajectory_representation_bezier->pos_y, trajectory_representation_bezier->pos_z, trajectory_representation_bezier->delta, trajectory_representation_bezier->pos_yaw); +} + /** * @brief Send a trajectory_representation_bezier message * @param chan MAVLink channel to send the message @@ -203,11 +266,11 @@ static inline void mavlink_msg_trajectory_representation_bezier_send(mavlink_cha mavlink_trajectory_representation_bezier_t packet; packet.time_usec = time_usec; packet.valid_points = valid_points; - mav_array_memcpy(packet.pos_x, pos_x, sizeof(float)*5); - mav_array_memcpy(packet.pos_y, pos_y, sizeof(float)*5); - mav_array_memcpy(packet.pos_z, pos_z, sizeof(float)*5); - mav_array_memcpy(packet.delta, delta, sizeof(float)*5); - mav_array_memcpy(packet.pos_yaw, pos_yaw, sizeof(float)*5); + mav_array_assign_float(packet.pos_x, pos_x, 5); + mav_array_assign_float(packet.pos_y, pos_y, 5); + mav_array_assign_float(packet.pos_z, pos_z, 5); + mav_array_assign_float(packet.delta, delta, 5); + mav_array_assign_float(packet.pos_yaw, pos_yaw, 5); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER, (const char *)&packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); #endif } @@ -228,7 +291,7 @@ static inline void mavlink_msg_trajectory_representation_bezier_send_struct(mavl #if MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -250,11 +313,11 @@ static inline void mavlink_msg_trajectory_representation_bezier_send_buf(mavlink mavlink_trajectory_representation_bezier_t *packet = (mavlink_trajectory_representation_bezier_t *)msgbuf; packet->time_usec = time_usec; packet->valid_points = valid_points; - mav_array_memcpy(packet->pos_x, pos_x, sizeof(float)*5); - mav_array_memcpy(packet->pos_y, pos_y, sizeof(float)*5); - mav_array_memcpy(packet->pos_z, pos_z, sizeof(float)*5); - mav_array_memcpy(packet->delta, delta, sizeof(float)*5); - mav_array_memcpy(packet->pos_yaw, pos_yaw, sizeof(float)*5); + mav_array_assign_float(packet->pos_x, pos_x, 5); + mav_array_assign_float(packet->pos_y, pos_y, 5); + mav_array_assign_float(packet->pos_z, pos_z, 5); + mav_array_assign_float(packet->delta, delta, 5); + mav_array_assign_float(packet->pos_yaw, pos_yaw, 5); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER, (const char *)packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_waypoints.h b/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_waypoints.h index 470c4626fa7..0581ebaaae5 100644 --- a/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_waypoints.h +++ b/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_waypoints.h @@ -17,7 +17,7 @@ typedef struct __mavlink_trajectory_representation_waypoints_t { float acc_z[5]; /*< [m/s/s] Z-acceleration of waypoint, set to NaN if not being used*/ float pos_yaw[5]; /*< [rad] Yaw angle, set to NaN if not being used*/ float vel_yaw[5]; /*< [rad/s] Yaw rate, set to NaN if not being used*/ - uint16_t command[5]; /*< Scheduled action for each waypoint, UINT16_MAX if not being used.*/ + uint16_t command[5]; /*< MAV_CMD command id of waypoint, set to UINT16_MAX if not being used.*/ uint8_t valid_points; /*< Number of valid points (up-to 5 waypoints are possible)*/ } mavlink_trajectory_representation_waypoints_t; @@ -104,12 +104,78 @@ typedef struct __mavlink_trajectory_representation_waypoints_t { * @param acc_z [m/s/s] Z-acceleration of waypoint, set to NaN if not being used * @param pos_yaw [rad] Yaw angle, set to NaN if not being used * @param vel_yaw [rad/s] Yaw rate, set to NaN if not being used - * @param command Scheduled action for each waypoint, UINT16_MAX if not being used. + * @param command MAV_CMD command id of waypoint, set to UINT16_MAX if not being used. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_trajectory_representation_waypoints_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_usec, uint8_t valid_points, const float *pos_x, const float *pos_y, const float *pos_z, const float *vel_x, const float *vel_y, const float *vel_z, const float *acc_x, const float *acc_y, const float *acc_z, const float *pos_yaw, const float *vel_yaw, const uint16_t *command) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 238, valid_points); + _mav_put_float_array(buf, 8, pos_x, 5); + _mav_put_float_array(buf, 28, pos_y, 5); + _mav_put_float_array(buf, 48, pos_z, 5); + _mav_put_float_array(buf, 68, vel_x, 5); + _mav_put_float_array(buf, 88, vel_y, 5); + _mav_put_float_array(buf, 108, vel_z, 5); + _mav_put_float_array(buf, 128, acc_x, 5); + _mav_put_float_array(buf, 148, acc_y, 5); + _mav_put_float_array(buf, 168, acc_z, 5); + _mav_put_float_array(buf, 188, pos_yaw, 5); + _mav_put_float_array(buf, 208, vel_yaw, 5); + _mav_put_uint16_t_array(buf, 228, command, 5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN); +#else + mavlink_trajectory_representation_waypoints_t packet; + packet.time_usec = time_usec; + packet.valid_points = valid_points; + mav_array_assign_float(packet.pos_x, pos_x, 5); + mav_array_assign_float(packet.pos_y, pos_y, 5); + mav_array_assign_float(packet.pos_z, pos_z, 5); + mav_array_assign_float(packet.vel_x, vel_x, 5); + mav_array_assign_float(packet.vel_y, vel_y, 5); + mav_array_assign_float(packet.vel_z, vel_z, 5); + mav_array_assign_float(packet.acc_x, acc_x, 5); + mav_array_assign_float(packet.acc_y, acc_y, 5); + mav_array_assign_float(packet.acc_z, acc_z, 5); + mav_array_assign_float(packet.pos_yaw, pos_yaw, 5); + mav_array_assign_float(packet.vel_yaw, vel_yaw, 5); + mav_array_assign_uint16_t(packet.command, command, 5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); +} + +/** + * @brief Pack a trajectory_representation_waypoints message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param valid_points Number of valid points (up-to 5 waypoints are possible) + * @param pos_x [m] X-coordinate of waypoint, set to NaN if not being used + * @param pos_y [m] Y-coordinate of waypoint, set to NaN if not being used + * @param pos_z [m] Z-coordinate of waypoint, set to NaN if not being used + * @param vel_x [m/s] X-velocity of waypoint, set to NaN if not being used + * @param vel_y [m/s] Y-velocity of waypoint, set to NaN if not being used + * @param vel_z [m/s] Z-velocity of waypoint, set to NaN if not being used + * @param acc_x [m/s/s] X-acceleration of waypoint, set to NaN if not being used + * @param acc_y [m/s/s] Y-acceleration of waypoint, set to NaN if not being used + * @param acc_z [m/s/s] Z-acceleration of waypoint, set to NaN if not being used + * @param pos_yaw [rad] Yaw angle, set to NaN if not being used + * @param vel_yaw [rad/s] Yaw rate, set to NaN if not being used + * @param command MAV_CMD command id of waypoint, set to UINT16_MAX if not being used. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint8_t valid_points, const float *pos_x, const float *pos_y, const float *pos_z, const float *vel_x, const float *vel_y, const float *vel_z, const float *acc_x, const float *acc_y, const float *acc_z, const float *pos_yaw, const float *vel_yaw, const uint16_t *command) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN]; _mav_put_uint64_t(buf, 0, time_usec); @@ -147,7 +213,11 @@ static inline uint16_t mavlink_msg_trajectory_representation_waypoints_pack(uint #endif msg->msgid = MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN); +#endif } /** @@ -169,7 +239,7 @@ static inline uint16_t mavlink_msg_trajectory_representation_waypoints_pack(uint * @param acc_z [m/s/s] Z-acceleration of waypoint, set to NaN if not being used * @param pos_yaw [rad] Yaw angle, set to NaN if not being used * @param vel_yaw [rad/s] Yaw rate, set to NaN if not being used - * @param command Scheduled action for each waypoint, UINT16_MAX if not being used. + * @param command MAV_CMD command id of waypoint, set to UINT16_MAX if not being used. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_trajectory_representation_waypoints_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -197,18 +267,18 @@ static inline uint16_t mavlink_msg_trajectory_representation_waypoints_pack_chan mavlink_trajectory_representation_waypoints_t packet; packet.time_usec = time_usec; packet.valid_points = valid_points; - mav_array_memcpy(packet.pos_x, pos_x, sizeof(float)*5); - mav_array_memcpy(packet.pos_y, pos_y, sizeof(float)*5); - mav_array_memcpy(packet.pos_z, pos_z, sizeof(float)*5); - mav_array_memcpy(packet.vel_x, vel_x, sizeof(float)*5); - mav_array_memcpy(packet.vel_y, vel_y, sizeof(float)*5); - mav_array_memcpy(packet.vel_z, vel_z, sizeof(float)*5); - mav_array_memcpy(packet.acc_x, acc_x, sizeof(float)*5); - mav_array_memcpy(packet.acc_y, acc_y, sizeof(float)*5); - mav_array_memcpy(packet.acc_z, acc_z, sizeof(float)*5); - mav_array_memcpy(packet.pos_yaw, pos_yaw, sizeof(float)*5); - mav_array_memcpy(packet.vel_yaw, vel_yaw, sizeof(float)*5); - mav_array_memcpy(packet.command, command, sizeof(uint16_t)*5); + mav_array_assign_float(packet.pos_x, pos_x, 5); + mav_array_assign_float(packet.pos_y, pos_y, 5); + mav_array_assign_float(packet.pos_z, pos_z, 5); + mav_array_assign_float(packet.vel_x, vel_x, 5); + mav_array_assign_float(packet.vel_y, vel_y, 5); + mav_array_assign_float(packet.vel_z, vel_z, 5); + mav_array_assign_float(packet.acc_x, acc_x, 5); + mav_array_assign_float(packet.acc_y, acc_y, 5); + mav_array_assign_float(packet.acc_z, acc_z, 5); + mav_array_assign_float(packet.pos_yaw, pos_yaw, 5); + mav_array_assign_float(packet.vel_yaw, vel_yaw, 5); + mav_array_assign_uint16_t(packet.command, command, 5); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN); #endif @@ -243,6 +313,20 @@ static inline uint16_t mavlink_msg_trajectory_representation_waypoints_encode_ch return mavlink_msg_trajectory_representation_waypoints_pack_chan(system_id, component_id, chan, msg, trajectory_representation_waypoints->time_usec, trajectory_representation_waypoints->valid_points, trajectory_representation_waypoints->pos_x, trajectory_representation_waypoints->pos_y, trajectory_representation_waypoints->pos_z, trajectory_representation_waypoints->vel_x, trajectory_representation_waypoints->vel_y, trajectory_representation_waypoints->vel_z, trajectory_representation_waypoints->acc_x, trajectory_representation_waypoints->acc_y, trajectory_representation_waypoints->acc_z, trajectory_representation_waypoints->pos_yaw, trajectory_representation_waypoints->vel_yaw, trajectory_representation_waypoints->command); } +/** + * @brief Encode a trajectory_representation_waypoints struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param trajectory_representation_waypoints C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_trajectory_representation_waypoints_t* trajectory_representation_waypoints) +{ + return mavlink_msg_trajectory_representation_waypoints_pack_status(system_id, component_id, _status, msg, trajectory_representation_waypoints->time_usec, trajectory_representation_waypoints->valid_points, trajectory_representation_waypoints->pos_x, trajectory_representation_waypoints->pos_y, trajectory_representation_waypoints->pos_z, trajectory_representation_waypoints->vel_x, trajectory_representation_waypoints->vel_y, trajectory_representation_waypoints->vel_z, trajectory_representation_waypoints->acc_x, trajectory_representation_waypoints->acc_y, trajectory_representation_waypoints->acc_z, trajectory_representation_waypoints->pos_yaw, trajectory_representation_waypoints->vel_yaw, trajectory_representation_waypoints->command); +} + /** * @brief Send a trajectory_representation_waypoints message * @param chan MAVLink channel to send the message @@ -260,7 +344,7 @@ static inline uint16_t mavlink_msg_trajectory_representation_waypoints_encode_ch * @param acc_z [m/s/s] Z-acceleration of waypoint, set to NaN if not being used * @param pos_yaw [rad] Yaw angle, set to NaN if not being used * @param vel_yaw [rad/s] Yaw rate, set to NaN if not being used - * @param command Scheduled action for each waypoint, UINT16_MAX if not being used. + * @param command MAV_CMD command id of waypoint, set to UINT16_MAX if not being used. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -287,18 +371,18 @@ static inline void mavlink_msg_trajectory_representation_waypoints_send(mavlink_ mavlink_trajectory_representation_waypoints_t packet; packet.time_usec = time_usec; packet.valid_points = valid_points; - mav_array_memcpy(packet.pos_x, pos_x, sizeof(float)*5); - mav_array_memcpy(packet.pos_y, pos_y, sizeof(float)*5); - mav_array_memcpy(packet.pos_z, pos_z, sizeof(float)*5); - mav_array_memcpy(packet.vel_x, vel_x, sizeof(float)*5); - mav_array_memcpy(packet.vel_y, vel_y, sizeof(float)*5); - mav_array_memcpy(packet.vel_z, vel_z, sizeof(float)*5); - mav_array_memcpy(packet.acc_x, acc_x, sizeof(float)*5); - mav_array_memcpy(packet.acc_y, acc_y, sizeof(float)*5); - mav_array_memcpy(packet.acc_z, acc_z, sizeof(float)*5); - mav_array_memcpy(packet.pos_yaw, pos_yaw, sizeof(float)*5); - mav_array_memcpy(packet.vel_yaw, vel_yaw, sizeof(float)*5); - mav_array_memcpy(packet.command, command, sizeof(uint16_t)*5); + mav_array_assign_float(packet.pos_x, pos_x, 5); + mav_array_assign_float(packet.pos_y, pos_y, 5); + mav_array_assign_float(packet.pos_z, pos_z, 5); + mav_array_assign_float(packet.vel_x, vel_x, 5); + mav_array_assign_float(packet.vel_y, vel_y, 5); + mav_array_assign_float(packet.vel_z, vel_z, 5); + mav_array_assign_float(packet.acc_x, acc_x, 5); + mav_array_assign_float(packet.acc_y, acc_y, 5); + mav_array_assign_float(packet.acc_z, acc_z, 5); + mav_array_assign_float(packet.pos_yaw, pos_yaw, 5); + mav_array_assign_float(packet.vel_yaw, vel_yaw, 5); + mav_array_assign_uint16_t(packet.command, command, 5); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS, (const char *)&packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); #endif } @@ -319,7 +403,7 @@ static inline void mavlink_msg_trajectory_representation_waypoints_send_struct(m #if MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -348,18 +432,18 @@ static inline void mavlink_msg_trajectory_representation_waypoints_send_buf(mavl mavlink_trajectory_representation_waypoints_t *packet = (mavlink_trajectory_representation_waypoints_t *)msgbuf; packet->time_usec = time_usec; packet->valid_points = valid_points; - mav_array_memcpy(packet->pos_x, pos_x, sizeof(float)*5); - mav_array_memcpy(packet->pos_y, pos_y, sizeof(float)*5); - mav_array_memcpy(packet->pos_z, pos_z, sizeof(float)*5); - mav_array_memcpy(packet->vel_x, vel_x, sizeof(float)*5); - mav_array_memcpy(packet->vel_y, vel_y, sizeof(float)*5); - mav_array_memcpy(packet->vel_z, vel_z, sizeof(float)*5); - mav_array_memcpy(packet->acc_x, acc_x, sizeof(float)*5); - mav_array_memcpy(packet->acc_y, acc_y, sizeof(float)*5); - mav_array_memcpy(packet->acc_z, acc_z, sizeof(float)*5); - mav_array_memcpy(packet->pos_yaw, pos_yaw, sizeof(float)*5); - mav_array_memcpy(packet->vel_yaw, vel_yaw, sizeof(float)*5); - mav_array_memcpy(packet->command, command, sizeof(uint16_t)*5); + mav_array_assign_float(packet->pos_x, pos_x, 5); + mav_array_assign_float(packet->pos_y, pos_y, 5); + mav_array_assign_float(packet->pos_z, pos_z, 5); + mav_array_assign_float(packet->vel_x, vel_x, 5); + mav_array_assign_float(packet->vel_y, vel_y, 5); + mav_array_assign_float(packet->vel_z, vel_z, 5); + mav_array_assign_float(packet->acc_x, acc_x, 5); + mav_array_assign_float(packet->acc_y, acc_y, 5); + mav_array_assign_float(packet->acc_z, acc_z, 5); + mav_array_assign_float(packet->pos_yaw, pos_yaw, 5); + mav_array_assign_float(packet->vel_yaw, vel_yaw, 5); + mav_array_assign_uint16_t(packet->command, command, 5); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS, (const char *)packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); #endif } @@ -503,7 +587,7 @@ static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_vel_y /** * @brief Get field command from trajectory_representation_waypoints message * - * @return Scheduled action for each waypoint, UINT16_MAX if not being used. + * @return MAV_CMD command id of waypoint, set to UINT16_MAX if not being used. */ static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_command(const mavlink_message_t* msg, uint16_t *command) { diff --git a/lib/main/MAVLink/common/mavlink_msg_tunnel.h b/lib/main/MAVLink/common/mavlink_msg_tunnel.h index 871315882d3..7cb55585349 100644 --- a/lib/main/MAVLink/common/mavlink_msg_tunnel.h +++ b/lib/main/MAVLink/common/mavlink_msg_tunnel.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_tunnel_pack(uint8_t system_id, uint8_t compon packet.target_system = target_system; packet.target_component = target_component; packet.payload_length = payload_length; - mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*128); + mav_array_assign_uint8_t(packet.payload, payload, 128); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TUNNEL_LEN); #endif @@ -85,6 +85,49 @@ static inline uint16_t mavlink_msg_tunnel_pack(uint8_t system_id, uint8_t compon return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN, MAVLINK_MSG_ID_TUNNEL_CRC); } +/** + * @brief Pack a tunnel message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (can be 0 for broadcast, but this is discouraged) + * @param target_component Component ID (can be 0 for broadcast, but this is discouraged) + * @param payload_type A code that identifies the content of the payload (0 for unknown, which is the default). If this code is less than 32768, it is a 'registered' payload type and the corresponding code should be added to the MAV_TUNNEL_PAYLOAD_TYPE enum. Software creators can register blocks of types as needed. Codes greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + * @param payload_length Length of the data transported in payload + * @param payload Variable length payload. The payload length is defined by payload_length. The entire content of this block is opaque unless you understand the encoding specified by payload_type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_tunnel_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t payload_type, uint8_t payload_length, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TUNNEL_LEN]; + _mav_put_uint16_t(buf, 0, payload_type); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, payload_length); + _mav_put_uint8_t_array(buf, 5, payload, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TUNNEL_LEN); +#else + mavlink_tunnel_t packet; + packet.payload_type = payload_type; + packet.target_system = target_system; + packet.target_component = target_component; + packet.payload_length = payload_length; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TUNNEL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TUNNEL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN, MAVLINK_MSG_ID_TUNNEL_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN); +#endif +} + /** * @brief Pack a tunnel message on a channel * @param system_id ID of this system @@ -116,7 +159,7 @@ static inline uint16_t mavlink_msg_tunnel_pack_chan(uint8_t system_id, uint8_t c packet.target_system = target_system; packet.target_component = target_component; packet.payload_length = payload_length; - mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*128); + mav_array_assign_uint8_t(packet.payload, payload, 128); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TUNNEL_LEN); #endif @@ -151,6 +194,20 @@ static inline uint16_t mavlink_msg_tunnel_encode_chan(uint8_t system_id, uint8_t return mavlink_msg_tunnel_pack_chan(system_id, component_id, chan, msg, tunnel->target_system, tunnel->target_component, tunnel->payload_type, tunnel->payload_length, tunnel->payload); } +/** + * @brief Encode a tunnel struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param tunnel C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_tunnel_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_tunnel_t* tunnel) +{ + return mavlink_msg_tunnel_pack_status(system_id, component_id, _status, msg, tunnel->target_system, tunnel->target_component, tunnel->payload_type, tunnel->payload_length, tunnel->payload); +} + /** * @brief Send a tunnel message * @param chan MAVLink channel to send the message @@ -179,7 +236,7 @@ static inline void mavlink_msg_tunnel_send(mavlink_channel_t chan, uint8_t targe packet.target_system = target_system; packet.target_component = target_component; packet.payload_length = payload_length; - mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*128); + mav_array_assign_uint8_t(packet.payload, payload, 128); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TUNNEL, (const char *)&packet, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN, MAVLINK_MSG_ID_TUNNEL_CRC); #endif } @@ -200,7 +257,7 @@ static inline void mavlink_msg_tunnel_send_struct(mavlink_channel_t chan, const #if MAVLINK_MSG_ID_TUNNEL_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -222,7 +279,7 @@ static inline void mavlink_msg_tunnel_send_buf(mavlink_message_t *msgbuf, mavlin packet->target_system = target_system; packet->target_component = target_component; packet->payload_length = payload_length; - mav_array_memcpy(packet->payload, payload, sizeof(uint8_t)*128); + mav_array_assign_uint8_t(packet->payload, payload, 128); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TUNNEL, (const char *)packet, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN, MAVLINK_MSG_ID_TUNNEL_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_uavcan_node_info.h b/lib/main/MAVLink/common/mavlink_msg_uavcan_node_info.h index a042c5c4e5f..d388a054888 100644 --- a/lib/main/MAVLink/common/mavlink_msg_uavcan_node_info.h +++ b/lib/main/MAVLink/common/mavlink_msg_uavcan_node_info.h @@ -7,7 +7,7 @@ typedef struct __mavlink_uavcan_node_info_t { uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ uint32_t uptime_sec; /*< [s] Time since the start-up of the node.*/ - uint32_t sw_vcs_commit; /*< Version control system (VCS) revision identifier (e.g. git short commit hash). Zero if unknown.*/ + uint32_t sw_vcs_commit; /*< Version control system (VCS) revision identifier (e.g. git short commit hash). 0 if unknown.*/ char name[80]; /*< Node name string. For example, "sapog.px4.io".*/ uint8_t hw_version_major; /*< Hardware major version number.*/ uint8_t hw_version_minor; /*< Hardware minor version number.*/ @@ -74,12 +74,63 @@ typedef struct __mavlink_uavcan_node_info_t { * @param hw_unique_id Hardware unique 128-bit ID. * @param sw_version_major Software major version number. * @param sw_version_minor Software minor version number. - * @param sw_vcs_commit Version control system (VCS) revision identifier (e.g. git short commit hash). Zero if unknown. + * @param sw_vcs_commit Version control system (VCS) revision identifier (e.g. git short commit hash). 0 if unknown. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_uavcan_node_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_usec, uint32_t uptime_sec, const char *name, uint8_t hw_version_major, uint8_t hw_version_minor, const uint8_t *hw_unique_id, uint8_t sw_version_major, uint8_t sw_version_minor, uint32_t sw_vcs_commit) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime_sec); + _mav_put_uint32_t(buf, 12, sw_vcs_commit); + _mav_put_uint8_t(buf, 96, hw_version_major); + _mav_put_uint8_t(buf, 97, hw_version_minor); + _mav_put_uint8_t(buf, 114, sw_version_major); + _mav_put_uint8_t(buf, 115, sw_version_minor); + _mav_put_char_array(buf, 16, name, 80); + _mav_put_uint8_t_array(buf, 98, hw_unique_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN); +#else + mavlink_uavcan_node_info_t packet; + packet.time_usec = time_usec; + packet.uptime_sec = uptime_sec; + packet.sw_vcs_commit = sw_vcs_commit; + packet.hw_version_major = hw_version_major; + packet.hw_version_minor = hw_version_minor; + packet.sw_version_major = sw_version_major; + packet.sw_version_minor = sw_version_minor; + mav_array_assign_char(packet.name, name, 80); + mav_array_assign_uint8_t(packet.hw_unique_id, hw_unique_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_UAVCAN_NODE_INFO; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); +} + +/** + * @brief Pack a uavcan_node_info message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime_sec [s] Time since the start-up of the node. + * @param name Node name string. For example, "sapog.px4.io". + * @param hw_version_major Hardware major version number. + * @param hw_version_minor Hardware minor version number. + * @param hw_unique_id Hardware unique 128-bit ID. + * @param sw_version_major Software major version number. + * @param sw_version_minor Software minor version number. + * @param sw_vcs_commit Version control system (VCS) revision identifier (e.g. git short commit hash). 0 if unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_uavcan_node_info_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint32_t uptime_sec, const char *name, uint8_t hw_version_major, uint8_t hw_version_minor, const uint8_t *hw_unique_id, uint8_t sw_version_major, uint8_t sw_version_minor, uint32_t sw_vcs_commit) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN]; _mav_put_uint64_t(buf, 0, time_usec); @@ -107,7 +158,11 @@ static inline uint16_t mavlink_msg_uavcan_node_info_pack(uint8_t system_id, uint #endif msg->msgid = MAVLINK_MSG_ID_UAVCAN_NODE_INFO; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN); +#endif } /** @@ -124,7 +179,7 @@ static inline uint16_t mavlink_msg_uavcan_node_info_pack(uint8_t system_id, uint * @param hw_unique_id Hardware unique 128-bit ID. * @param sw_version_major Software major version number. * @param sw_version_minor Software minor version number. - * @param sw_vcs_commit Version control system (VCS) revision identifier (e.g. git short commit hash). Zero if unknown. + * @param sw_vcs_commit Version control system (VCS) revision identifier (e.g. git short commit hash). 0 if unknown. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_uavcan_node_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -152,8 +207,8 @@ static inline uint16_t mavlink_msg_uavcan_node_info_pack_chan(uint8_t system_id, packet.hw_version_minor = hw_version_minor; packet.sw_version_major = sw_version_major; packet.sw_version_minor = sw_version_minor; - mav_array_memcpy(packet.name, name, sizeof(char)*80); - mav_array_memcpy(packet.hw_unique_id, hw_unique_id, sizeof(uint8_t)*16); + mav_array_assign_char(packet.name, name, 80); + mav_array_assign_uint8_t(packet.hw_unique_id, hw_unique_id, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN); #endif @@ -188,6 +243,20 @@ static inline uint16_t mavlink_msg_uavcan_node_info_encode_chan(uint8_t system_i return mavlink_msg_uavcan_node_info_pack_chan(system_id, component_id, chan, msg, uavcan_node_info->time_usec, uavcan_node_info->uptime_sec, uavcan_node_info->name, uavcan_node_info->hw_version_major, uavcan_node_info->hw_version_minor, uavcan_node_info->hw_unique_id, uavcan_node_info->sw_version_major, uavcan_node_info->sw_version_minor, uavcan_node_info->sw_vcs_commit); } +/** + * @brief Encode a uavcan_node_info struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param uavcan_node_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_uavcan_node_info_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_uavcan_node_info_t* uavcan_node_info) +{ + return mavlink_msg_uavcan_node_info_pack_status(system_id, component_id, _status, msg, uavcan_node_info->time_usec, uavcan_node_info->uptime_sec, uavcan_node_info->name, uavcan_node_info->hw_version_major, uavcan_node_info->hw_version_minor, uavcan_node_info->hw_unique_id, uavcan_node_info->sw_version_major, uavcan_node_info->sw_version_minor, uavcan_node_info->sw_vcs_commit); +} + /** * @brief Send a uavcan_node_info message * @param chan MAVLink channel to send the message @@ -200,7 +269,7 @@ static inline uint16_t mavlink_msg_uavcan_node_info_encode_chan(uint8_t system_i * @param hw_unique_id Hardware unique 128-bit ID. * @param sw_version_major Software major version number. * @param sw_version_minor Software minor version number. - * @param sw_vcs_commit Version control system (VCS) revision identifier (e.g. git short commit hash). Zero if unknown. + * @param sw_vcs_commit Version control system (VCS) revision identifier (e.g. git short commit hash). 0 if unknown. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -227,8 +296,8 @@ static inline void mavlink_msg_uavcan_node_info_send(mavlink_channel_t chan, uin packet.hw_version_minor = hw_version_minor; packet.sw_version_major = sw_version_major; packet.sw_version_minor = sw_version_minor; - mav_array_memcpy(packet.name, name, sizeof(char)*80); - mav_array_memcpy(packet.hw_unique_id, hw_unique_id, sizeof(uint8_t)*16); + mav_array_assign_char(packet.name, name, 80); + mav_array_assign_uint8_t(packet.hw_unique_id, hw_unique_id, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_INFO, (const char *)&packet, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); #endif } @@ -249,7 +318,7 @@ static inline void mavlink_msg_uavcan_node_info_send_struct(mavlink_channel_t ch #if MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -278,8 +347,8 @@ static inline void mavlink_msg_uavcan_node_info_send_buf(mavlink_message_t *msgb packet->hw_version_minor = hw_version_minor; packet->sw_version_major = sw_version_major; packet->sw_version_minor = sw_version_minor; - mav_array_memcpy(packet->name, name, sizeof(char)*80); - mav_array_memcpy(packet->hw_unique_id, hw_unique_id, sizeof(uint8_t)*16); + mav_array_assign_char(packet->name, name, 80); + mav_array_assign_uint8_t(packet->hw_unique_id, hw_unique_id, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_INFO, (const char *)packet, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); #endif } @@ -373,7 +442,7 @@ static inline uint8_t mavlink_msg_uavcan_node_info_get_sw_version_minor(const ma /** * @brief Get field sw_vcs_commit from uavcan_node_info message * - * @return Version control system (VCS) revision identifier (e.g. git short commit hash). Zero if unknown. + * @return Version control system (VCS) revision identifier (e.g. git short commit hash). 0 if unknown. */ static inline uint32_t mavlink_msg_uavcan_node_info_get_sw_vcs_commit(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_uavcan_node_status.h b/lib/main/MAVLink/common/mavlink_msg_uavcan_node_status.h index 6f1d8d8fd3f..743f31c46bb 100644 --- a/lib/main/MAVLink/common/mavlink_msg_uavcan_node_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_uavcan_node_status.h @@ -93,6 +93,54 @@ static inline uint16_t mavlink_msg_uavcan_node_status_pack(uint8_t system_id, ui return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_CRC); } +/** + * @brief Pack a uavcan_node_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime_sec [s] Time since the start-up of the node. + * @param health Generalized node health status. + * @param mode Generalized operating mode. + * @param sub_mode Not used currently. + * @param vendor_specific_status_code Vendor-specific status information. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_uavcan_node_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint32_t uptime_sec, uint8_t health, uint8_t mode, uint8_t sub_mode, uint16_t vendor_specific_status_code) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime_sec); + _mav_put_uint16_t(buf, 12, vendor_specific_status_code); + _mav_put_uint8_t(buf, 14, health); + _mav_put_uint8_t(buf, 15, mode); + _mav_put_uint8_t(buf, 16, sub_mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN); +#else + mavlink_uavcan_node_status_t packet; + packet.time_usec = time_usec; + packet.uptime_sec = uptime_sec; + packet.vendor_specific_status_code = vendor_specific_status_code; + packet.health = health; + packet.mode = mode; + packet.sub_mode = sub_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_UAVCAN_NODE_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN); +#endif +} + /** * @brief Pack a uavcan_node_status message on a channel * @param system_id ID of this system @@ -164,6 +212,20 @@ static inline uint16_t mavlink_msg_uavcan_node_status_encode_chan(uint8_t system return mavlink_msg_uavcan_node_status_pack_chan(system_id, component_id, chan, msg, uavcan_node_status->time_usec, uavcan_node_status->uptime_sec, uavcan_node_status->health, uavcan_node_status->mode, uavcan_node_status->sub_mode, uavcan_node_status->vendor_specific_status_code); } +/** + * @brief Encode a uavcan_node_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param uavcan_node_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_uavcan_node_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_uavcan_node_status_t* uavcan_node_status) +{ + return mavlink_msg_uavcan_node_status_pack_status(system_id, component_id, _status, msg, uavcan_node_status->time_usec, uavcan_node_status->uptime_sec, uavcan_node_status->health, uavcan_node_status->mode, uavcan_node_status->sub_mode, uavcan_node_status->vendor_specific_status_code); +} + /** * @brief Send a uavcan_node_status message * @param chan MAVLink channel to send the message @@ -218,7 +280,7 @@ static inline void mavlink_msg_uavcan_node_status_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_utm_global_position.h b/lib/main/MAVLink/common/mavlink_msg_utm_global_position.h index 0c8ee72263a..2e257cc1af0 100644 --- a/lib/main/MAVLink/common/mavlink_msg_utm_global_position.h +++ b/lib/main/MAVLink/common/mavlink_msg_utm_global_position.h @@ -155,7 +155,7 @@ static inline uint16_t mavlink_msg_utm_global_position_pack(uint8_t system_id, u packet.update_rate = update_rate; packet.flight_state = flight_state; packet.flags = flags; - mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*18); + mav_array_assign_uint8_t(packet.uas_id, uas_id, 18); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN); #endif @@ -163,6 +163,88 @@ static inline uint16_t mavlink_msg_utm_global_position_pack(uint8_t system_id, u return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC); } +/** + * @brief Pack a utm_global_position message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time [us] Time of applicability of position (microseconds since UNIX epoch). + * @param uas_id Unique UAS ID. + * @param lat [degE7] Latitude (WGS84) + * @param lon [degE7] Longitude (WGS84) + * @param alt [mm] Altitude (WGS84) + * @param relative_alt [mm] Altitude above ground + * @param vx [cm/s] Ground X speed (latitude, positive north) + * @param vy [cm/s] Ground Y speed (longitude, positive east) + * @param vz [cm/s] Ground Z speed (altitude, positive down) + * @param h_acc [mm] Horizontal position uncertainty (standard deviation) + * @param v_acc [mm] Altitude uncertainty (standard deviation) + * @param vel_acc [cm/s] Speed uncertainty (standard deviation) + * @param next_lat [degE7] Next waypoint, latitude (WGS84) + * @param next_lon [degE7] Next waypoint, longitude (WGS84) + * @param next_alt [mm] Next waypoint, altitude (WGS84) + * @param update_rate [cs] Time until next update. Set to 0 if unknown or in data driven mode. + * @param flight_state Flight state + * @param flags Bitwise OR combination of the data available flags. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_utm_global_position_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time, const uint8_t *uas_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t h_acc, uint16_t v_acc, uint16_t vel_acc, int32_t next_lat, int32_t next_lon, int32_t next_alt, uint16_t update_rate, uint8_t flight_state, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN]; + _mav_put_uint64_t(buf, 0, time); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_int32_t(buf, 20, relative_alt); + _mav_put_int32_t(buf, 24, next_lat); + _mav_put_int32_t(buf, 28, next_lon); + _mav_put_int32_t(buf, 32, next_alt); + _mav_put_int16_t(buf, 36, vx); + _mav_put_int16_t(buf, 38, vy); + _mav_put_int16_t(buf, 40, vz); + _mav_put_uint16_t(buf, 42, h_acc); + _mav_put_uint16_t(buf, 44, v_acc); + _mav_put_uint16_t(buf, 46, vel_acc); + _mav_put_uint16_t(buf, 48, update_rate); + _mav_put_uint8_t(buf, 68, flight_state); + _mav_put_uint8_t(buf, 69, flags); + _mav_put_uint8_t_array(buf, 50, uas_id, 18); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN); +#else + mavlink_utm_global_position_t packet; + packet.time = time; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.next_lat = next_lat; + packet.next_lon = next_lon; + packet.next_alt = next_alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_acc = vel_acc; + packet.update_rate = update_rate; + packet.flight_state = flight_state; + packet.flags = flags; + mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*18); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_UTM_GLOBAL_POSITION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN); +#endif +} + /** * @brief Pack a utm_global_position message on a channel * @param system_id ID of this system @@ -233,7 +315,7 @@ static inline uint16_t mavlink_msg_utm_global_position_pack_chan(uint8_t system_ packet.update_rate = update_rate; packet.flight_state = flight_state; packet.flags = flags; - mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*18); + mav_array_assign_uint8_t(packet.uas_id, uas_id, 18); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN); #endif @@ -268,6 +350,20 @@ static inline uint16_t mavlink_msg_utm_global_position_encode_chan(uint8_t syste return mavlink_msg_utm_global_position_pack_chan(system_id, component_id, chan, msg, utm_global_position->time, utm_global_position->uas_id, utm_global_position->lat, utm_global_position->lon, utm_global_position->alt, utm_global_position->relative_alt, utm_global_position->vx, utm_global_position->vy, utm_global_position->vz, utm_global_position->h_acc, utm_global_position->v_acc, utm_global_position->vel_acc, utm_global_position->next_lat, utm_global_position->next_lon, utm_global_position->next_alt, utm_global_position->update_rate, utm_global_position->flight_state, utm_global_position->flags); } +/** + * @brief Encode a utm_global_position struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param utm_global_position C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_utm_global_position_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_utm_global_position_t* utm_global_position) +{ + return mavlink_msg_utm_global_position_pack_status(system_id, component_id, _status, msg, utm_global_position->time, utm_global_position->uas_id, utm_global_position->lat, utm_global_position->lon, utm_global_position->alt, utm_global_position->relative_alt, utm_global_position->vx, utm_global_position->vy, utm_global_position->vz, utm_global_position->h_acc, utm_global_position->v_acc, utm_global_position->vel_acc, utm_global_position->next_lat, utm_global_position->next_lon, utm_global_position->next_alt, utm_global_position->update_rate, utm_global_position->flight_state, utm_global_position->flags); +} + /** * @brief Send a utm_global_position message * @param chan MAVLink channel to send the message @@ -335,7 +431,7 @@ static inline void mavlink_msg_utm_global_position_send(mavlink_channel_t chan, packet.update_rate = update_rate; packet.flight_state = flight_state; packet.flags = flags; - mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*18); + mav_array_assign_uint8_t(packet.uas_id, uas_id, 18); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, (const char *)&packet, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC); #endif } @@ -356,7 +452,7 @@ static inline void mavlink_msg_utm_global_position_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -404,7 +500,7 @@ static inline void mavlink_msg_utm_global_position_send_buf(mavlink_message_t *m packet->update_rate = update_rate; packet->flight_state = flight_state; packet->flags = flags; - mav_array_memcpy(packet->uas_id, uas_id, sizeof(uint8_t)*18); + mav_array_assign_uint8_t(packet->uas_id, uas_id, 18); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, (const char *)packet, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_v2_extension.h b/lib/main/MAVLink/common/mavlink_msg_v2_extension.h index 689e891146a..4eff8fc9420 100755 --- a/lib/main/MAVLink/common/mavlink_msg_v2_extension.h +++ b/lib/main/MAVLink/common/mavlink_msg_v2_extension.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_v2_extension_pack(uint8_t system_id, uint8_t packet.target_network = target_network; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*249); + mav_array_assign_uint8_t(packet.payload, payload, 249); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_V2_EXTENSION_LEN); #endif @@ -85,6 +85,49 @@ static inline uint16_t mavlink_msg_v2_extension_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_V2_EXTENSION_MIN_LEN, MAVLINK_MSG_ID_V2_EXTENSION_LEN, MAVLINK_MSG_ID_V2_EXTENSION_CRC); } +/** + * @brief Pack a v2_extension message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param target_network Network ID (0 for broadcast) + * @param target_system System ID (0 for broadcast) + * @param target_component Component ID (0 for broadcast) + * @param message_type A code that identifies the software component that understands this message (analogous to USB device classes or mime type strings). If this code is less than 32768, it is considered a 'registered' protocol extension and the corresponding entry should be added to https://github.com/mavlink/mavlink/definition_files/extension_message_ids.xml. Software creators can register blocks of message IDs as needed (useful for GCS specific metadata, etc...). Message_types greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + * @param payload Variable length payload. The length must be encoded in the payload as part of the message_type protocol, e.g. by including the length as payload data, or by terminating the payload data with a non-zero marker. This is required in order to reconstruct zero-terminated payloads that are (or otherwise would be) trimmed by MAVLink 2 empty-byte truncation. The entire content of the payload block is opaque unless you understand the encoding message_type. The particular encoding used can be extension specific and might not always be documented as part of the MAVLink specification. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_v2_extension_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t target_network, uint8_t target_system, uint8_t target_component, uint16_t message_type, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_V2_EXTENSION_LEN]; + _mav_put_uint16_t(buf, 0, message_type); + _mav_put_uint8_t(buf, 2, target_network); + _mav_put_uint8_t(buf, 3, target_system); + _mav_put_uint8_t(buf, 4, target_component); + _mav_put_uint8_t_array(buf, 5, payload, 249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#else + mavlink_v2_extension_t packet; + packet.message_type = message_type; + packet.target_network = target_network; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_V2_EXTENSION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_V2_EXTENSION_MIN_LEN, MAVLINK_MSG_ID_V2_EXTENSION_LEN, MAVLINK_MSG_ID_V2_EXTENSION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_V2_EXTENSION_MIN_LEN, MAVLINK_MSG_ID_V2_EXTENSION_LEN); +#endif +} + /** * @brief Pack a v2_extension message on a channel * @param system_id ID of this system @@ -116,7 +159,7 @@ static inline uint16_t mavlink_msg_v2_extension_pack_chan(uint8_t system_id, uin packet.target_network = target_network; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*249); + mav_array_assign_uint8_t(packet.payload, payload, 249); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_V2_EXTENSION_LEN); #endif @@ -151,6 +194,20 @@ static inline uint16_t mavlink_msg_v2_extension_encode_chan(uint8_t system_id, u return mavlink_msg_v2_extension_pack_chan(system_id, component_id, chan, msg, v2_extension->target_network, v2_extension->target_system, v2_extension->target_component, v2_extension->message_type, v2_extension->payload); } +/** + * @brief Encode a v2_extension struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param v2_extension C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_v2_extension_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_v2_extension_t* v2_extension) +{ + return mavlink_msg_v2_extension_pack_status(system_id, component_id, _status, msg, v2_extension->target_network, v2_extension->target_system, v2_extension->target_component, v2_extension->message_type, v2_extension->payload); +} + /** * @brief Send a v2_extension message * @param chan MAVLink channel to send the message @@ -179,7 +236,7 @@ static inline void mavlink_msg_v2_extension_send(mavlink_channel_t chan, uint8_t packet.target_network = target_network; packet.target_system = target_system; packet.target_component = target_component; - mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*249); + mav_array_assign_uint8_t(packet.payload, payload, 249); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_V2_EXTENSION, (const char *)&packet, MAVLINK_MSG_ID_V2_EXTENSION_MIN_LEN, MAVLINK_MSG_ID_V2_EXTENSION_LEN, MAVLINK_MSG_ID_V2_EXTENSION_CRC); #endif } @@ -200,7 +257,7 @@ static inline void mavlink_msg_v2_extension_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_V2_EXTENSION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -222,7 +279,7 @@ static inline void mavlink_msg_v2_extension_send_buf(mavlink_message_t *msgbuf, packet->target_network = target_network; packet->target_system = target_system; packet->target_component = target_component; - mav_array_memcpy(packet->payload, payload, sizeof(uint8_t)*249); + mav_array_assign_uint8_t(packet->payload, payload, 249); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_V2_EXTENSION, (const char *)packet, MAVLINK_MSG_ID_V2_EXTENSION_MIN_LEN, MAVLINK_MSG_ID_V2_EXTENSION_LEN, MAVLINK_MSG_ID_V2_EXTENSION_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_vfr_hud.h b/lib/main/MAVLink/common/mavlink_msg_vfr_hud.h index f1706ea65c6..5597261f035 100755 --- a/lib/main/MAVLink/common/mavlink_msg_vfr_hud.h +++ b/lib/main/MAVLink/common/mavlink_msg_vfr_hud.h @@ -93,6 +93,54 @@ static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t compo return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VFR_HUD_MIN_LEN, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); } +/** + * @brief Pack a vfr_hud message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param airspeed [m/s] Vehicle speed in form appropriate for vehicle type. For standard aircraft this is typically calibrated airspeed (CAS) or indicated airspeed (IAS) - either of which can be used by a pilot to estimate stall speed. + * @param groundspeed [m/s] Current ground speed. + * @param heading [deg] Current heading in compass units (0-360, 0=north). + * @param throttle [%] Current throttle setting (0 to 100). + * @param alt [m] Current altitude (MSL). + * @param climb [m/s] Current climb rate. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_vfr_hud_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VFR_HUD_LEN]; + _mav_put_float(buf, 0, airspeed); + _mav_put_float(buf, 4, groundspeed); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, climb); + _mav_put_int16_t(buf, 16, heading); + _mav_put_uint16_t(buf, 18, throttle); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VFR_HUD_LEN); +#else + mavlink_vfr_hud_t packet; + packet.airspeed = airspeed; + packet.groundspeed = groundspeed; + packet.alt = alt; + packet.climb = climb; + packet.heading = heading; + packet.throttle = throttle; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VFR_HUD_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VFR_HUD; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_VFR_HUD_MIN_LEN, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_VFR_HUD_MIN_LEN, MAVLINK_MSG_ID_VFR_HUD_LEN); +#endif +} + /** * @brief Pack a vfr_hud message on a channel * @param system_id ID of this system @@ -164,6 +212,20 @@ static inline uint16_t mavlink_msg_vfr_hud_encode_chan(uint8_t system_id, uint8_ return mavlink_msg_vfr_hud_pack_chan(system_id, component_id, chan, msg, vfr_hud->airspeed, vfr_hud->groundspeed, vfr_hud->heading, vfr_hud->throttle, vfr_hud->alt, vfr_hud->climb); } +/** + * @brief Encode a vfr_hud struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param vfr_hud C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vfr_hud_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_vfr_hud_t* vfr_hud) +{ + return mavlink_msg_vfr_hud_pack_status(system_id, component_id, _status, msg, vfr_hud->airspeed, vfr_hud->groundspeed, vfr_hud->heading, vfr_hud->throttle, vfr_hud->alt, vfr_hud->climb); +} + /** * @brief Send a vfr_hud message * @param chan MAVLink channel to send the message @@ -218,7 +280,7 @@ static inline void mavlink_msg_vfr_hud_send_struct(mavlink_channel_t chan, const #if MAVLINK_MSG_ID_VFR_HUD_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_vibration.h b/lib/main/MAVLink/common/mavlink_msg_vibration.h index acc99c4f63f..b7a29f1faf3 100755 --- a/lib/main/MAVLink/common/mavlink_msg_vibration.h +++ b/lib/main/MAVLink/common/mavlink_msg_vibration.h @@ -99,6 +99,57 @@ static inline uint16_t mavlink_msg_vibration_pack(uint8_t system_id, uint8_t com return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VIBRATION_MIN_LEN, MAVLINK_MSG_ID_VIBRATION_LEN, MAVLINK_MSG_ID_VIBRATION_CRC); } +/** + * @brief Pack a vibration message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param vibration_x Vibration levels on X-axis + * @param vibration_y Vibration levels on Y-axis + * @param vibration_z Vibration levels on Z-axis + * @param clipping_0 first accelerometer clipping count + * @param clipping_1 second accelerometer clipping count + * @param clipping_2 third accelerometer clipping count + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_vibration_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, float vibration_x, float vibration_y, float vibration_z, uint32_t clipping_0, uint32_t clipping_1, uint32_t clipping_2) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VIBRATION_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, vibration_x); + _mav_put_float(buf, 12, vibration_y); + _mav_put_float(buf, 16, vibration_z); + _mav_put_uint32_t(buf, 20, clipping_0); + _mav_put_uint32_t(buf, 24, clipping_1); + _mav_put_uint32_t(buf, 28, clipping_2); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VIBRATION_LEN); +#else + mavlink_vibration_t packet; + packet.time_usec = time_usec; + packet.vibration_x = vibration_x; + packet.vibration_y = vibration_y; + packet.vibration_z = vibration_z; + packet.clipping_0 = clipping_0; + packet.clipping_1 = clipping_1; + packet.clipping_2 = clipping_2; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VIBRATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VIBRATION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_VIBRATION_MIN_LEN, MAVLINK_MSG_ID_VIBRATION_LEN, MAVLINK_MSG_ID_VIBRATION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_VIBRATION_MIN_LEN, MAVLINK_MSG_ID_VIBRATION_LEN); +#endif +} + /** * @brief Pack a vibration message on a channel * @param system_id ID of this system @@ -173,6 +224,20 @@ static inline uint16_t mavlink_msg_vibration_encode_chan(uint8_t system_id, uint return mavlink_msg_vibration_pack_chan(system_id, component_id, chan, msg, vibration->time_usec, vibration->vibration_x, vibration->vibration_y, vibration->vibration_z, vibration->clipping_0, vibration->clipping_1, vibration->clipping_2); } +/** + * @brief Encode a vibration struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param vibration C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vibration_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_vibration_t* vibration) +{ + return mavlink_msg_vibration_pack_status(system_id, component_id, _status, msg, vibration->time_usec, vibration->vibration_x, vibration->vibration_y, vibration->vibration_z, vibration->clipping_0, vibration->clipping_1, vibration->clipping_2); +} + /** * @brief Send a vibration message * @param chan MAVLink channel to send the message @@ -230,7 +295,7 @@ static inline void mavlink_msg_vibration_send_struct(mavlink_channel_t chan, con #if MAVLINK_MSG_ID_VIBRATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_vicon_position_estimate.h b/lib/main/MAVLink/common/mavlink_msg_vicon_position_estimate.h index 753f526037d..8a5980460c9 100755 --- a/lib/main/MAVLink/common/mavlink_msg_vicon_position_estimate.h +++ b/lib/main/MAVLink/common/mavlink_msg_vicon_position_estimate.h @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); + mav_array_assign_float(packet.covariance, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); #endif @@ -103,6 +103,58 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); } +/** + * @brief Pack a vicon_position_estimate message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param usec [us] Timestamp (UNIX time or time since system boot) + * @param x [m] Global X position + * @param y [m] Global Y position + * @param z [m] Global Z position + * @param roll [rad] Roll angle + * @param pitch [rad] Pitch angle + * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_vicon_position_estimate_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + _mav_put_float_array(buf, 32, covariance, 21); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#else + mavlink_vicon_position_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#endif +} + /** * @brief Pack a vicon_position_estimate message on a channel * @param system_id ID of this system @@ -143,7 +195,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t sys packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); + mav_array_assign_float(packet.covariance, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); #endif @@ -178,6 +230,20 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode_chan(uint8_t s return mavlink_msg_vicon_position_estimate_pack_chan(system_id, component_id, chan, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw, vicon_position_estimate->covariance); } +/** + * @brief Encode a vicon_position_estimate struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param vicon_position_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vicon_position_estimate_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_vicon_position_estimate_t* vicon_position_estimate) +{ + return mavlink_msg_vicon_position_estimate_pack_status(system_id, component_id, _status, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw, vicon_position_estimate->covariance); +} + /** * @brief Send a vicon_position_estimate message * @param chan MAVLink channel to send the message @@ -215,7 +281,7 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); + mav_array_assign_float(packet.covariance, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); #endif } @@ -236,7 +302,7 @@ static inline void mavlink_msg_vicon_position_estimate_send_struct(mavlink_chann #if MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -264,7 +330,7 @@ static inline void mavlink_msg_vicon_position_estimate_send_buf(mavlink_message_ packet->roll = roll; packet->pitch = pitch; packet->yaw = yaw; - mav_array_memcpy(packet->covariance, covariance, sizeof(float)*21); + mav_array_assign_float(packet->covariance, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_video_stream_information.h b/lib/main/MAVLink/common/mavlink_msg_video_stream_information.h index aa6d84b7437..5118b1e335a 100644 --- a/lib/main/MAVLink/common/mavlink_msg_video_stream_information.h +++ b/lib/main/MAVLink/common/mavlink_msg_video_stream_information.h @@ -17,11 +17,13 @@ typedef struct __mavlink_video_stream_information_t { uint8_t type; /*< Type of stream.*/ char name[32]; /*< Stream name.*/ char uri[160]; /*< Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to).*/ + uint8_t encoding; /*< Encoding of stream.*/ + uint8_t camera_device_id; /*< Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id).*/ } mavlink_video_stream_information_t; -#define MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN 213 +#define MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN 215 #define MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN 213 -#define MAVLINK_MSG_ID_269_LEN 213 +#define MAVLINK_MSG_ID_269_LEN 215 #define MAVLINK_MSG_ID_269_MIN_LEN 213 #define MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC 109 @@ -34,7 +36,7 @@ typedef struct __mavlink_video_stream_information_t { #define MAVLINK_MESSAGE_INFO_VIDEO_STREAM_INFORMATION { \ 269, \ "VIDEO_STREAM_INFORMATION", \ - 12, \ + 14, \ { { "stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_video_stream_information_t, stream_id) }, \ { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_video_stream_information_t, count) }, \ { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_video_stream_information_t, type) }, \ @@ -47,12 +49,14 @@ typedef struct __mavlink_video_stream_information_t { { "hfov", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_video_stream_information_t, hfov) }, \ { "name", NULL, MAVLINK_TYPE_CHAR, 32, 21, offsetof(mavlink_video_stream_information_t, name) }, \ { "uri", NULL, MAVLINK_TYPE_CHAR, 160, 53, offsetof(mavlink_video_stream_information_t, uri) }, \ + { "encoding", NULL, MAVLINK_TYPE_UINT8_T, 0, 213, offsetof(mavlink_video_stream_information_t, encoding) }, \ + { "camera_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 214, offsetof(mavlink_video_stream_information_t, camera_device_id) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_VIDEO_STREAM_INFORMATION { \ "VIDEO_STREAM_INFORMATION", \ - 12, \ + 14, \ { { "stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_video_stream_information_t, stream_id) }, \ { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_video_stream_information_t, count) }, \ { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_video_stream_information_t, type) }, \ @@ -65,6 +69,8 @@ typedef struct __mavlink_video_stream_information_t { { "hfov", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_video_stream_information_t, hfov) }, \ { "name", NULL, MAVLINK_TYPE_CHAR, 32, 21, offsetof(mavlink_video_stream_information_t, name) }, \ { "uri", NULL, MAVLINK_TYPE_CHAR, 160, 53, offsetof(mavlink_video_stream_information_t, uri) }, \ + { "encoding", NULL, MAVLINK_TYPE_UINT8_T, 0, 213, offsetof(mavlink_video_stream_information_t, encoding) }, \ + { "camera_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 214, offsetof(mavlink_video_stream_information_t, camera_device_id) }, \ } \ } #endif @@ -87,10 +93,12 @@ typedef struct __mavlink_video_stream_information_t { * @param hfov [deg] Horizontal Field of view. * @param name Stream name. * @param uri Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to). + * @param encoding Encoding of stream. + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_video_stream_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t stream_id, uint8_t count, uint8_t type, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov, const char *name, const char *uri) + uint8_t stream_id, uint8_t count, uint8_t type, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov, const char *name, const char *uri, uint8_t encoding, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN]; @@ -104,6 +112,8 @@ static inline uint16_t mavlink_msg_video_stream_information_pack(uint8_t system_ _mav_put_uint8_t(buf, 18, stream_id); _mav_put_uint8_t(buf, 19, count); _mav_put_uint8_t(buf, 20, type); + _mav_put_uint8_t(buf, 213, encoding); + _mav_put_uint8_t(buf, 214, camera_device_id); _mav_put_char_array(buf, 21, name, 32); _mav_put_char_array(buf, 53, uri, 160); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN); @@ -119,13 +129,85 @@ static inline uint16_t mavlink_msg_video_stream_information_pack(uint8_t system_ packet.stream_id = stream_id; packet.count = count; packet.type = type; + packet.encoding = encoding; + packet.camera_device_id = camera_device_id; + mav_array_assign_char(packet.name, name, 32); + mav_array_assign_char(packet.uri, uri, 160); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); +} + +/** + * @brief Pack a video_stream_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param stream_id Video Stream ID (1 for first, 2 for second, etc.) + * @param count Number of streams available. + * @param type Type of stream. + * @param flags Bitmap of stream status flags. + * @param framerate [Hz] Frame rate. + * @param resolution_h [pix] Horizontal resolution. + * @param resolution_v [pix] Vertical resolution. + * @param bitrate [bits/s] Bit rate. + * @param rotation [deg] Video image rotation clockwise. + * @param hfov [deg] Horizontal Field of view. + * @param name Stream name. + * @param uri Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to). + * @param encoding Encoding of stream. + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_video_stream_information_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t stream_id, uint8_t count, uint8_t type, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov, const char *name, const char *uri, uint8_t encoding, uint8_t camera_device_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN]; + _mav_put_float(buf, 0, framerate); + _mav_put_uint32_t(buf, 4, bitrate); + _mav_put_uint16_t(buf, 8, flags); + _mav_put_uint16_t(buf, 10, resolution_h); + _mav_put_uint16_t(buf, 12, resolution_v); + _mav_put_uint16_t(buf, 14, rotation); + _mav_put_uint16_t(buf, 16, hfov); + _mav_put_uint8_t(buf, 18, stream_id); + _mav_put_uint8_t(buf, 19, count); + _mav_put_uint8_t(buf, 20, type); + _mav_put_uint8_t(buf, 213, encoding); + _mav_put_uint8_t(buf, 214, camera_device_id); + _mav_put_char_array(buf, 21, name, 32); + _mav_put_char_array(buf, 53, uri, 160); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN); +#else + mavlink_video_stream_information_t packet; + packet.framerate = framerate; + packet.bitrate = bitrate; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.rotation = rotation; + packet.hfov = hfov; + packet.stream_id = stream_id; + packet.count = count; + packet.type = type; + packet.encoding = encoding; + packet.camera_device_id = camera_device_id; mav_array_memcpy(packet.name, name, sizeof(char)*32); mav_array_memcpy(packet.uri, uri, sizeof(char)*160); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN); #endif msg->msgid = MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN); +#endif } /** @@ -146,11 +228,13 @@ static inline uint16_t mavlink_msg_video_stream_information_pack(uint8_t system_ * @param hfov [deg] Horizontal Field of view. * @param name Stream name. * @param uri Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to). + * @param encoding Encoding of stream. + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_video_stream_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t stream_id,uint8_t count,uint8_t type,uint16_t flags,float framerate,uint16_t resolution_h,uint16_t resolution_v,uint32_t bitrate,uint16_t rotation,uint16_t hfov,const char *name,const char *uri) + uint8_t stream_id,uint8_t count,uint8_t type,uint16_t flags,float framerate,uint16_t resolution_h,uint16_t resolution_v,uint32_t bitrate,uint16_t rotation,uint16_t hfov,const char *name,const char *uri,uint8_t encoding,uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN]; @@ -164,6 +248,8 @@ static inline uint16_t mavlink_msg_video_stream_information_pack_chan(uint8_t sy _mav_put_uint8_t(buf, 18, stream_id); _mav_put_uint8_t(buf, 19, count); _mav_put_uint8_t(buf, 20, type); + _mav_put_uint8_t(buf, 213, encoding); + _mav_put_uint8_t(buf, 214, camera_device_id); _mav_put_char_array(buf, 21, name, 32); _mav_put_char_array(buf, 53, uri, 160); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN); @@ -179,8 +265,10 @@ static inline uint16_t mavlink_msg_video_stream_information_pack_chan(uint8_t sy packet.stream_id = stream_id; packet.count = count; packet.type = type; - mav_array_memcpy(packet.name, name, sizeof(char)*32); - mav_array_memcpy(packet.uri, uri, sizeof(char)*160); + packet.encoding = encoding; + packet.camera_device_id = camera_device_id; + mav_array_assign_char(packet.name, name, 32); + mav_array_assign_char(packet.uri, uri, 160); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN); #endif @@ -198,7 +286,7 @@ static inline uint16_t mavlink_msg_video_stream_information_pack_chan(uint8_t sy */ static inline uint16_t mavlink_msg_video_stream_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_video_stream_information_t* video_stream_information) { - return mavlink_msg_video_stream_information_pack(system_id, component_id, msg, video_stream_information->stream_id, video_stream_information->count, video_stream_information->type, video_stream_information->flags, video_stream_information->framerate, video_stream_information->resolution_h, video_stream_information->resolution_v, video_stream_information->bitrate, video_stream_information->rotation, video_stream_information->hfov, video_stream_information->name, video_stream_information->uri); + return mavlink_msg_video_stream_information_pack(system_id, component_id, msg, video_stream_information->stream_id, video_stream_information->count, video_stream_information->type, video_stream_information->flags, video_stream_information->framerate, video_stream_information->resolution_h, video_stream_information->resolution_v, video_stream_information->bitrate, video_stream_information->rotation, video_stream_information->hfov, video_stream_information->name, video_stream_information->uri, video_stream_information->encoding, video_stream_information->camera_device_id); } /** @@ -212,7 +300,21 @@ static inline uint16_t mavlink_msg_video_stream_information_encode(uint8_t syste */ static inline uint16_t mavlink_msg_video_stream_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_video_stream_information_t* video_stream_information) { - return mavlink_msg_video_stream_information_pack_chan(system_id, component_id, chan, msg, video_stream_information->stream_id, video_stream_information->count, video_stream_information->type, video_stream_information->flags, video_stream_information->framerate, video_stream_information->resolution_h, video_stream_information->resolution_v, video_stream_information->bitrate, video_stream_information->rotation, video_stream_information->hfov, video_stream_information->name, video_stream_information->uri); + return mavlink_msg_video_stream_information_pack_chan(system_id, component_id, chan, msg, video_stream_information->stream_id, video_stream_information->count, video_stream_information->type, video_stream_information->flags, video_stream_information->framerate, video_stream_information->resolution_h, video_stream_information->resolution_v, video_stream_information->bitrate, video_stream_information->rotation, video_stream_information->hfov, video_stream_information->name, video_stream_information->uri, video_stream_information->encoding, video_stream_information->camera_device_id); +} + +/** + * @brief Encode a video_stream_information struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param video_stream_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_video_stream_information_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_video_stream_information_t* video_stream_information) +{ + return mavlink_msg_video_stream_information_pack_status(system_id, component_id, _status, msg, video_stream_information->stream_id, video_stream_information->count, video_stream_information->type, video_stream_information->flags, video_stream_information->framerate, video_stream_information->resolution_h, video_stream_information->resolution_v, video_stream_information->bitrate, video_stream_information->rotation, video_stream_information->hfov, video_stream_information->name, video_stream_information->uri, video_stream_information->encoding, video_stream_information->camera_device_id); } /** @@ -231,10 +333,12 @@ static inline uint16_t mavlink_msg_video_stream_information_encode_chan(uint8_t * @param hfov [deg] Horizontal Field of view. * @param name Stream name. * @param uri Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to). + * @param encoding Encoding of stream. + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_video_stream_information_send(mavlink_channel_t chan, uint8_t stream_id, uint8_t count, uint8_t type, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov, const char *name, const char *uri) +static inline void mavlink_msg_video_stream_information_send(mavlink_channel_t chan, uint8_t stream_id, uint8_t count, uint8_t type, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov, const char *name, const char *uri, uint8_t encoding, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN]; @@ -248,6 +352,8 @@ static inline void mavlink_msg_video_stream_information_send(mavlink_channel_t c _mav_put_uint8_t(buf, 18, stream_id); _mav_put_uint8_t(buf, 19, count); _mav_put_uint8_t(buf, 20, type); + _mav_put_uint8_t(buf, 213, encoding); + _mav_put_uint8_t(buf, 214, camera_device_id); _mav_put_char_array(buf, 21, name, 32); _mav_put_char_array(buf, 53, uri, 160); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, buf, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); @@ -263,8 +369,10 @@ static inline void mavlink_msg_video_stream_information_send(mavlink_channel_t c packet.stream_id = stream_id; packet.count = count; packet.type = type; - mav_array_memcpy(packet.name, name, sizeof(char)*32); - mav_array_memcpy(packet.uri, uri, sizeof(char)*160); + packet.encoding = encoding; + packet.camera_device_id = camera_device_id; + mav_array_assign_char(packet.name, name, 32); + mav_array_assign_char(packet.uri, uri, 160); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); #endif } @@ -277,7 +385,7 @@ static inline void mavlink_msg_video_stream_information_send(mavlink_channel_t c static inline void mavlink_msg_video_stream_information_send_struct(mavlink_channel_t chan, const mavlink_video_stream_information_t* video_stream_information) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_video_stream_information_send(chan, video_stream_information->stream_id, video_stream_information->count, video_stream_information->type, video_stream_information->flags, video_stream_information->framerate, video_stream_information->resolution_h, video_stream_information->resolution_v, video_stream_information->bitrate, video_stream_information->rotation, video_stream_information->hfov, video_stream_information->name, video_stream_information->uri); + mavlink_msg_video_stream_information_send(chan, video_stream_information->stream_id, video_stream_information->count, video_stream_information->type, video_stream_information->flags, video_stream_information->framerate, video_stream_information->resolution_h, video_stream_information->resolution_v, video_stream_information->bitrate, video_stream_information->rotation, video_stream_information->hfov, video_stream_information->name, video_stream_information->uri, video_stream_information->encoding, video_stream_information->camera_device_id); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, (const char *)video_stream_information, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); #endif @@ -285,13 +393,13 @@ static inline void mavlink_msg_video_stream_information_send_struct(mavlink_chan #if MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_video_stream_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t stream_id, uint8_t count, uint8_t type, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov, const char *name, const char *uri) +static inline void mavlink_msg_video_stream_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t stream_id, uint8_t count, uint8_t type, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov, const char *name, const char *uri, uint8_t encoding, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -305,6 +413,8 @@ static inline void mavlink_msg_video_stream_information_send_buf(mavlink_message _mav_put_uint8_t(buf, 18, stream_id); _mav_put_uint8_t(buf, 19, count); _mav_put_uint8_t(buf, 20, type); + _mav_put_uint8_t(buf, 213, encoding); + _mav_put_uint8_t(buf, 214, camera_device_id); _mav_put_char_array(buf, 21, name, 32); _mav_put_char_array(buf, 53, uri, 160); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, buf, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); @@ -320,8 +430,10 @@ static inline void mavlink_msg_video_stream_information_send_buf(mavlink_message packet->stream_id = stream_id; packet->count = count; packet->type = type; - mav_array_memcpy(packet->name, name, sizeof(char)*32); - mav_array_memcpy(packet->uri, uri, sizeof(char)*160); + packet->encoding = encoding; + packet->camera_device_id = camera_device_id; + mav_array_assign_char(packet->name, name, 32); + mav_array_assign_char(packet->uri, uri, 160); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); #endif } @@ -452,6 +564,26 @@ static inline uint16_t mavlink_msg_video_stream_information_get_uri(const mavlin return _MAV_RETURN_char_array(msg, uri, 160, 53); } +/** + * @brief Get field encoding from video_stream_information message + * + * @return Encoding of stream. + */ +static inline uint8_t mavlink_msg_video_stream_information_get_encoding(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 213); +} + +/** + * @brief Get field camera_device_id from video_stream_information message + * + * @return Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + */ +static inline uint8_t mavlink_msg_video_stream_information_get_camera_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 214); +} + /** * @brief Decode a video_stream_information message into a struct * @@ -473,6 +605,8 @@ static inline void mavlink_msg_video_stream_information_decode(const mavlink_mes video_stream_information->type = mavlink_msg_video_stream_information_get_type(msg); mavlink_msg_video_stream_information_get_name(msg, video_stream_information->name); mavlink_msg_video_stream_information_get_uri(msg, video_stream_information->uri); + video_stream_information->encoding = mavlink_msg_video_stream_information_get_encoding(msg); + video_stream_information->camera_device_id = mavlink_msg_video_stream_information_get_camera_device_id(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN; memset(video_stream_information, 0, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_video_stream_status.h b/lib/main/MAVLink/common/mavlink_msg_video_stream_status.h index 380941005ee..f73b169890a 100644 --- a/lib/main/MAVLink/common/mavlink_msg_video_stream_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_video_stream_status.h @@ -13,11 +13,12 @@ typedef struct __mavlink_video_stream_status_t { uint16_t rotation; /*< [deg] Video image rotation clockwise*/ uint16_t hfov; /*< [deg] Horizontal Field of view*/ uint8_t stream_id; /*< Video Stream ID (1 for first, 2 for second, etc.)*/ + uint8_t camera_device_id; /*< Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id).*/ } mavlink_video_stream_status_t; -#define MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN 19 +#define MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN 20 #define MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN 19 -#define MAVLINK_MSG_ID_270_LEN 19 +#define MAVLINK_MSG_ID_270_LEN 20 #define MAVLINK_MSG_ID_270_MIN_LEN 19 #define MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC 59 @@ -29,7 +30,7 @@ typedef struct __mavlink_video_stream_status_t { #define MAVLINK_MESSAGE_INFO_VIDEO_STREAM_STATUS { \ 270, \ "VIDEO_STREAM_STATUS", \ - 8, \ + 9, \ { { "stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_video_stream_status_t, stream_id) }, \ { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_video_stream_status_t, flags) }, \ { "framerate", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_video_stream_status_t, framerate) }, \ @@ -38,12 +39,13 @@ typedef struct __mavlink_video_stream_status_t { { "bitrate", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_video_stream_status_t, bitrate) }, \ { "rotation", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_video_stream_status_t, rotation) }, \ { "hfov", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_video_stream_status_t, hfov) }, \ + { "camera_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_video_stream_status_t, camera_device_id) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_VIDEO_STREAM_STATUS { \ "VIDEO_STREAM_STATUS", \ - 8, \ + 9, \ { { "stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_video_stream_status_t, stream_id) }, \ { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_video_stream_status_t, flags) }, \ { "framerate", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_video_stream_status_t, framerate) }, \ @@ -52,6 +54,7 @@ typedef struct __mavlink_video_stream_status_t { { "bitrate", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_video_stream_status_t, bitrate) }, \ { "rotation", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_video_stream_status_t, rotation) }, \ { "hfov", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_video_stream_status_t, hfov) }, \ + { "camera_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_video_stream_status_t, camera_device_id) }, \ } \ } #endif @@ -70,10 +73,11 @@ typedef struct __mavlink_video_stream_status_t { * @param bitrate [bits/s] Bit rate * @param rotation [deg] Video image rotation clockwise * @param hfov [deg] Horizontal Field of view + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_video_stream_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t stream_id, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov) + uint8_t stream_id, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN]; @@ -85,6 +89,7 @@ static inline uint16_t mavlink_msg_video_stream_status_pack(uint8_t system_id, u _mav_put_uint16_t(buf, 14, rotation); _mav_put_uint16_t(buf, 16, hfov); _mav_put_uint8_t(buf, 18, stream_id); + _mav_put_uint8_t(buf, 19, camera_device_id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN); #else @@ -97,6 +102,7 @@ static inline uint16_t mavlink_msg_video_stream_status_pack(uint8_t system_id, u packet.rotation = rotation; packet.hfov = hfov; packet.stream_id = stream_id; + packet.camera_device_id = camera_device_id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN); #endif @@ -105,6 +111,63 @@ static inline uint16_t mavlink_msg_video_stream_status_pack(uint8_t system_id, u return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); } +/** + * @brief Pack a video_stream_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param stream_id Video Stream ID (1 for first, 2 for second, etc.) + * @param flags Bitmap of stream status flags + * @param framerate [Hz] Frame rate + * @param resolution_h [pix] Horizontal resolution + * @param resolution_v [pix] Vertical resolution + * @param bitrate [bits/s] Bit rate + * @param rotation [deg] Video image rotation clockwise + * @param hfov [deg] Horizontal Field of view + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_video_stream_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t stream_id, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov, uint8_t camera_device_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN]; + _mav_put_float(buf, 0, framerate); + _mav_put_uint32_t(buf, 4, bitrate); + _mav_put_uint16_t(buf, 8, flags); + _mav_put_uint16_t(buf, 10, resolution_h); + _mav_put_uint16_t(buf, 12, resolution_v); + _mav_put_uint16_t(buf, 14, rotation); + _mav_put_uint16_t(buf, 16, hfov); + _mav_put_uint8_t(buf, 18, stream_id); + _mav_put_uint8_t(buf, 19, camera_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN); +#else + mavlink_video_stream_status_t packet; + packet.framerate = framerate; + packet.bitrate = bitrate; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.rotation = rotation; + packet.hfov = hfov; + packet.stream_id = stream_id; + packet.camera_device_id = camera_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VIDEO_STREAM_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN); +#endif +} + /** * @brief Pack a video_stream_status message on a channel * @param system_id ID of this system @@ -119,11 +182,12 @@ static inline uint16_t mavlink_msg_video_stream_status_pack(uint8_t system_id, u * @param bitrate [bits/s] Bit rate * @param rotation [deg] Video image rotation clockwise * @param hfov [deg] Horizontal Field of view + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_video_stream_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t stream_id,uint16_t flags,float framerate,uint16_t resolution_h,uint16_t resolution_v,uint32_t bitrate,uint16_t rotation,uint16_t hfov) + uint8_t stream_id,uint16_t flags,float framerate,uint16_t resolution_h,uint16_t resolution_v,uint32_t bitrate,uint16_t rotation,uint16_t hfov,uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN]; @@ -135,6 +199,7 @@ static inline uint16_t mavlink_msg_video_stream_status_pack_chan(uint8_t system_ _mav_put_uint16_t(buf, 14, rotation); _mav_put_uint16_t(buf, 16, hfov); _mav_put_uint8_t(buf, 18, stream_id); + _mav_put_uint8_t(buf, 19, camera_device_id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN); #else @@ -147,6 +212,7 @@ static inline uint16_t mavlink_msg_video_stream_status_pack_chan(uint8_t system_ packet.rotation = rotation; packet.hfov = hfov; packet.stream_id = stream_id; + packet.camera_device_id = camera_device_id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN); #endif @@ -165,7 +231,7 @@ static inline uint16_t mavlink_msg_video_stream_status_pack_chan(uint8_t system_ */ static inline uint16_t mavlink_msg_video_stream_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_video_stream_status_t* video_stream_status) { - return mavlink_msg_video_stream_status_pack(system_id, component_id, msg, video_stream_status->stream_id, video_stream_status->flags, video_stream_status->framerate, video_stream_status->resolution_h, video_stream_status->resolution_v, video_stream_status->bitrate, video_stream_status->rotation, video_stream_status->hfov); + return mavlink_msg_video_stream_status_pack(system_id, component_id, msg, video_stream_status->stream_id, video_stream_status->flags, video_stream_status->framerate, video_stream_status->resolution_h, video_stream_status->resolution_v, video_stream_status->bitrate, video_stream_status->rotation, video_stream_status->hfov, video_stream_status->camera_device_id); } /** @@ -179,7 +245,21 @@ static inline uint16_t mavlink_msg_video_stream_status_encode(uint8_t system_id, */ static inline uint16_t mavlink_msg_video_stream_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_video_stream_status_t* video_stream_status) { - return mavlink_msg_video_stream_status_pack_chan(system_id, component_id, chan, msg, video_stream_status->stream_id, video_stream_status->flags, video_stream_status->framerate, video_stream_status->resolution_h, video_stream_status->resolution_v, video_stream_status->bitrate, video_stream_status->rotation, video_stream_status->hfov); + return mavlink_msg_video_stream_status_pack_chan(system_id, component_id, chan, msg, video_stream_status->stream_id, video_stream_status->flags, video_stream_status->framerate, video_stream_status->resolution_h, video_stream_status->resolution_v, video_stream_status->bitrate, video_stream_status->rotation, video_stream_status->hfov, video_stream_status->camera_device_id); +} + +/** + * @brief Encode a video_stream_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param video_stream_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_video_stream_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_video_stream_status_t* video_stream_status) +{ + return mavlink_msg_video_stream_status_pack_status(system_id, component_id, _status, msg, video_stream_status->stream_id, video_stream_status->flags, video_stream_status->framerate, video_stream_status->resolution_h, video_stream_status->resolution_v, video_stream_status->bitrate, video_stream_status->rotation, video_stream_status->hfov, video_stream_status->camera_device_id); } /** @@ -194,10 +274,11 @@ static inline uint16_t mavlink_msg_video_stream_status_encode_chan(uint8_t syste * @param bitrate [bits/s] Bit rate * @param rotation [deg] Video image rotation clockwise * @param hfov [deg] Horizontal Field of view + * @param camera_device_id Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_video_stream_status_send(mavlink_channel_t chan, uint8_t stream_id, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov) +static inline void mavlink_msg_video_stream_status_send(mavlink_channel_t chan, uint8_t stream_id, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN]; @@ -209,6 +290,7 @@ static inline void mavlink_msg_video_stream_status_send(mavlink_channel_t chan, _mav_put_uint16_t(buf, 14, rotation); _mav_put_uint16_t(buf, 16, hfov); _mav_put_uint8_t(buf, 18, stream_id); + _mav_put_uint8_t(buf, 19, camera_device_id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, buf, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); #else @@ -221,6 +303,7 @@ static inline void mavlink_msg_video_stream_status_send(mavlink_channel_t chan, packet.rotation = rotation; packet.hfov = hfov; packet.stream_id = stream_id; + packet.camera_device_id = camera_device_id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, (const char *)&packet, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); #endif @@ -234,7 +317,7 @@ static inline void mavlink_msg_video_stream_status_send(mavlink_channel_t chan, static inline void mavlink_msg_video_stream_status_send_struct(mavlink_channel_t chan, const mavlink_video_stream_status_t* video_stream_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_video_stream_status_send(chan, video_stream_status->stream_id, video_stream_status->flags, video_stream_status->framerate, video_stream_status->resolution_h, video_stream_status->resolution_v, video_stream_status->bitrate, video_stream_status->rotation, video_stream_status->hfov); + mavlink_msg_video_stream_status_send(chan, video_stream_status->stream_id, video_stream_status->flags, video_stream_status->framerate, video_stream_status->resolution_h, video_stream_status->resolution_v, video_stream_status->bitrate, video_stream_status->rotation, video_stream_status->hfov, video_stream_status->camera_device_id); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, (const char *)video_stream_status, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); #endif @@ -242,13 +325,13 @@ static inline void mavlink_msg_video_stream_status_send_struct(mavlink_channel_t #if MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_video_stream_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t stream_id, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov) +static inline void mavlink_msg_video_stream_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t stream_id, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov, uint8_t camera_device_id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -260,6 +343,7 @@ static inline void mavlink_msg_video_stream_status_send_buf(mavlink_message_t *m _mav_put_uint16_t(buf, 14, rotation); _mav_put_uint16_t(buf, 16, hfov); _mav_put_uint8_t(buf, 18, stream_id); + _mav_put_uint8_t(buf, 19, camera_device_id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, buf, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); #else @@ -272,6 +356,7 @@ static inline void mavlink_msg_video_stream_status_send_buf(mavlink_message_t *m packet->rotation = rotation; packet->hfov = hfov; packet->stream_id = stream_id; + packet->camera_device_id = camera_device_id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, (const char *)packet, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); #endif @@ -363,6 +448,16 @@ static inline uint16_t mavlink_msg_video_stream_status_get_hfov(const mavlink_me return _MAV_RETURN_uint16_t(msg, 16); } +/** + * @brief Get field camera_device_id from video_stream_status message + * + * @return Camera id of a non-MAVLink camera attached to an autopilot (1-6). 0 if the component is a MAVLink camera (with its own component id). + */ +static inline uint8_t mavlink_msg_video_stream_status_get_camera_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 19); +} + /** * @brief Decode a video_stream_status message into a struct * @@ -380,6 +475,7 @@ static inline void mavlink_msg_video_stream_status_decode(const mavlink_message_ video_stream_status->rotation = mavlink_msg_video_stream_status_get_rotation(msg); video_stream_status->hfov = mavlink_msg_video_stream_status_get_hfov(msg); video_stream_status->stream_id = mavlink_msg_video_stream_status_get_stream_id(msg); + video_stream_status->camera_device_id = mavlink_msg_video_stream_status_get_camera_device_id(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN? msg->len : MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN; memset(video_stream_status, 0, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_vision_position_estimate.h b/lib/main/MAVLink/common/mavlink_msg_vision_position_estimate.h index 438b8c5bb39..1865a69b82d 100755 --- a/lib/main/MAVLink/common/mavlink_msg_vision_position_estimate.h +++ b/lib/main/MAVLink/common/mavlink_msg_vision_position_estimate.h @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_ packet.pitch = pitch; packet.yaw = yaw; packet.reset_counter = reset_counter; - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); + mav_array_assign_float(packet.covariance, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); #endif @@ -109,6 +109,61 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); } +/** + * @brief Pack a vision_position_estimate message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param usec [us] Timestamp (UNIX time or time since system boot) + * @param x [m] Local X position + * @param y [m] Local Y position + * @param z [m] Local Z position + * @param roll [rad] Roll angle + * @param pitch [rad] Pitch angle + * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_vision_position_estimate_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance, uint8_t reset_counter) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + _mav_put_uint8_t(buf, 116, reset_counter); + _mav_put_float_array(buf, 32, covariance, 21); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#else + mavlink_vision_position_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#endif +} + /** * @brief Pack a vision_position_estimate message on a channel * @param system_id ID of this system @@ -152,7 +207,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t sy packet.pitch = pitch; packet.yaw = yaw; packet.reset_counter = reset_counter; - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); + mav_array_assign_float(packet.covariance, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); #endif @@ -187,6 +242,20 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode_chan(uint8_t return mavlink_msg_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw, vision_position_estimate->covariance, vision_position_estimate->reset_counter); } +/** + * @brief Encode a vision_position_estimate struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param vision_position_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vision_position_estimate_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_vision_position_estimate_t* vision_position_estimate) +{ + return mavlink_msg_vision_position_estimate_pack_status(system_id, component_id, _status, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw, vision_position_estimate->covariance, vision_position_estimate->reset_counter); +} + /** * @brief Send a vision_position_estimate message * @param chan MAVLink channel to send the message @@ -227,7 +296,7 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c packet.pitch = pitch; packet.yaw = yaw; packet.reset_counter = reset_counter; - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); + mav_array_assign_float(packet.covariance, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); #endif } @@ -248,7 +317,7 @@ static inline void mavlink_msg_vision_position_estimate_send_struct(mavlink_chan #if MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -278,7 +347,7 @@ static inline void mavlink_msg_vision_position_estimate_send_buf(mavlink_message packet->pitch = pitch; packet->yaw = yaw; packet->reset_counter = reset_counter; - mav_array_memcpy(packet->covariance, covariance, sizeof(float)*21); + mav_array_assign_float(packet->covariance, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_vision_speed_estimate.h b/lib/main/MAVLink/common/mavlink_msg_vision_speed_estimate.h index 7c993e8fd65..af3bb01374d 100755 --- a/lib/main/MAVLink/common/mavlink_msg_vision_speed_estimate.h +++ b/lib/main/MAVLink/common/mavlink_msg_vision_speed_estimate.h @@ -83,7 +83,7 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, packet.y = y; packet.z = z; packet.reset_counter = reset_counter; - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*9); + mav_array_assign_float(packet.covariance, covariance, 9); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); #endif @@ -91,6 +91,52 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); } +/** + * @brief Pack a vision_speed_estimate message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param usec [us] Timestamp (UNIX time or time since system boot) + * @param x [m/s] Global X speed + * @param y [m/s] Global Y speed + * @param z [m/s] Global Z speed + * @param covariance Row-major representation of 3x3 linear velocity covariance matrix (states: vx, vy, vz; 1st three entries - 1st row, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_vision_speed_estimate_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t usec, float x, float y, float z, const float *covariance, uint8_t reset_counter) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_uint8_t(buf, 56, reset_counter); + _mav_put_float_array(buf, 20, covariance, 9); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#else + mavlink_vision_speed_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*9); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#endif +} + /** * @brief Pack a vision_speed_estimate message on a channel * @param system_id ID of this system @@ -125,7 +171,7 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t syste packet.y = y; packet.z = z; packet.reset_counter = reset_counter; - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*9); + mav_array_assign_float(packet.covariance, covariance, 9); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); #endif @@ -160,6 +206,20 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode_chan(uint8_t sys return mavlink_msg_vision_speed_estimate_pack_chan(system_id, component_id, chan, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z, vision_speed_estimate->covariance, vision_speed_estimate->reset_counter); } +/** + * @brief Encode a vision_speed_estimate struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param vision_speed_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vision_speed_estimate_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_vision_speed_estimate_t* vision_speed_estimate) +{ + return mavlink_msg_vision_speed_estimate_pack_status(system_id, component_id, _status, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z, vision_speed_estimate->covariance, vision_speed_estimate->reset_counter); +} + /** * @brief Send a vision_speed_estimate message * @param chan MAVLink channel to send the message @@ -191,7 +251,7 @@ static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan packet.y = y; packet.z = z; packet.reset_counter = reset_counter; - mav_array_memcpy(packet.covariance, covariance, sizeof(float)*9); + mav_array_assign_float(packet.covariance, covariance, 9); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); #endif } @@ -212,7 +272,7 @@ static inline void mavlink_msg_vision_speed_estimate_send_struct(mavlink_channel #if MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -236,7 +296,7 @@ static inline void mavlink_msg_vision_speed_estimate_send_buf(mavlink_message_t packet->y = y; packet->z = z; packet->reset_counter = reset_counter; - mav_array_memcpy(packet->covariance, covariance, sizeof(float)*9); + mav_array_assign_float(packet->covariance, covariance, 9); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_wheel_distance.h b/lib/main/MAVLink/common/mavlink_msg_wheel_distance.h index d506f003601..ac99950b41b 100644 --- a/lib/main/MAVLink/common/mavlink_msg_wheel_distance.h +++ b/lib/main/MAVLink/common/mavlink_msg_wheel_distance.h @@ -65,7 +65,7 @@ static inline uint16_t mavlink_msg_wheel_distance_pack(uint8_t system_id, uint8_ mavlink_wheel_distance_t packet; packet.time_usec = time_usec; packet.count = count; - mav_array_memcpy(packet.distance, distance, sizeof(double)*16); + mav_array_assign_double(packet.distance, distance, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN); #endif @@ -73,6 +73,43 @@ static inline uint16_t mavlink_msg_wheel_distance_pack(uint8_t system_id, uint8_ return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC); } +/** + * @brief Pack a wheel_distance message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (synced to UNIX time or since system boot). + * @param count Number of wheels reported. + * @param distance [m] Distance reported by individual wheel encoders. Forward rotations increase values, reverse rotations decrease them. Not all wheels will necessarily have wheel encoders; the mapping of encoders to wheel positions must be agreed/understood by the endpoints. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_wheel_distance_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, uint8_t count, const double *distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 136, count); + _mav_put_double_array(buf, 8, distance, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN); +#else + mavlink_wheel_distance_t packet; + packet.time_usec = time_usec; + packet.count = count; + mav_array_memcpy(packet.distance, distance, sizeof(double)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_WHEEL_DISTANCE; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN); +#endif +} + /** * @brief Pack a wheel_distance message on a channel * @param system_id ID of this system @@ -98,7 +135,7 @@ static inline uint16_t mavlink_msg_wheel_distance_pack_chan(uint8_t system_id, u mavlink_wheel_distance_t packet; packet.time_usec = time_usec; packet.count = count; - mav_array_memcpy(packet.distance, distance, sizeof(double)*16); + mav_array_assign_double(packet.distance, distance, 16); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN); #endif @@ -133,6 +170,20 @@ static inline uint16_t mavlink_msg_wheel_distance_encode_chan(uint8_t system_id, return mavlink_msg_wheel_distance_pack_chan(system_id, component_id, chan, msg, wheel_distance->time_usec, wheel_distance->count, wheel_distance->distance); } +/** + * @brief Encode a wheel_distance struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param wheel_distance C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_wheel_distance_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_wheel_distance_t* wheel_distance) +{ + return mavlink_msg_wheel_distance_pack_status(system_id, component_id, _status, msg, wheel_distance->time_usec, wheel_distance->count, wheel_distance->distance); +} + /** * @brief Send a wheel_distance message * @param chan MAVLink channel to send the message @@ -155,7 +206,7 @@ static inline void mavlink_msg_wheel_distance_send(mavlink_channel_t chan, uint6 mavlink_wheel_distance_t packet; packet.time_usec = time_usec; packet.count = count; - mav_array_memcpy(packet.distance, distance, sizeof(double)*16); + mav_array_assign_double(packet.distance, distance, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WHEEL_DISTANCE, (const char *)&packet, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC); #endif } @@ -176,7 +227,7 @@ static inline void mavlink_msg_wheel_distance_send_struct(mavlink_channel_t chan #if MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -194,7 +245,7 @@ static inline void mavlink_msg_wheel_distance_send_buf(mavlink_message_t *msgbuf mavlink_wheel_distance_t *packet = (mavlink_wheel_distance_t *)msgbuf; packet->time_usec = time_usec; packet->count = count; - mav_array_memcpy(packet->distance, distance, sizeof(double)*16); + mav_array_assign_double(packet->distance, distance, 16); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WHEEL_DISTANCE, (const char *)packet, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_wifi_config_ap.h b/lib/main/MAVLink/common/mavlink_msg_wifi_config_ap.h index f5abc2473b2..8e115ffabad 100644 --- a/lib/main/MAVLink/common/mavlink_msg_wifi_config_ap.h +++ b/lib/main/MAVLink/common/mavlink_msg_wifi_config_ap.h @@ -60,6 +60,42 @@ typedef struct __mavlink_wifi_config_ap_t { static inline uint16_t mavlink_msg_wifi_config_ap_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char *ssid, const char *password, int8_t mode, int8_t response) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN]; + _mav_put_int8_t(buf, 96, mode); + _mav_put_int8_t(buf, 97, response); + _mav_put_char_array(buf, 0, ssid, 32); + _mav_put_char_array(buf, 32, password, 64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN); +#else + mavlink_wifi_config_ap_t packet; + packet.mode = mode; + packet.response = response; + mav_array_assign_char(packet.ssid, ssid, 32); + mav_array_assign_char(packet.password, password, 64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_WIFI_CONFIG_AP; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); +} + +/** + * @brief Pack a wifi_config_ap message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param ssid Name of Wi-Fi network (SSID). Blank to leave it unchanged when setting. Current SSID when sent back as a response. + * @param password Password. Blank for an open AP. MD5 hash when message is sent back as a response. + * @param mode WiFi Mode. + * @param response Message acceptance response (sent back to GS). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_wifi_config_ap_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + const char *ssid, const char *password, int8_t mode, int8_t response) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN]; _mav_put_int8_t(buf, 96, mode); @@ -77,7 +113,11 @@ static inline uint16_t mavlink_msg_wifi_config_ap_pack(uint8_t system_id, uint8_ #endif msg->msgid = MAVLINK_MSG_ID_WIFI_CONFIG_AP; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN); +#endif } /** @@ -107,8 +147,8 @@ static inline uint16_t mavlink_msg_wifi_config_ap_pack_chan(uint8_t system_id, u mavlink_wifi_config_ap_t packet; packet.mode = mode; packet.response = response; - mav_array_memcpy(packet.ssid, ssid, sizeof(char)*32); - mav_array_memcpy(packet.password, password, sizeof(char)*64); + mav_array_assign_char(packet.ssid, ssid, 32); + mav_array_assign_char(packet.password, password, 64); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN); #endif @@ -143,6 +183,20 @@ static inline uint16_t mavlink_msg_wifi_config_ap_encode_chan(uint8_t system_id, return mavlink_msg_wifi_config_ap_pack_chan(system_id, component_id, chan, msg, wifi_config_ap->ssid, wifi_config_ap->password, wifi_config_ap->mode, wifi_config_ap->response); } +/** + * @brief Encode a wifi_config_ap struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param wifi_config_ap C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_wifi_config_ap_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_wifi_config_ap_t* wifi_config_ap) +{ + return mavlink_msg_wifi_config_ap_pack_status(system_id, component_id, _status, msg, wifi_config_ap->ssid, wifi_config_ap->password, wifi_config_ap->mode, wifi_config_ap->response); +} + /** * @brief Send a wifi_config_ap message * @param chan MAVLink channel to send the message @@ -167,8 +221,8 @@ static inline void mavlink_msg_wifi_config_ap_send(mavlink_channel_t chan, const mavlink_wifi_config_ap_t packet; packet.mode = mode; packet.response = response; - mav_array_memcpy(packet.ssid, ssid, sizeof(char)*32); - mav_array_memcpy(packet.password, password, sizeof(char)*64); + mav_array_assign_char(packet.ssid, ssid, 32); + mav_array_assign_char(packet.password, password, 64); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIFI_CONFIG_AP, (const char *)&packet, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); #endif } @@ -189,7 +243,7 @@ static inline void mavlink_msg_wifi_config_ap_send_struct(mavlink_channel_t chan #if MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -208,8 +262,8 @@ static inline void mavlink_msg_wifi_config_ap_send_buf(mavlink_message_t *msgbuf mavlink_wifi_config_ap_t *packet = (mavlink_wifi_config_ap_t *)msgbuf; packet->mode = mode; packet->response = response; - mav_array_memcpy(packet->ssid, ssid, sizeof(char)*32); - mav_array_memcpy(packet->password, password, sizeof(char)*64); + mav_array_assign_char(packet->ssid, ssid, 32); + mav_array_assign_char(packet->password, password, 64); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIFI_CONFIG_AP, (const char *)packet, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); #endif } diff --git a/lib/main/MAVLink/common/mavlink_msg_winch_status.h b/lib/main/MAVLink/common/mavlink_msg_winch_status.h index 62bd61695f7..c8574ef1f91 100644 --- a/lib/main/MAVLink/common/mavlink_msg_winch_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_winch_status.h @@ -105,6 +105,60 @@ static inline uint16_t mavlink_msg_winch_status_pack(uint8_t system_id, uint8_t return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN, MAVLINK_MSG_ID_WINCH_STATUS_LEN, MAVLINK_MSG_ID_WINCH_STATUS_CRC); } +/** + * @brief Pack a winch_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (synced to UNIX time or since system boot). + * @param line_length [m] Length of line released. NaN if unknown + * @param speed [m/s] Speed line is being released or retracted. Positive values if being released, negative values if being retracted, NaN if unknown + * @param tension [kg] Tension on the line. NaN if unknown + * @param voltage [V] Voltage of the battery supplying the winch. NaN if unknown + * @param current [A] Current draw from the winch. NaN if unknown + * @param temperature [degC] Temperature of the motor. INT16_MAX if unknown + * @param status Status flags + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_winch_status_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, float line_length, float speed, float tension, float voltage, float current, int16_t temperature, uint32_t status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WINCH_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, line_length); + _mav_put_float(buf, 12, speed); + _mav_put_float(buf, 16, tension); + _mav_put_float(buf, 20, voltage); + _mav_put_float(buf, 24, current); + _mav_put_uint32_t(buf, 28, status); + _mav_put_int16_t(buf, 32, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WINCH_STATUS_LEN); +#else + mavlink_winch_status_t packet; + packet.time_usec = time_usec; + packet.line_length = line_length; + packet.speed = speed; + packet.tension = tension; + packet.voltage = voltage; + packet.current = current; + packet.status = status; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WINCH_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_WINCH_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN, MAVLINK_MSG_ID_WINCH_STATUS_LEN, MAVLINK_MSG_ID_WINCH_STATUS_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN, MAVLINK_MSG_ID_WINCH_STATUS_LEN); +#endif +} + /** * @brief Pack a winch_status message on a channel * @param system_id ID of this system @@ -182,6 +236,20 @@ static inline uint16_t mavlink_msg_winch_status_encode_chan(uint8_t system_id, u return mavlink_msg_winch_status_pack_chan(system_id, component_id, chan, msg, winch_status->time_usec, winch_status->line_length, winch_status->speed, winch_status->tension, winch_status->voltage, winch_status->current, winch_status->temperature, winch_status->status); } +/** + * @brief Encode a winch_status struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param winch_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_winch_status_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_winch_status_t* winch_status) +{ + return mavlink_msg_winch_status_pack_status(system_id, component_id, _status, msg, winch_status->time_usec, winch_status->line_length, winch_status->speed, winch_status->tension, winch_status->voltage, winch_status->current, winch_status->temperature, winch_status->status); +} + /** * @brief Send a winch_status message * @param chan MAVLink channel to send the message @@ -242,7 +310,7 @@ static inline void mavlink_msg_winch_status_send_struct(mavlink_channel_t chan, #if MAVLINK_MSG_ID_WINCH_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/common/mavlink_msg_wind_cov.h b/lib/main/MAVLink/common/mavlink_msg_wind_cov.h index abce953e21b..28ea62ea4ad 100755 --- a/lib/main/MAVLink/common/mavlink_msg_wind_cov.h +++ b/lib/main/MAVLink/common/mavlink_msg_wind_cov.h @@ -6,14 +6,14 @@ typedef struct __mavlink_wind_cov_t { uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ - float wind_x; /*< [m/s] Wind in X (NED) direction*/ - float wind_y; /*< [m/s] Wind in Y (NED) direction*/ - float wind_z; /*< [m/s] Wind in Z (NED) direction*/ - float var_horiz; /*< [m/s] Variability of the wind in XY. RMS of a 1 Hz lowpassed wind estimate.*/ - float var_vert; /*< [m/s] Variability of the wind in Z. RMS of a 1 Hz lowpassed wind estimate.*/ - float wind_alt; /*< [m] Altitude (MSL) that this measurement was taken at*/ - float horiz_accuracy; /*< [m] Horizontal speed 1-STD accuracy*/ - float vert_accuracy; /*< [m] Vertical speed 1-STD accuracy*/ + float wind_x; /*< [m/s] Wind in North (NED) direction (NAN if unknown)*/ + float wind_y; /*< [m/s] Wind in East (NED) direction (NAN if unknown)*/ + float wind_z; /*< [m/s] Wind in down (NED) direction (NAN if unknown)*/ + float var_horiz; /*< [m/s] Variability of wind in XY, 1-STD estimated from a 1 Hz lowpassed wind estimate (NAN if unknown)*/ + float var_vert; /*< [m/s] Variability of wind in Z, 1-STD estimated from a 1 Hz lowpassed wind estimate (NAN if unknown)*/ + float wind_alt; /*< [m] Altitude (MSL) that this measurement was taken at (NAN if unknown)*/ + float horiz_accuracy; /*< [m/s] Horizontal speed 1-STD accuracy (0 if unknown)*/ + float vert_accuracy; /*< [m/s] Vertical speed 1-STD accuracy (0 if unknown)*/ } mavlink_wind_cov_t; #define MAVLINK_MSG_ID_WIND_COV_LEN 40 @@ -66,14 +66,14 @@ typedef struct __mavlink_wind_cov_t { * @param msg The MAVLink message to compress the data into * * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. - * @param wind_x [m/s] Wind in X (NED) direction - * @param wind_y [m/s] Wind in Y (NED) direction - * @param wind_z [m/s] Wind in Z (NED) direction - * @param var_horiz [m/s] Variability of the wind in XY. RMS of a 1 Hz lowpassed wind estimate. - * @param var_vert [m/s] Variability of the wind in Z. RMS of a 1 Hz lowpassed wind estimate. - * @param wind_alt [m] Altitude (MSL) that this measurement was taken at - * @param horiz_accuracy [m] Horizontal speed 1-STD accuracy - * @param vert_accuracy [m] Vertical speed 1-STD accuracy + * @param wind_x [m/s] Wind in North (NED) direction (NAN if unknown) + * @param wind_y [m/s] Wind in East (NED) direction (NAN if unknown) + * @param wind_z [m/s] Wind in down (NED) direction (NAN if unknown) + * @param var_horiz [m/s] Variability of wind in XY, 1-STD estimated from a 1 Hz lowpassed wind estimate (NAN if unknown) + * @param var_vert [m/s] Variability of wind in Z, 1-STD estimated from a 1 Hz lowpassed wind estimate (NAN if unknown) + * @param wind_alt [m] Altitude (MSL) that this measurement was taken at (NAN if unknown) + * @param horiz_accuracy [m/s] Horizontal speed 1-STD accuracy (0 if unknown) + * @param vert_accuracy [m/s] Vertical speed 1-STD accuracy (0 if unknown) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_wind_cov_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -111,6 +111,63 @@ static inline uint16_t mavlink_msg_wind_cov_pack(uint8_t system_id, uint8_t comp return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WIND_COV_MIN_LEN, MAVLINK_MSG_ID_WIND_COV_LEN, MAVLINK_MSG_ID_WIND_COV_CRC); } +/** + * @brief Pack a wind_cov message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param wind_x [m/s] Wind in North (NED) direction (NAN if unknown) + * @param wind_y [m/s] Wind in East (NED) direction (NAN if unknown) + * @param wind_z [m/s] Wind in down (NED) direction (NAN if unknown) + * @param var_horiz [m/s] Variability of wind in XY, 1-STD estimated from a 1 Hz lowpassed wind estimate (NAN if unknown) + * @param var_vert [m/s] Variability of wind in Z, 1-STD estimated from a 1 Hz lowpassed wind estimate (NAN if unknown) + * @param wind_alt [m] Altitude (MSL) that this measurement was taken at (NAN if unknown) + * @param horiz_accuracy [m/s] Horizontal speed 1-STD accuracy (0 if unknown) + * @param vert_accuracy [m/s] Vertical speed 1-STD accuracy (0 if unknown) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_wind_cov_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t time_usec, float wind_x, float wind_y, float wind_z, float var_horiz, float var_vert, float wind_alt, float horiz_accuracy, float vert_accuracy) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WIND_COV_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, wind_x); + _mav_put_float(buf, 12, wind_y); + _mav_put_float(buf, 16, wind_z); + _mav_put_float(buf, 20, var_horiz); + _mav_put_float(buf, 24, var_vert); + _mav_put_float(buf, 28, wind_alt); + _mav_put_float(buf, 32, horiz_accuracy); + _mav_put_float(buf, 36, vert_accuracy); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WIND_COV_LEN); +#else + mavlink_wind_cov_t packet; + packet.time_usec = time_usec; + packet.wind_x = wind_x; + packet.wind_y = wind_y; + packet.wind_z = wind_z; + packet.var_horiz = var_horiz; + packet.var_vert = var_vert; + packet.wind_alt = wind_alt; + packet.horiz_accuracy = horiz_accuracy; + packet.vert_accuracy = vert_accuracy; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIND_COV_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_WIND_COV; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_WIND_COV_MIN_LEN, MAVLINK_MSG_ID_WIND_COV_LEN, MAVLINK_MSG_ID_WIND_COV_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_WIND_COV_MIN_LEN, MAVLINK_MSG_ID_WIND_COV_LEN); +#endif +} + /** * @brief Pack a wind_cov message on a channel * @param system_id ID of this system @@ -118,14 +175,14 @@ static inline uint16_t mavlink_msg_wind_cov_pack(uint8_t system_id, uint8_t comp * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. - * @param wind_x [m/s] Wind in X (NED) direction - * @param wind_y [m/s] Wind in Y (NED) direction - * @param wind_z [m/s] Wind in Z (NED) direction - * @param var_horiz [m/s] Variability of the wind in XY. RMS of a 1 Hz lowpassed wind estimate. - * @param var_vert [m/s] Variability of the wind in Z. RMS of a 1 Hz lowpassed wind estimate. - * @param wind_alt [m] Altitude (MSL) that this measurement was taken at - * @param horiz_accuracy [m] Horizontal speed 1-STD accuracy - * @param vert_accuracy [m] Vertical speed 1-STD accuracy + * @param wind_x [m/s] Wind in North (NED) direction (NAN if unknown) + * @param wind_y [m/s] Wind in East (NED) direction (NAN if unknown) + * @param wind_z [m/s] Wind in down (NED) direction (NAN if unknown) + * @param var_horiz [m/s] Variability of wind in XY, 1-STD estimated from a 1 Hz lowpassed wind estimate (NAN if unknown) + * @param var_vert [m/s] Variability of wind in Z, 1-STD estimated from a 1 Hz lowpassed wind estimate (NAN if unknown) + * @param wind_alt [m] Altitude (MSL) that this measurement was taken at (NAN if unknown) + * @param horiz_accuracy [m/s] Horizontal speed 1-STD accuracy (0 if unknown) + * @param vert_accuracy [m/s] Vertical speed 1-STD accuracy (0 if unknown) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_wind_cov_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -191,19 +248,33 @@ static inline uint16_t mavlink_msg_wind_cov_encode_chan(uint8_t system_id, uint8 return mavlink_msg_wind_cov_pack_chan(system_id, component_id, chan, msg, wind_cov->time_usec, wind_cov->wind_x, wind_cov->wind_y, wind_cov->wind_z, wind_cov->var_horiz, wind_cov->var_vert, wind_cov->wind_alt, wind_cov->horiz_accuracy, wind_cov->vert_accuracy); } +/** + * @brief Encode a wind_cov struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param wind_cov C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_wind_cov_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_wind_cov_t* wind_cov) +{ + return mavlink_msg_wind_cov_pack_status(system_id, component_id, _status, msg, wind_cov->time_usec, wind_cov->wind_x, wind_cov->wind_y, wind_cov->wind_z, wind_cov->var_horiz, wind_cov->var_vert, wind_cov->wind_alt, wind_cov->horiz_accuracy, wind_cov->vert_accuracy); +} + /** * @brief Send a wind_cov message * @param chan MAVLink channel to send the message * * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. - * @param wind_x [m/s] Wind in X (NED) direction - * @param wind_y [m/s] Wind in Y (NED) direction - * @param wind_z [m/s] Wind in Z (NED) direction - * @param var_horiz [m/s] Variability of the wind in XY. RMS of a 1 Hz lowpassed wind estimate. - * @param var_vert [m/s] Variability of the wind in Z. RMS of a 1 Hz lowpassed wind estimate. - * @param wind_alt [m] Altitude (MSL) that this measurement was taken at - * @param horiz_accuracy [m] Horizontal speed 1-STD accuracy - * @param vert_accuracy [m] Vertical speed 1-STD accuracy + * @param wind_x [m/s] Wind in North (NED) direction (NAN if unknown) + * @param wind_y [m/s] Wind in East (NED) direction (NAN if unknown) + * @param wind_z [m/s] Wind in down (NED) direction (NAN if unknown) + * @param var_horiz [m/s] Variability of wind in XY, 1-STD estimated from a 1 Hz lowpassed wind estimate (NAN if unknown) + * @param var_vert [m/s] Variability of wind in Z, 1-STD estimated from a 1 Hz lowpassed wind estimate (NAN if unknown) + * @param wind_alt [m] Altitude (MSL) that this measurement was taken at (NAN if unknown) + * @param horiz_accuracy [m/s] Horizontal speed 1-STD accuracy (0 if unknown) + * @param vert_accuracy [m/s] Vertical speed 1-STD accuracy (0 if unknown) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -254,7 +325,7 @@ static inline void mavlink_msg_wind_cov_send_struct(mavlink_channel_t chan, cons #if MAVLINK_MSG_ID_WIND_COV_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -310,7 +381,7 @@ static inline uint64_t mavlink_msg_wind_cov_get_time_usec(const mavlink_message_ /** * @brief Get field wind_x from wind_cov message * - * @return [m/s] Wind in X (NED) direction + * @return [m/s] Wind in North (NED) direction (NAN if unknown) */ static inline float mavlink_msg_wind_cov_get_wind_x(const mavlink_message_t* msg) { @@ -320,7 +391,7 @@ static inline float mavlink_msg_wind_cov_get_wind_x(const mavlink_message_t* msg /** * @brief Get field wind_y from wind_cov message * - * @return [m/s] Wind in Y (NED) direction + * @return [m/s] Wind in East (NED) direction (NAN if unknown) */ static inline float mavlink_msg_wind_cov_get_wind_y(const mavlink_message_t* msg) { @@ -330,7 +401,7 @@ static inline float mavlink_msg_wind_cov_get_wind_y(const mavlink_message_t* msg /** * @brief Get field wind_z from wind_cov message * - * @return [m/s] Wind in Z (NED) direction + * @return [m/s] Wind in down (NED) direction (NAN if unknown) */ static inline float mavlink_msg_wind_cov_get_wind_z(const mavlink_message_t* msg) { @@ -340,7 +411,7 @@ static inline float mavlink_msg_wind_cov_get_wind_z(const mavlink_message_t* msg /** * @brief Get field var_horiz from wind_cov message * - * @return [m/s] Variability of the wind in XY. RMS of a 1 Hz lowpassed wind estimate. + * @return [m/s] Variability of wind in XY, 1-STD estimated from a 1 Hz lowpassed wind estimate (NAN if unknown) */ static inline float mavlink_msg_wind_cov_get_var_horiz(const mavlink_message_t* msg) { @@ -350,7 +421,7 @@ static inline float mavlink_msg_wind_cov_get_var_horiz(const mavlink_message_t* /** * @brief Get field var_vert from wind_cov message * - * @return [m/s] Variability of the wind in Z. RMS of a 1 Hz lowpassed wind estimate. + * @return [m/s] Variability of wind in Z, 1-STD estimated from a 1 Hz lowpassed wind estimate (NAN if unknown) */ static inline float mavlink_msg_wind_cov_get_var_vert(const mavlink_message_t* msg) { @@ -360,7 +431,7 @@ static inline float mavlink_msg_wind_cov_get_var_vert(const mavlink_message_t* m /** * @brief Get field wind_alt from wind_cov message * - * @return [m] Altitude (MSL) that this measurement was taken at + * @return [m] Altitude (MSL) that this measurement was taken at (NAN if unknown) */ static inline float mavlink_msg_wind_cov_get_wind_alt(const mavlink_message_t* msg) { @@ -370,7 +441,7 @@ static inline float mavlink_msg_wind_cov_get_wind_alt(const mavlink_message_t* m /** * @brief Get field horiz_accuracy from wind_cov message * - * @return [m] Horizontal speed 1-STD accuracy + * @return [m/s] Horizontal speed 1-STD accuracy (0 if unknown) */ static inline float mavlink_msg_wind_cov_get_horiz_accuracy(const mavlink_message_t* msg) { @@ -380,7 +451,7 @@ static inline float mavlink_msg_wind_cov_get_horiz_accuracy(const mavlink_messag /** * @brief Get field vert_accuracy from wind_cov message * - * @return [m] Vertical speed 1-STD accuracy + * @return [m/s] Vertical speed 1-STD accuracy (0 if unknown) */ static inline float mavlink_msg_wind_cov_get_vert_accuracy(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/testsuite.h b/lib/main/MAVLink/common/testsuite.h index 8cea375547c..69e255d6432 100755 --- a/lib/main/MAVLink/common/testsuite.h +++ b/lib/main/MAVLink/common/testsuite.h @@ -1,6 +1,6 @@ /** @file * @brief MAVLink comm protocol testsuite generated from common.xml - * @see http://qgroundcontrol.org/mavlink/ + * @see https://mavlink.io/en/ */ #pragma once #ifndef COMMON_TESTSUITE_H @@ -12,17 +12,17 @@ extern "C" { #ifndef MAVLINK_TEST_ALL #define MAVLINK_TEST_ALL -static void mavlink_test_minimal(uint8_t, uint8_t, mavlink_message_t *last_msg); +static void mavlink_test_standard(uint8_t, uint8_t, mavlink_message_t *last_msg); static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { - mavlink_test_minimal(system_id, component_id, last_msg); + mavlink_test_standard(system_id, component_id, last_msg); mavlink_test_common(system_id, component_id, last_msg); } #endif -#include "../minimal/testsuite.h" +#include "../standard/testsuite.h" static void mavlink_test_sys_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -37,7 +37,7 @@ static void mavlink_test_sys_status(uint8_t system_id, uint8_t component_id, mav uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_sys_status_t packet_in = { - 963497464,963497672,963497880,17859,17963,18067,18171,18275,18379,18483,18587,18691,223 + 963497464,963497672,963497880,17859,17963,18067,18171,18275,18379,18483,18587,18691,223,963499076,963499284,963499492 }; mavlink_sys_status_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -54,6 +54,9 @@ static void mavlink_test_sys_status(uint8_t system_id, uint8_t component_id, mav packet1.errors_count3 = packet_in.errors_count3; packet1.errors_count4 = packet_in.errors_count4; packet1.battery_remaining = packet_in.battery_remaining; + packet1.onboard_control_sensors_present_extended = packet_in.onboard_control_sensors_present_extended; + packet1.onboard_control_sensors_enabled_extended = packet_in.onboard_control_sensors_enabled_extended; + packet1.onboard_control_sensors_health_extended = packet_in.onboard_control_sensors_health_extended; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -68,12 +71,12 @@ static void mavlink_test_sys_status(uint8_t system_id, uint8_t component_id, mav MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_sys_status_pack(system_id, component_id, &msg , packet1.onboard_control_sensors_present , packet1.onboard_control_sensors_enabled , packet1.onboard_control_sensors_health , packet1.load , packet1.voltage_battery , packet1.current_battery , packet1.battery_remaining , packet1.drop_rate_comm , packet1.errors_comm , packet1.errors_count1 , packet1.errors_count2 , packet1.errors_count3 , packet1.errors_count4 ); + mavlink_msg_sys_status_pack(system_id, component_id, &msg , packet1.onboard_control_sensors_present , packet1.onboard_control_sensors_enabled , packet1.onboard_control_sensors_health , packet1.load , packet1.voltage_battery , packet1.current_battery , packet1.battery_remaining , packet1.drop_rate_comm , packet1.errors_comm , packet1.errors_count1 , packet1.errors_count2 , packet1.errors_count3 , packet1.errors_count4 , packet1.onboard_control_sensors_present_extended , packet1.onboard_control_sensors_enabled_extended , packet1.onboard_control_sensors_health_extended ); mavlink_msg_sys_status_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_sys_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.onboard_control_sensors_present , packet1.onboard_control_sensors_enabled , packet1.onboard_control_sensors_health , packet1.load , packet1.voltage_battery , packet1.current_battery , packet1.battery_remaining , packet1.drop_rate_comm , packet1.errors_comm , packet1.errors_count1 , packet1.errors_count2 , packet1.errors_count3 , packet1.errors_count4 ); + mavlink_msg_sys_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.onboard_control_sensors_present , packet1.onboard_control_sensors_enabled , packet1.onboard_control_sensors_health , packet1.load , packet1.voltage_battery , packet1.current_battery , packet1.battery_remaining , packet1.drop_rate_comm , packet1.errors_comm , packet1.errors_count1 , packet1.errors_count2 , packet1.errors_count3 , packet1.errors_count4 , packet1.onboard_control_sensors_present_extended , packet1.onboard_control_sensors_enabled_extended , packet1.onboard_control_sensors_health_extended ); mavlink_msg_sys_status_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -86,9 +89,14 @@ static void mavlink_test_sys_status(uint8_t system_id, uint8_t component_id, mav MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_sys_status_send(MAVLINK_COMM_1 , packet1.onboard_control_sensors_present , packet1.onboard_control_sensors_enabled , packet1.onboard_control_sensors_health , packet1.load , packet1.voltage_battery , packet1.current_battery , packet1.battery_remaining , packet1.drop_rate_comm , packet1.errors_comm , packet1.errors_count1 , packet1.errors_count2 , packet1.errors_count3 , packet1.errors_count4 ); + mavlink_msg_sys_status_send(MAVLINK_COMM_1 , packet1.onboard_control_sensors_present , packet1.onboard_control_sensors_enabled , packet1.onboard_control_sensors_health , packet1.load , packet1.voltage_battery , packet1.current_battery , packet1.battery_remaining , packet1.drop_rate_comm , packet1.errors_comm , packet1.errors_count1 , packet1.errors_count2 , packet1.errors_count3 , packet1.errors_count4 , packet1.onboard_control_sensors_present_extended , packet1.onboard_control_sensors_enabled_extended , packet1.onboard_control_sensors_health_extended ); mavlink_msg_sys_status_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("SYS_STATUS") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_SYS_STATUS) != NULL); +#endif } static void mavlink_test_system_time(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -144,6 +152,11 @@ static void mavlink_test_system_time(uint8_t system_id, uint8_t component_id, ma mavlink_msg_system_time_send(MAVLINK_COMM_1 , packet1.time_unix_usec , packet1.time_boot_ms ); mavlink_msg_system_time_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("SYSTEM_TIME") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_SYSTEM_TIME) != NULL); +#endif } static void mavlink_test_ping(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -201,6 +214,11 @@ static void mavlink_test_ping(uint8_t system_id, uint8_t component_id, mavlink_m mavlink_msg_ping_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.seq , packet1.target_system , packet1.target_component ); mavlink_msg_ping_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("PING") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_PING) != NULL); +#endif } static void mavlink_test_change_operator_control(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -258,6 +276,11 @@ static void mavlink_test_change_operator_control(uint8_t system_id, uint8_t comp mavlink_msg_change_operator_control_send(MAVLINK_COMM_1 , packet1.target_system , packet1.control_request , packet1.version , packet1.passkey ); mavlink_msg_change_operator_control_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("CHANGE_OPERATOR_CONTROL") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL) != NULL); +#endif } static void mavlink_test_change_operator_control_ack(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -314,6 +337,11 @@ static void mavlink_test_change_operator_control_ack(uint8_t system_id, uint8_t mavlink_msg_change_operator_control_ack_send(MAVLINK_COMM_1 , packet1.gcs_system_id , packet1.control_request , packet1.ack ); mavlink_msg_change_operator_control_ack_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("CHANGE_OPERATOR_CONTROL_ACK") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK) != NULL); +#endif } static void mavlink_test_auth_key(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -368,6 +396,11 @@ static void mavlink_test_auth_key(uint8_t system_id, uint8_t component_id, mavli mavlink_msg_auth_key_send(MAVLINK_COMM_1 , packet1.key ); mavlink_msg_auth_key_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("AUTH_KEY") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_AUTH_KEY) != NULL); +#endif } static void mavlink_test_link_node_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -432,6 +465,11 @@ static void mavlink_test_link_node_status(uint8_t system_id, uint8_t component_i mavlink_msg_link_node_status_send(MAVLINK_COMM_1 , packet1.timestamp , packet1.tx_buf , packet1.rx_buf , packet1.tx_rate , packet1.rx_rate , packet1.rx_parse_err , packet1.tx_overflows , packet1.rx_overflows , packet1.messages_sent , packet1.messages_received , packet1.messages_lost ); mavlink_msg_link_node_status_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("LINK_NODE_STATUS") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_LINK_NODE_STATUS) != NULL); +#endif } static void mavlink_test_set_mode(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -488,65 +526,11 @@ static void mavlink_test_set_mode(uint8_t system_id, uint8_t component_id, mavli mavlink_msg_set_mode_send(MAVLINK_COMM_1 , packet1.target_system , packet1.base_mode , packet1.custom_mode ); mavlink_msg_set_mode_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); -} -static void mavlink_test_param_ack_transaction(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ -#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 - mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0); - if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION >= 256) { - return; - } -#endif - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_param_ack_transaction_t packet_in = { - 17.0,17,84,"GHIJKLMNOPQRSTU",199,10 - }; - mavlink_param_ack_transaction_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.param_value = packet_in.param_value; - packet1.target_system = packet_in.target_system; - packet1.target_component = packet_in.target_component; - packet1.param_type = packet_in.param_type; - packet1.param_result = packet_in.param_result; - - mav_array_memcpy(packet1.param_id, packet_in.param_id, sizeof(char)*16); - -#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 - if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { - // cope with extensions - memset(MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PARAM_ACK_TRANSACTION_MIN_LEN); - } +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("SET_MODE") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_SET_MODE) != NULL); #endif - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_param_ack_transaction_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_param_ack_transaction_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_param_ack_transaction_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_value , packet1.param_type , packet1.param_result ); - mavlink_msg_param_ack_transaction_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_param_ack_transaction_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_value , packet1.param_type , packet1.param_result ); - mavlink_msg_param_ack_transaction_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_MISSION_CHANGED >= 256) { - return; - } -#endif - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_mission_changed_t packet_in = { - 17235,17339,17,84,151 - }; - mavlink_mission_changed_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.start_index = packet_in.start_index; - packet1.end_index = packet_in.end_index; - packet1.origin_sysid = packet_in.origin_sysid; - packet1.origin_compid = packet_in.origin_compid; - packet1.mission_type = packet_in.mission_type; - - -#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 - if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { - // cope with extensions - memset(MAVLINK_MSG_ID_MISSION_CHANGED_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_MISSION_CHANGED_MIN_LEN); - } +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("MISSION_REQUEST_INT") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_MISSION_REQUEST_INT) != NULL); #endif - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_changed_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_mission_changed_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_changed_pack(system_id, component_id, &msg , packet1.start_index , packet1.end_index , packet1.origin_sysid , packet1.origin_compid , packet1.mission_type ); - mavlink_msg_mission_changed_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_changed_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.start_index , packet1.end_index , packet1.origin_sysid , packet1.origin_compid , packet1.mission_type ); - mavlink_msg_mission_changed_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -3677,12 +3872,12 @@ static void mavlink_test_set_attitude_target(uint8_t system_id, uint8_t componen MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_set_attitude_target_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.type_mask , packet1.q , packet1.body_roll_rate , packet1.body_pitch_rate , packet1.body_yaw_rate , packet1.thrust ); + mavlink_msg_set_attitude_target_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.type_mask , packet1.q , packet1.body_roll_rate , packet1.body_pitch_rate , packet1.body_yaw_rate , packet1.thrust , packet1.thrust_body ); mavlink_msg_set_attitude_target_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_set_attitude_target_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.type_mask , packet1.q , packet1.body_roll_rate , packet1.body_pitch_rate , packet1.body_yaw_rate , packet1.thrust ); + mavlink_msg_set_attitude_target_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.type_mask , packet1.q , packet1.body_roll_rate , packet1.body_pitch_rate , packet1.body_yaw_rate , packet1.thrust , packet1.thrust_body ); mavlink_msg_set_attitude_target_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -3695,9 +3890,14 @@ static void mavlink_test_set_attitude_target(uint8_t system_id, uint8_t componen MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_set_attitude_target_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.type_mask , packet1.q , packet1.body_roll_rate , packet1.body_pitch_rate , packet1.body_yaw_rate , packet1.thrust ); + mavlink_msg_set_attitude_target_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.type_mask , packet1.q , packet1.body_roll_rate , packet1.body_pitch_rate , packet1.body_yaw_rate , packet1.thrust , packet1.thrust_body ); mavlink_msg_set_attitude_target_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("SET_ATTITUDE_TARGET") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_SET_ATTITUDE_TARGET) != NULL); +#endif } static void mavlink_test_attitude_target(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -3758,6 +3958,11 @@ static void mavlink_test_attitude_target(uint8_t system_id, uint8_t component_id mavlink_msg_attitude_target_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.type_mask , packet1.q , packet1.body_roll_rate , packet1.body_pitch_rate , packet1.body_yaw_rate , packet1.thrust ); mavlink_msg_attitude_target_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("ATTITUDE_TARGET") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_ATTITUDE_TARGET) != NULL); +#endif } static void mavlink_test_set_position_target_local_ned(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -3827,6 +4032,11 @@ static void mavlink_test_set_position_target_local_ned(uint8_t system_id, uint8_ mavlink_msg_set_position_target_local_ned_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.coordinate_frame , packet1.type_mask , packet1.x , packet1.y , packet1.z , packet1.vx , packet1.vy , packet1.vz , packet1.afx , packet1.afy , packet1.afz , packet1.yaw , packet1.yaw_rate ); mavlink_msg_set_position_target_local_ned_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("SET_POSITION_TARGET_LOCAL_NED") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED) != NULL); +#endif } static void mavlink_test_position_target_local_ned(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -3894,6 +4104,11 @@ static void mavlink_test_position_target_local_ned(uint8_t system_id, uint8_t co mavlink_msg_position_target_local_ned_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.coordinate_frame , packet1.type_mask , packet1.x , packet1.y , packet1.z , packet1.vx , packet1.vy , packet1.vz , packet1.afx , packet1.afy , packet1.afz , packet1.yaw , packet1.yaw_rate ); mavlink_msg_position_target_local_ned_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("POSITION_TARGET_LOCAL_NED") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED) != NULL); +#endif } static void mavlink_test_set_position_target_global_int(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -3963,6 +4178,11 @@ static void mavlink_test_set_position_target_global_int(uint8_t system_id, uint8 mavlink_msg_set_position_target_global_int_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.target_system , packet1.target_component , packet1.coordinate_frame , packet1.type_mask , packet1.lat_int , packet1.lon_int , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.afx , packet1.afy , packet1.afz , packet1.yaw , packet1.yaw_rate ); mavlink_msg_set_position_target_global_int_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("SET_POSITION_TARGET_GLOBAL_INT") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT) != NULL); +#endif } static void mavlink_test_position_target_global_int(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -4030,6 +4250,11 @@ static void mavlink_test_position_target_global_int(uint8_t system_id, uint8_t c mavlink_msg_position_target_global_int_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.coordinate_frame , packet1.type_mask , packet1.lat_int , packet1.lon_int , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.afx , packet1.afy , packet1.afz , packet1.yaw , packet1.yaw_rate ); mavlink_msg_position_target_global_int_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("POSITION_TARGET_GLOBAL_INT") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT) != NULL); +#endif } static void mavlink_test_local_position_ned_system_global_offset(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -4090,6 +4315,11 @@ static void mavlink_test_local_position_ned_system_global_offset(uint8_t system_ mavlink_msg_local_position_ned_system_global_offset_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); mavlink_msg_local_position_ned_system_global_offset_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET) != NULL); +#endif } static void mavlink_test_hil_state(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -4159,6 +4389,11 @@ static void mavlink_test_hil_state(uint8_t system_id, uint8_t component_id, mavl mavlink_msg_hil_state_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.roll , packet1.pitch , packet1.yaw , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.xacc , packet1.yacc , packet1.zacc ); mavlink_msg_hil_state_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("HIL_STATE") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_HIL_STATE) != NULL); +#endif } static void mavlink_test_hil_controls(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -4223,6 +4458,11 @@ static void mavlink_test_hil_controls(uint8_t system_id, uint8_t component_id, m mavlink_msg_hil_controls_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.roll_ailerons , packet1.pitch_elevator , packet1.yaw_rudder , packet1.throttle , packet1.aux1 , packet1.aux2 , packet1.aux3 , packet1.aux4 , packet1.mode , packet1.nav_mode ); mavlink_msg_hil_controls_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("HIL_CONTROLS") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_HIL_CONTROLS) != NULL); +#endif } static void mavlink_test_hil_rc_inputs_raw(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -4290,6 +4530,11 @@ static void mavlink_test_hil_rc_inputs_raw(uint8_t system_id, uint8_t component_ mavlink_msg_hil_rc_inputs_raw_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.chan9_raw , packet1.chan10_raw , packet1.chan11_raw , packet1.chan12_raw , packet1.rssi ); mavlink_msg_hil_rc_inputs_raw_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("HIL_RC_INPUTS_RAW") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW) != NULL); +#endif } static void mavlink_test_hil_actuator_controls(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -4347,6 +4592,11 @@ static void mavlink_test_hil_actuator_controls(uint8_t system_id, uint8_t compon mavlink_msg_hil_actuator_controls_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.controls , packet1.mode , packet1.flags ); mavlink_msg_hil_actuator_controls_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("HIL_ACTUATOR_CONTROLS") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS) != NULL); +#endif } static void mavlink_test_optical_flow(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -4410,6 +4660,11 @@ static void mavlink_test_optical_flow(uint8_t system_id, uint8_t component_id, m mavlink_msg_optical_flow_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance , packet1.flow_rate_x , packet1.flow_rate_y ); mavlink_msg_optical_flow_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("OPTICAL_FLOW") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_OPTICAL_FLOW) != NULL); +#endif } static void mavlink_test_global_vision_position_estimate(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -4472,6 +4727,11 @@ static void mavlink_test_global_vision_position_estimate(uint8_t system_id, uint mavlink_msg_global_vision_position_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance , packet1.reset_counter ); mavlink_msg_global_vision_position_estimate_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("GLOBAL_VISION_POSITION_ESTIMATE") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE) != NULL); +#endif } static void mavlink_test_vision_position_estimate(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -4534,6 +4794,11 @@ static void mavlink_test_vision_position_estimate(uint8_t system_id, uint8_t com mavlink_msg_vision_position_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance , packet1.reset_counter ); mavlink_msg_vision_position_estimate_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("VISION_POSITION_ESTIMATE") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) != NULL); +#endif } static void mavlink_test_vision_speed_estimate(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -4593,6 +4858,11 @@ static void mavlink_test_vision_speed_estimate(uint8_t system_id, uint8_t compon mavlink_msg_vision_speed_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.covariance , packet1.reset_counter ); mavlink_msg_vision_speed_estimate_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("VISION_SPEED_ESTIMATE") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE) != NULL); +#endif } static void mavlink_test_vicon_position_estimate(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -4654,6 +4924,11 @@ static void mavlink_test_vicon_position_estimate(uint8_t system_id, uint8_t comp mavlink_msg_vicon_position_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance ); mavlink_msg_vicon_position_estimate_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("VICON_POSITION_ESTIMATE") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) != NULL); +#endif } static void mavlink_test_highres_imu(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -4723,6 +4998,11 @@ static void mavlink_test_highres_imu(uint8_t system_id, uint8_t component_id, ma mavlink_msg_highres_imu_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated , packet1.id ); mavlink_msg_highres_imu_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("HIGHRES_IMU") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_HIGHRES_IMU) != NULL); +#endif } static void mavlink_test_optical_flow_rad(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -4788,6 +5068,11 @@ static void mavlink_test_optical_flow_rad(uint8_t system_id, uint8_t component_i mavlink_msg_optical_flow_rad_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.sensor_id , packet1.integration_time_us , packet1.integrated_x , packet1.integrated_y , packet1.integrated_xgyro , packet1.integrated_ygyro , packet1.integrated_zgyro , packet1.temperature , packet1.quality , packet1.time_delta_distance_us , packet1.distance ); mavlink_msg_optical_flow_rad_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("OPTICAL_FLOW_RAD") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD) != NULL); +#endif } static void mavlink_test_hil_sensor(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -4857,6 +5142,11 @@ static void mavlink_test_hil_sensor(uint8_t system_id, uint8_t component_id, mav mavlink_msg_hil_sensor_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated , packet1.id ); mavlink_msg_hil_sensor_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("HIL_SENSOR") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_HIL_SENSOR) != NULL); +#endif } static void mavlink_test_sim_state(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -4871,7 +5161,7 @@ static void mavlink_test_sim_state(uint8_t system_id, uint8_t component_id, mavl uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_sim_state_t packet_in = { - 17.0,45.0,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,297.0,325.0,353.0,381.0,409.0,437.0,465.0,493.0,521.0,549.0,577.0 + 17.0,45.0,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,297.0,325.0,353.0,381.0,409.0,437.0,465.0,493.0,521.0,549.0,577.0,963501832,963502040 }; mavlink_sim_state_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4896,6 +5186,8 @@ static void mavlink_test_sim_state(uint8_t system_id, uint8_t component_id, mavl packet1.vn = packet_in.vn; packet1.ve = packet_in.ve; packet1.vd = packet_in.vd; + packet1.lat_int = packet_in.lat_int; + packet1.lon_int = packet_in.lon_int; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -4910,12 +5202,12 @@ static void mavlink_test_sim_state(uint8_t system_id, uint8_t component_id, mavl MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_sim_state_pack(system_id, component_id, &msg , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.roll , packet1.pitch , packet1.yaw , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.lat , packet1.lon , packet1.alt , packet1.std_dev_horz , packet1.std_dev_vert , packet1.vn , packet1.ve , packet1.vd ); + mavlink_msg_sim_state_pack(system_id, component_id, &msg , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.roll , packet1.pitch , packet1.yaw , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.lat , packet1.lon , packet1.alt , packet1.std_dev_horz , packet1.std_dev_vert , packet1.vn , packet1.ve , packet1.vd , packet1.lat_int , packet1.lon_int ); mavlink_msg_sim_state_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_sim_state_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.roll , packet1.pitch , packet1.yaw , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.lat , packet1.lon , packet1.alt , packet1.std_dev_horz , packet1.std_dev_vert , packet1.vn , packet1.ve , packet1.vd ); + mavlink_msg_sim_state_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.roll , packet1.pitch , packet1.yaw , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.lat , packet1.lon , packet1.alt , packet1.std_dev_horz , packet1.std_dev_vert , packet1.vn , packet1.ve , packet1.vd , packet1.lat_int , packet1.lon_int ); mavlink_msg_sim_state_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -4928,9 +5220,14 @@ static void mavlink_test_sim_state(uint8_t system_id, uint8_t component_id, mavl MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_sim_state_send(MAVLINK_COMM_1 , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.roll , packet1.pitch , packet1.yaw , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.lat , packet1.lon , packet1.alt , packet1.std_dev_horz , packet1.std_dev_vert , packet1.vn , packet1.ve , packet1.vd ); + mavlink_msg_sim_state_send(MAVLINK_COMM_1 , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.roll , packet1.pitch , packet1.yaw , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.lat , packet1.lon , packet1.alt , packet1.std_dev_horz , packet1.std_dev_vert , packet1.vn , packet1.ve , packet1.vd , packet1.lat_int , packet1.lon_int ); mavlink_msg_sim_state_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("SIM_STATE") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_SIM_STATE) != NULL); +#endif } static void mavlink_test_radio_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -4991,6 +5288,11 @@ static void mavlink_test_radio_status(uint8_t system_id, uint8_t component_id, m mavlink_msg_radio_status_send(MAVLINK_COMM_1 , packet1.rssi , packet1.remrssi , packet1.txbuf , packet1.noise , packet1.remnoise , packet1.rxerrors , packet1.fixed ); mavlink_msg_radio_status_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("RADIO_STATUS") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_RADIO_STATUS) != NULL); +#endif } static void mavlink_test_file_transfer_protocol(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -5048,6 +5350,11 @@ static void mavlink_test_file_transfer_protocol(uint8_t system_id, uint8_t compo mavlink_msg_file_transfer_protocol_send(MAVLINK_COMM_1 , packet1.target_network , packet1.target_system , packet1.target_component , packet1.payload ); mavlink_msg_file_transfer_protocol_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("FILE_TRANSFER_PROTOCOL") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) != NULL); +#endif } static void mavlink_test_timesync(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -5062,12 +5369,14 @@ static void mavlink_test_timesync(uint8_t system_id, uint8_t component_id, mavli uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_timesync_t packet_in = { - 93372036854775807LL,93372036854776311LL + 93372036854775807LL,93372036854776311LL,53,120 }; mavlink_timesync_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.tc1 = packet_in.tc1; packet1.ts1 = packet_in.ts1; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -5082,12 +5391,12 @@ static void mavlink_test_timesync(uint8_t system_id, uint8_t component_id, mavli MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_timesync_pack(system_id, component_id, &msg , packet1.tc1 , packet1.ts1 ); + mavlink_msg_timesync_pack(system_id, component_id, &msg , packet1.tc1 , packet1.ts1 , packet1.target_system , packet1.target_component ); mavlink_msg_timesync_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_timesync_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.tc1 , packet1.ts1 ); + mavlink_msg_timesync_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.tc1 , packet1.ts1 , packet1.target_system , packet1.target_component ); mavlink_msg_timesync_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -5100,9 +5409,14 @@ static void mavlink_test_timesync(uint8_t system_id, uint8_t component_id, mavli MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_timesync_send(MAVLINK_COMM_1 , packet1.tc1 , packet1.ts1 ); + mavlink_msg_timesync_send(MAVLINK_COMM_1 , packet1.tc1 , packet1.ts1 , packet1.target_system , packet1.target_component ); mavlink_msg_timesync_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("TIMESYNC") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_TIMESYNC) != NULL); +#endif } static void mavlink_test_camera_trigger(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -5158,6 +5472,11 @@ static void mavlink_test_camera_trigger(uint8_t system_id, uint8_t component_id, mavlink_msg_camera_trigger_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.seq ); mavlink_msg_camera_trigger_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("CAMERA_TRIGGER") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_CAMERA_TRIGGER) != NULL); +#endif } static void mavlink_test_hil_gps(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -5226,6 +5545,11 @@ static void mavlink_test_hil_gps(uint8_t system_id, uint8_t component_id, mavlin mavlink_msg_hil_gps_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.vn , packet1.ve , packet1.vd , packet1.cog , packet1.satellites_visible , packet1.id , packet1.yaw ); mavlink_msg_hil_gps_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("HIL_GPS") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_HIL_GPS) != NULL); +#endif } static void mavlink_test_hil_optical_flow(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -5291,6 +5615,11 @@ static void mavlink_test_hil_optical_flow(uint8_t system_id, uint8_t component_i mavlink_msg_hil_optical_flow_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.sensor_id , packet1.integration_time_us , packet1.integrated_x , packet1.integrated_y , packet1.integrated_xgyro , packet1.integrated_ygyro , packet1.integrated_zgyro , packet1.temperature , packet1.quality , packet1.time_delta_distance_us , packet1.distance ); mavlink_msg_hil_optical_flow_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("HIL_OPTICAL_FLOW") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_HIL_OPTICAL_FLOW) != NULL); +#endif } static void mavlink_test_hil_state_quaternion(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -5360,6 +5689,11 @@ static void mavlink_test_hil_state_quaternion(uint8_t system_id, uint8_t compone mavlink_msg_hil_state_quaternion_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.attitude_quaternion , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.ind_airspeed , packet1.true_airspeed , packet1.xacc , packet1.yacc , packet1.zacc ); mavlink_msg_hil_state_quaternion_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("HIL_STATE_QUATERNION") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_HIL_STATE_QUATERNION) != NULL); +#endif } static void mavlink_test_scaled_imu2(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -5424,6 +5758,11 @@ static void mavlink_test_scaled_imu2(uint8_t system_id, uint8_t component_id, ma mavlink_msg_scaled_imu2_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.temperature ); mavlink_msg_scaled_imu2_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("SCALED_IMU2") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_SCALED_IMU2) != NULL); +#endif } static void mavlink_test_log_request_list(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -5481,6 +5820,11 @@ static void mavlink_test_log_request_list(uint8_t system_id, uint8_t component_i mavlink_msg_log_request_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.start , packet1.end ); mavlink_msg_log_request_list_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("LOG_REQUEST_LIST") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_LOG_REQUEST_LIST) != NULL); +#endif } static void mavlink_test_log_entry(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -5539,6 +5883,11 @@ static void mavlink_test_log_entry(uint8_t system_id, uint8_t component_id, mavl mavlink_msg_log_entry_send(MAVLINK_COMM_1 , packet1.id , packet1.num_logs , packet1.last_log_num , packet1.time_utc , packet1.size ); mavlink_msg_log_entry_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("LOG_ENTRY") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_LOG_ENTRY) != NULL); +#endif } static void mavlink_test_log_request_data(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -5597,6 +5946,11 @@ static void mavlink_test_log_request_data(uint8_t system_id, uint8_t component_i mavlink_msg_log_request_data_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.id , packet1.ofs , packet1.count ); mavlink_msg_log_request_data_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("LOG_REQUEST_DATA") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_LOG_REQUEST_DATA) != NULL); +#endif } static void mavlink_test_log_data(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -5654,6 +6008,11 @@ static void mavlink_test_log_data(uint8_t system_id, uint8_t component_id, mavli mavlink_msg_log_data_send(MAVLINK_COMM_1 , packet1.id , packet1.ofs , packet1.count , packet1.data ); mavlink_msg_log_data_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("LOG_DATA") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_LOG_DATA) != NULL); +#endif } static void mavlink_test_log_erase(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -5709,6 +6068,11 @@ static void mavlink_test_log_erase(uint8_t system_id, uint8_t component_id, mavl mavlink_msg_log_erase_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component ); mavlink_msg_log_erase_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("LOG_ERASE") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_LOG_ERASE) != NULL); +#endif } static void mavlink_test_log_request_end(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -5764,6 +6128,11 @@ static void mavlink_test_log_request_end(uint8_t system_id, uint8_t component_id mavlink_msg_log_request_end_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component ); mavlink_msg_log_request_end_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("LOG_REQUEST_END") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_LOG_REQUEST_END) != NULL); +#endif } static void mavlink_test_gps_inject_data(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -5821,6 +6190,11 @@ static void mavlink_test_gps_inject_data(uint8_t system_id, uint8_t component_id mavlink_msg_gps_inject_data_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.len , packet1.data ); mavlink_msg_gps_inject_data_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("GPS_INJECT_DATA") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_GPS_INJECT_DATA) != NULL); +#endif } static void mavlink_test_gps2_raw(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -5835,7 +6209,7 @@ static void mavlink_test_gps2_raw(uint8_t system_id, uint8_t component_id, mavli uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_gps2_raw_t packet_in = { - 93372036854775807ULL,963497880,963498088,963498296,963498504,18483,18587,18691,18795,101,168,235,19055 + 93372036854775807ULL,963497880,963498088,963498296,963498504,18483,18587,18691,18795,101,168,235,19055,963499388,963499596,963499804,963500012,963500220 }; mavlink_gps2_raw_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -5852,6 +6226,11 @@ static void mavlink_test_gps2_raw(uint8_t system_id, uint8_t component_id, mavli packet1.satellites_visible = packet_in.satellites_visible; packet1.dgps_numch = packet_in.dgps_numch; packet1.yaw = packet_in.yaw; + packet1.alt_ellipsoid = packet_in.alt_ellipsoid; + packet1.h_acc = packet_in.h_acc; + packet1.v_acc = packet_in.v_acc; + packet1.vel_acc = packet_in.vel_acc; + packet1.hdg_acc = packet_in.hdg_acc; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -5866,12 +6245,12 @@ static void mavlink_test_gps2_raw(uint8_t system_id, uint8_t component_id, mavli MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_gps2_raw_pack(system_id, component_id, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age , packet1.yaw ); + mavlink_msg_gps2_raw_pack(system_id, component_id, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age , packet1.yaw , packet1.alt_ellipsoid , packet1.h_acc , packet1.v_acc , packet1.vel_acc , packet1.hdg_acc ); mavlink_msg_gps2_raw_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_gps2_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age , packet1.yaw ); + mavlink_msg_gps2_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age , packet1.yaw , packet1.alt_ellipsoid , packet1.h_acc , packet1.v_acc , packet1.vel_acc , packet1.hdg_acc ); mavlink_msg_gps2_raw_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -5884,9 +6263,14 @@ static void mavlink_test_gps2_raw(uint8_t system_id, uint8_t component_id, mavli MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_gps2_raw_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age , packet1.yaw ); + mavlink_msg_gps2_raw_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age , packet1.yaw , packet1.alt_ellipsoid , packet1.h_acc , packet1.v_acc , packet1.vel_acc , packet1.hdg_acc ); mavlink_msg_gps2_raw_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("GPS2_RAW") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_GPS2_RAW) != NULL); +#endif } static void mavlink_test_power_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -5943,6 +6327,11 @@ static void mavlink_test_power_status(uint8_t system_id, uint8_t component_id, m mavlink_msg_power_status_send(MAVLINK_COMM_1 , packet1.Vcc , packet1.Vservo , packet1.flags ); mavlink_msg_power_status_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("POWER_STATUS") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_POWER_STATUS) != NULL); +#endif } static void mavlink_test_serial_control(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -5957,7 +6346,7 @@ static void mavlink_test_serial_control(uint8_t system_id, uint8_t component_id, uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_serial_control_t packet_in = { - 963497464,17443,151,218,29,{ 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165 } + 963497464,17443,151,218,29,{ 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165 },178,245 }; mavlink_serial_control_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -5966,6 +6355,8 @@ static void mavlink_test_serial_control(uint8_t system_id, uint8_t component_id, packet1.device = packet_in.device; packet1.flags = packet_in.flags; packet1.count = packet_in.count; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*70); @@ -5981,12 +6372,12 @@ static void mavlink_test_serial_control(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_serial_control_pack(system_id, component_id, &msg , packet1.device , packet1.flags , packet1.timeout , packet1.baudrate , packet1.count , packet1.data ); + mavlink_msg_serial_control_pack(system_id, component_id, &msg , packet1.device , packet1.flags , packet1.timeout , packet1.baudrate , packet1.count , packet1.data , packet1.target_system , packet1.target_component ); mavlink_msg_serial_control_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_serial_control_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.device , packet1.flags , packet1.timeout , packet1.baudrate , packet1.count , packet1.data ); + mavlink_msg_serial_control_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.device , packet1.flags , packet1.timeout , packet1.baudrate , packet1.count , packet1.data , packet1.target_system , packet1.target_component ); mavlink_msg_serial_control_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -5999,9 +6390,14 @@ static void mavlink_test_serial_control(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_serial_control_send(MAVLINK_COMM_1 , packet1.device , packet1.flags , packet1.timeout , packet1.baudrate , packet1.count , packet1.data ); + mavlink_msg_serial_control_send(MAVLINK_COMM_1 , packet1.device , packet1.flags , packet1.timeout , packet1.baudrate , packet1.count , packet1.data , packet1.target_system , packet1.target_component ); mavlink_msg_serial_control_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("SERIAL_CONTROL") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_SERIAL_CONTROL) != NULL); +#endif } static void mavlink_test_gps_rtk(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -6068,6 +6464,11 @@ static void mavlink_test_gps_rtk(uint8_t system_id, uint8_t component_id, mavlin mavlink_msg_gps_rtk_send(MAVLINK_COMM_1 , packet1.time_last_baseline_ms , packet1.rtk_receiver_id , packet1.wn , packet1.tow , packet1.rtk_health , packet1.rtk_rate , packet1.nsats , packet1.baseline_coords_type , packet1.baseline_a_mm , packet1.baseline_b_mm , packet1.baseline_c_mm , packet1.accuracy , packet1.iar_num_hypotheses ); mavlink_msg_gps_rtk_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("GPS_RTK") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_GPS_RTK) != NULL); +#endif } static void mavlink_test_gps2_rtk(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -6134,6 +6535,11 @@ static void mavlink_test_gps2_rtk(uint8_t system_id, uint8_t component_id, mavli mavlink_msg_gps2_rtk_send(MAVLINK_COMM_1 , packet1.time_last_baseline_ms , packet1.rtk_receiver_id , packet1.wn , packet1.tow , packet1.rtk_health , packet1.rtk_rate , packet1.nsats , packet1.baseline_coords_type , packet1.baseline_a_mm , packet1.baseline_b_mm , packet1.baseline_c_mm , packet1.accuracy , packet1.iar_num_hypotheses ); mavlink_msg_gps2_rtk_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("GPS2_RTK") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_GPS2_RTK) != NULL); +#endif } static void mavlink_test_scaled_imu3(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -6198,6 +6604,11 @@ static void mavlink_test_scaled_imu3(uint8_t system_id, uint8_t component_id, ma mavlink_msg_scaled_imu3_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.temperature ); mavlink_msg_scaled_imu3_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("SCALED_IMU3") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_SCALED_IMU3) != NULL); +#endif } static void mavlink_test_data_transmission_handshake(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -6258,6 +6669,11 @@ static void mavlink_test_data_transmission_handshake(uint8_t system_id, uint8_t mavlink_msg_data_transmission_handshake_send(MAVLINK_COMM_1 , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality ); mavlink_msg_data_transmission_handshake_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("DATA_TRANSMISSION_HANDSHAKE") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE) != NULL); +#endif } static void mavlink_test_encapsulated_data(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -6313,6 +6729,11 @@ static void mavlink_test_encapsulated_data(uint8_t system_id, uint8_t component_ mavlink_msg_encapsulated_data_send(MAVLINK_COMM_1 , packet1.seqnr , packet1.data ); mavlink_msg_encapsulated_data_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("ENCAPSULATED_DATA") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_ENCAPSULATED_DATA) != NULL); +#endif } static void mavlink_test_distance_sensor(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -6378,6 +6799,11 @@ static void mavlink_test_distance_sensor(uint8_t system_id, uint8_t component_id mavlink_msg_distance_sensor_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.min_distance , packet1.max_distance , packet1.current_distance , packet1.type , packet1.id , packet1.orientation , packet1.covariance , packet1.horizontal_fov , packet1.vertical_fov , packet1.quaternion , packet1.signal_quality ); mavlink_msg_distance_sensor_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("DISTANCE_SENSOR") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_DISTANCE_SENSOR) != NULL); +#endif } static void mavlink_test_terrain_request(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -6435,6 +6861,11 @@ static void mavlink_test_terrain_request(uint8_t system_id, uint8_t component_id mavlink_msg_terrain_request_send(MAVLINK_COMM_1 , packet1.lat , packet1.lon , packet1.grid_spacing , packet1.mask ); mavlink_msg_terrain_request_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("TERRAIN_REQUEST") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_TERRAIN_REQUEST) != NULL); +#endif } static void mavlink_test_terrain_data(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -6493,6 +6924,11 @@ static void mavlink_test_terrain_data(uint8_t system_id, uint8_t component_id, m mavlink_msg_terrain_data_send(MAVLINK_COMM_1 , packet1.lat , packet1.lon , packet1.grid_spacing , packet1.gridbit , packet1.data ); mavlink_msg_terrain_data_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("TERRAIN_DATA") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_TERRAIN_DATA) != NULL); +#endif } static void mavlink_test_terrain_check(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -6548,6 +6984,11 @@ static void mavlink_test_terrain_check(uint8_t system_id, uint8_t component_id, mavlink_msg_terrain_check_send(MAVLINK_COMM_1 , packet1.lat , packet1.lon ); mavlink_msg_terrain_check_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("TERRAIN_CHECK") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_TERRAIN_CHECK) != NULL); +#endif } static void mavlink_test_terrain_report(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -6608,6 +7049,11 @@ static void mavlink_test_terrain_report(uint8_t system_id, uint8_t component_id, mavlink_msg_terrain_report_send(MAVLINK_COMM_1 , packet1.lat , packet1.lon , packet1.spacing , packet1.terrain_height , packet1.current_height , packet1.pending , packet1.loaded ); mavlink_msg_terrain_report_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("TERRAIN_REPORT") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_TERRAIN_REPORT) != NULL); +#endif } static void mavlink_test_scaled_pressure2(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -6666,6 +7112,11 @@ static void mavlink_test_scaled_pressure2(uint8_t system_id, uint8_t component_i mavlink_msg_scaled_pressure2_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature , packet1.temperature_press_diff ); mavlink_msg_scaled_pressure2_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("SCALED_PRESSURE2") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_SCALED_PRESSURE2) != NULL); +#endif } static void mavlink_test_att_pos_mocap(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -6725,6 +7176,11 @@ static void mavlink_test_att_pos_mocap(uint8_t system_id, uint8_t component_id, mavlink_msg_att_pos_mocap_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.q , packet1.x , packet1.y , packet1.z , packet1.covariance ); mavlink_msg_att_pos_mocap_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("ATT_POS_MOCAP") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_ATT_POS_MOCAP) != NULL); +#endif } static void mavlink_test_set_actuator_control_target(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -6783,6 +7239,11 @@ static void mavlink_test_set_actuator_control_target(uint8_t system_id, uint8_t mavlink_msg_set_actuator_control_target_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.group_mlx , packet1.target_system , packet1.target_component , packet1.controls ); mavlink_msg_set_actuator_control_target_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("SET_ACTUATOR_CONTROL_TARGET") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET) != NULL); +#endif } static void mavlink_test_actuator_control_target(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -6839,6 +7300,11 @@ static void mavlink_test_actuator_control_target(uint8_t system_id, uint8_t comp mavlink_msg_actuator_control_target_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.group_mlx , packet1.controls ); mavlink_msg_actuator_control_target_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("ACTUATOR_CONTROL_TARGET") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET) != NULL); +#endif } static void mavlink_test_altitude(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -6899,6 +7365,11 @@ static void mavlink_test_altitude(uint8_t system_id, uint8_t component_id, mavli mavlink_msg_altitude_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.altitude_monotonic , packet1.altitude_amsl , packet1.altitude_local , packet1.altitude_relative , packet1.altitude_terrain , packet1.bottom_clearance ); mavlink_msg_altitude_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("ALTITUDE") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_ALTITUDE) != NULL); +#endif } static void mavlink_test_resource_request(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -6957,6 +7428,11 @@ static void mavlink_test_resource_request(uint8_t system_id, uint8_t component_i mavlink_msg_resource_request_send(MAVLINK_COMM_1 , packet1.request_id , packet1.uri_type , packet1.uri , packet1.transfer_type , packet1.storage ); mavlink_msg_resource_request_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("RESOURCE_REQUEST") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_RESOURCE_REQUEST) != NULL); +#endif } static void mavlink_test_scaled_pressure3(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -7015,6 +7491,11 @@ static void mavlink_test_scaled_pressure3(uint8_t system_id, uint8_t component_i mavlink_msg_scaled_pressure3_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature , packet1.temperature_press_diff ); mavlink_msg_scaled_pressure3_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("SCALED_PRESSURE3") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_SCALED_PRESSURE3) != NULL); +#endif } static void mavlink_test_follow_target(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -7079,6 +7560,11 @@ static void mavlink_test_follow_target(uint8_t system_id, uint8_t component_id, mavlink_msg_follow_target_send(MAVLINK_COMM_1 , packet1.timestamp , packet1.est_capabilities , packet1.lat , packet1.lon , packet1.alt , packet1.vel , packet1.acc , packet1.attitude_q , packet1.rates , packet1.position_cov , packet1.custom_state ); mavlink_msg_follow_target_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("FOLLOW_TARGET") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_FOLLOW_TARGET) != NULL); +#endif } static void mavlink_test_control_system_state(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -7149,6 +7635,11 @@ static void mavlink_test_control_system_state(uint8_t system_id, uint8_t compone mavlink_msg_control_system_state_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.x_acc , packet1.y_acc , packet1.z_acc , packet1.x_vel , packet1.y_vel , packet1.z_vel , packet1.x_pos , packet1.y_pos , packet1.z_pos , packet1.airspeed , packet1.vel_variance , packet1.pos_variance , packet1.q , packet1.roll_rate , packet1.pitch_rate , packet1.yaw_rate ); mavlink_msg_control_system_state_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("CONTROL_SYSTEM_STATE") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_CONTROL_SYSTEM_STATE) != NULL); +#endif } static void mavlink_test_battery_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -7216,71 +7707,11 @@ static void mavlink_test_battery_status(uint8_t system_id, uint8_t component_id, mavlink_msg_battery_status_send(MAVLINK_COMM_1 , packet1.id , packet1.battery_function , packet1.type , packet1.temperature , packet1.voltages , packet1.current_battery , packet1.current_consumed , packet1.energy_consumed , packet1.battery_remaining , packet1.time_remaining , packet1.charge_state , packet1.voltages_ext , packet1.mode , packet1.fault_bitmask ); mavlink_msg_battery_status_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); -} -static void mavlink_test_autopilot_version(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ -#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 - mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0); - if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_AUTOPILOT_VERSION >= 256) { - return; - } -#endif - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_autopilot_version_t packet_in = { - 93372036854775807ULL,93372036854776311ULL,963498296,963498504,963498712,963498920,18899,19003,{ 113, 114, 115, 116, 117, 118, 119, 120 },{ 137, 138, 139, 140, 141, 142, 143, 144 },{ 161, 162, 163, 164, 165, 166, 167, 168 },{ 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202 } - }; - mavlink_autopilot_version_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.capabilities = packet_in.capabilities; - packet1.uid = packet_in.uid; - packet1.flight_sw_version = packet_in.flight_sw_version; - packet1.middleware_sw_version = packet_in.middleware_sw_version; - packet1.os_sw_version = packet_in.os_sw_version; - packet1.board_version = packet_in.board_version; - packet1.vendor_id = packet_in.vendor_id; - packet1.product_id = packet_in.product_id; - - mav_array_memcpy(packet1.flight_custom_version, packet_in.flight_custom_version, sizeof(uint8_t)*8); - mav_array_memcpy(packet1.middleware_custom_version, packet_in.middleware_custom_version, sizeof(uint8_t)*8); - mav_array_memcpy(packet1.os_custom_version, packet_in.os_custom_version, sizeof(uint8_t)*8); - mav_array_memcpy(packet1.uid2, packet_in.uid2, sizeof(uint8_t)*18); - -#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 - if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { - // cope with extensions - memset(MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN); - } +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("BATTERY_STATUS") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_BATTERY_STATUS) != NULL); #endif - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_autopilot_version_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_autopilot_version_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_autopilot_version_pack(system_id, component_id, &msg , packet1.capabilities , packet1.flight_sw_version , packet1.middleware_sw_version , packet1.os_sw_version , packet1.board_version , packet1.flight_custom_version , packet1.middleware_custom_version , packet1.os_custom_version , packet1.vendor_id , packet1.product_id , packet1.uid , packet1.uid2 ); - mavlink_msg_autopilot_version_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_autopilot_version_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.capabilities , packet1.flight_sw_version , packet1.middleware_sw_version , packet1.os_sw_version , packet1.board_version , packet1.flight_custom_version , packet1.middleware_custom_version , packet1.os_custom_version , packet1.vendor_id , packet1.product_id , packet1.uid , packet1.uid2 ); - mavlink_msg_autopilot_version_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_camera_thermal_range_t packet_in = { + 963497464,45.0,73.0,101.0,129.0,157.0,185.0,89,156 + }; + mavlink_camera_thermal_range_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.max = packet_in.max; + packet1.max_point_x = packet_in.max_point_x; + packet1.max_point_y = packet_in.max_point_y; + packet1.min = packet_in.min; + packet1.min_point_x = packet_in.min_point_x; + packet1.min_point_y = packet_in.min_point_y; + packet1.stream_id = packet_in.stream_id; + packet1.camera_device_id = packet_in.camera_device_id; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CAMERA_THERMAL_RANGE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_thermal_range_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_camera_thermal_range_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_thermal_range_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.stream_id , packet1.camera_device_id , packet1.max , packet1.max_point_x , packet1.max_point_y , packet1.min , packet1.min_point_x , packet1.min_point_y ); + mavlink_msg_camera_thermal_range_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_thermal_range_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.stream_id , packet1.camera_device_id , packet1.max , packet1.max_point_x , packet1.max_point_y , packet1.min , packet1.min_point_x , packet1.min_point_y ); + mavlink_msg_camera_thermal_range_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -10490,7 +11262,12 @@ static void mavlink_test_esc_info(uint8_t system_id, uint8_t component_id, mavli mavlink_msg_esc_info_send(MAVLINK_COMM_1 , packet1.index , packet1.time_usec , packet1.counter , packet1.count , packet1.connection_type , packet1.info , packet1.failure_flags , packet1.error_count , packet1.temperature ); mavlink_msg_esc_info_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); -} + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("ESC_INFO") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_ESC_INFO) != NULL); +#endif +} static void mavlink_test_esc_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { @@ -10548,6 +11325,11 @@ static void mavlink_test_esc_status(uint8_t system_id, uint8_t component_id, mav mavlink_msg_esc_status_send(MAVLINK_COMM_1 , packet1.index , packet1.time_usec , packet1.rpm , packet1.voltage , packet1.current ); mavlink_msg_esc_status_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("ESC_STATUS") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_ESC_STATUS) != NULL); +#endif } static void mavlink_test_wifi_config_ap(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -10605,6 +11387,11 @@ static void mavlink_test_wifi_config_ap(uint8_t system_id, uint8_t component_id, mavlink_msg_wifi_config_ap_send(MAVLINK_COMM_1 , packet1.ssid , packet1.password , packet1.mode , packet1.response ); mavlink_msg_wifi_config_ap_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("WIFI_CONFIG_AP") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_WIFI_CONFIG_AP) != NULL); +#endif } static void mavlink_test_ais_vessel(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -10675,6 +11462,11 @@ static void mavlink_test_ais_vessel(uint8_t system_id, uint8_t component_id, mav mavlink_msg_ais_vessel_send(MAVLINK_COMM_1 , packet1.MMSI , packet1.lat , packet1.lon , packet1.COG , packet1.heading , packet1.velocity , packet1.turn_rate , packet1.navigational_status , packet1.type , packet1.dimension_bow , packet1.dimension_stern , packet1.dimension_port , packet1.dimension_starboard , packet1.callsign , packet1.name , packet1.tslc , packet1.flags ); mavlink_msg_ais_vessel_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("AIS_VESSEL") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_AIS_VESSEL) != NULL); +#endif } static void mavlink_test_uavcan_node_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -10734,6 +11526,11 @@ static void mavlink_test_uavcan_node_status(uint8_t system_id, uint8_t component mavlink_msg_uavcan_node_status_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.uptime_sec , packet1.health , packet1.mode , packet1.sub_mode , packet1.vendor_specific_status_code ); mavlink_msg_uavcan_node_status_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("UAVCAN_NODE_STATUS") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_UAVCAN_NODE_STATUS) != NULL); +#endif } static void mavlink_test_uavcan_node_info(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -10796,6 +11593,11 @@ static void mavlink_test_uavcan_node_info(uint8_t system_id, uint8_t component_i mavlink_msg_uavcan_node_info_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.uptime_sec , packet1.name , packet1.hw_version_major , packet1.hw_version_minor , packet1.hw_unique_id , packet1.sw_version_major , packet1.sw_version_minor , packet1.sw_vcs_commit ); mavlink_msg_uavcan_node_info_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("UAVCAN_NODE_INFO") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_UAVCAN_NODE_INFO) != NULL); +#endif } static void mavlink_test_param_ext_request_read(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -10853,6 +11655,11 @@ static void mavlink_test_param_ext_request_read(uint8_t system_id, uint8_t compo mavlink_msg_param_ext_request_read_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_index ); mavlink_msg_param_ext_request_read_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("PARAM_EXT_REQUEST_READ") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ) != NULL); +#endif } static void mavlink_test_param_ext_request_list(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -10908,6 +11715,11 @@ static void mavlink_test_param_ext_request_list(uint8_t system_id, uint8_t compo mavlink_msg_param_ext_request_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component ); mavlink_msg_param_ext_request_list_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("PARAM_EXT_REQUEST_LIST") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST) != NULL); +#endif } static void mavlink_test_param_ext_value(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -10966,6 +11778,11 @@ static void mavlink_test_param_ext_value(uint8_t system_id, uint8_t component_id mavlink_msg_param_ext_value_send(MAVLINK_COMM_1 , packet1.param_id , packet1.param_value , packet1.param_type , packet1.param_count , packet1.param_index ); mavlink_msg_param_ext_value_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("PARAM_EXT_VALUE") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_PARAM_EXT_VALUE) != NULL); +#endif } static void mavlink_test_param_ext_set(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -11024,6 +11841,11 @@ static void mavlink_test_param_ext_set(uint8_t system_id, uint8_t component_id, mavlink_msg_param_ext_set_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_value , packet1.param_type ); mavlink_msg_param_ext_set_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("PARAM_EXT_SET") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_PARAM_EXT_SET) != NULL); +#endif } static void mavlink_test_param_ext_ack(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -11081,6 +11903,11 @@ static void mavlink_test_param_ext_ack(uint8_t system_id, uint8_t component_id, mavlink_msg_param_ext_ack_send(MAVLINK_COMM_1 , packet1.param_id , packet1.param_value , packet1.param_type , packet1.param_result ); mavlink_msg_param_ext_ack_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("PARAM_EXT_ACK") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_PARAM_EXT_ACK) != NULL); +#endif } static void mavlink_test_obstacle_distance(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -11143,6 +11970,11 @@ static void mavlink_test_obstacle_distance(uint8_t system_id, uint8_t component_ mavlink_msg_obstacle_distance_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.sensor_type , packet1.distances , packet1.increment , packet1.min_distance , packet1.max_distance , packet1.increment_f , packet1.angle_offset , packet1.frame ); mavlink_msg_obstacle_distance_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("OBSTACLE_DISTANCE") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_OBSTACLE_DISTANCE) != NULL); +#endif } static void mavlink_test_odometry(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -11157,7 +11989,7 @@ static void mavlink_test_odometry(uint8_t system_id, uint8_t component_id, mavli uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_odometry_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0,{ 157.0, 158.0, 159.0, 160.0 },269.0,297.0,325.0,353.0,381.0,409.0,{ 437.0, 438.0, 439.0, 440.0, 441.0, 442.0, 443.0, 444.0, 445.0, 446.0, 447.0, 448.0, 449.0, 450.0, 451.0, 452.0, 453.0, 454.0, 455.0, 456.0, 457.0 },{ 1025.0, 1026.0, 1027.0, 1028.0, 1029.0, 1030.0, 1031.0, 1032.0, 1033.0, 1034.0, 1035.0, 1036.0, 1037.0, 1038.0, 1039.0, 1040.0, 1041.0, 1042.0, 1043.0, 1044.0, 1045.0 },177,244,55,122 + 93372036854775807ULL,73.0,101.0,129.0,{ 157.0, 158.0, 159.0, 160.0 },269.0,297.0,325.0,353.0,381.0,409.0,{ 437.0, 438.0, 439.0, 440.0, 441.0, 442.0, 443.0, 444.0, 445.0, 446.0, 447.0, 448.0, 449.0, 450.0, 451.0, 452.0, 453.0, 454.0, 455.0, 456.0, 457.0 },{ 1025.0, 1026.0, 1027.0, 1028.0, 1029.0, 1030.0, 1031.0, 1032.0, 1033.0, 1034.0, 1035.0, 1036.0, 1037.0, 1038.0, 1039.0, 1040.0, 1041.0, 1042.0, 1043.0, 1044.0, 1045.0 },177,244,55,122,189 }; mavlink_odometry_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -11175,6 +12007,7 @@ static void mavlink_test_odometry(uint8_t system_id, uint8_t component_id, mavli packet1.child_frame_id = packet_in.child_frame_id; packet1.reset_counter = packet_in.reset_counter; packet1.estimator_type = packet_in.estimator_type; + packet1.quality = packet_in.quality; mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); mav_array_memcpy(packet1.pose_covariance, packet_in.pose_covariance, sizeof(float)*21); @@ -11192,12 +12025,12 @@ static void mavlink_test_odometry(uint8_t system_id, uint8_t component_id, mavli MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_odometry_pack(system_id, component_id, &msg , packet1.time_usec , packet1.frame_id , packet1.child_frame_id , packet1.x , packet1.y , packet1.z , packet1.q , packet1.vx , packet1.vy , packet1.vz , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.pose_covariance , packet1.velocity_covariance , packet1.reset_counter , packet1.estimator_type ); + mavlink_msg_odometry_pack(system_id, component_id, &msg , packet1.time_usec , packet1.frame_id , packet1.child_frame_id , packet1.x , packet1.y , packet1.z , packet1.q , packet1.vx , packet1.vy , packet1.vz , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.pose_covariance , packet1.velocity_covariance , packet1.reset_counter , packet1.estimator_type , packet1.quality ); mavlink_msg_odometry_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_odometry_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.frame_id , packet1.child_frame_id , packet1.x , packet1.y , packet1.z , packet1.q , packet1.vx , packet1.vy , packet1.vz , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.pose_covariance , packet1.velocity_covariance , packet1.reset_counter , packet1.estimator_type ); + mavlink_msg_odometry_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.frame_id , packet1.child_frame_id , packet1.x , packet1.y , packet1.z , packet1.q , packet1.vx , packet1.vy , packet1.vz , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.pose_covariance , packet1.velocity_covariance , packet1.reset_counter , packet1.estimator_type , packet1.quality ); mavlink_msg_odometry_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -11210,9 +12043,14 @@ static void mavlink_test_odometry(uint8_t system_id, uint8_t component_id, mavli MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_odometry_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.frame_id , packet1.child_frame_id , packet1.x , packet1.y , packet1.z , packet1.q , packet1.vx , packet1.vy , packet1.vz , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.pose_covariance , packet1.velocity_covariance , packet1.reset_counter , packet1.estimator_type ); + mavlink_msg_odometry_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.frame_id , packet1.child_frame_id , packet1.x , packet1.y , packet1.z , packet1.q , packet1.vx , packet1.vy , packet1.vz , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.pose_covariance , packet1.velocity_covariance , packet1.reset_counter , packet1.estimator_type , packet1.quality ); mavlink_msg_odometry_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("ODOMETRY") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_ODOMETRY) != NULL); +#endif } static void mavlink_test_trajectory_representation_waypoints(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -11280,6 +12118,11 @@ static void mavlink_test_trajectory_representation_waypoints(uint8_t system_id, mavlink_msg_trajectory_representation_waypoints_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.valid_points , packet1.pos_x , packet1.pos_y , packet1.pos_z , packet1.vel_x , packet1.vel_y , packet1.vel_z , packet1.acc_x , packet1.acc_y , packet1.acc_z , packet1.pos_yaw , packet1.vel_yaw , packet1.command ); mavlink_msg_trajectory_representation_waypoints_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("TRAJECTORY_REPRESENTATION_WAYPOINTS") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS) != NULL); +#endif } static void mavlink_test_trajectory_representation_bezier(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -11340,6 +12183,11 @@ static void mavlink_test_trajectory_representation_bezier(uint8_t system_id, uin mavlink_msg_trajectory_representation_bezier_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.valid_points , packet1.pos_x , packet1.pos_y , packet1.pos_z , packet1.delta , packet1.pos_yaw ); mavlink_msg_trajectory_representation_bezier_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("TRAJECTORY_REPRESENTATION_BEZIER") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER) != NULL); +#endif } static void mavlink_test_cellular_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -11400,6 +12248,11 @@ static void mavlink_test_cellular_status(uint8_t system_id, uint8_t component_id mavlink_msg_cellular_status_send(MAVLINK_COMM_1 , packet1.status , packet1.failure_reason , packet1.type , packet1.quality , packet1.mcc , packet1.mnc , packet1.lac ); mavlink_msg_cellular_status_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("CELLULAR_STATUS") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_CELLULAR_STATUS) != NULL); +#endif } static void mavlink_test_isbd_link_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -11461,6 +12314,11 @@ static void mavlink_test_isbd_link_status(uint8_t system_id, uint8_t component_i mavlink_msg_isbd_link_status_send(MAVLINK_COMM_1 , packet1.timestamp , packet1.last_heartbeat , packet1.failed_sessions , packet1.successful_sessions , packet1.signal_quality , packet1.ring_pending , packet1.tx_session_pending , packet1.rx_session_pending ); mavlink_msg_isbd_link_status_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("ISBD_LINK_STATUS") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_ISBD_LINK_STATUS) != NULL); +#endif } static void mavlink_test_cellular_config(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -11522,6 +12380,11 @@ static void mavlink_test_cellular_config(uint8_t system_id, uint8_t component_id mavlink_msg_cellular_config_send(MAVLINK_COMM_1 , packet1.enable_lte , packet1.enable_pin , packet1.pin , packet1.new_pin , packet1.apn , packet1.puk , packet1.roaming , packet1.response ); mavlink_msg_cellular_config_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("CELLULAR_CONFIG") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_CELLULAR_CONFIG) != NULL); +#endif } static void mavlink_test_raw_rpm(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -11577,6 +12440,11 @@ static void mavlink_test_raw_rpm(uint8_t system_id, uint8_t component_id, mavlin mavlink_msg_raw_rpm_send(MAVLINK_COMM_1 , packet1.index , packet1.frequency ); mavlink_msg_raw_rpm_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("RAW_RPM") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_RAW_RPM) != NULL); +#endif } static void mavlink_test_utm_global_position(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -11648,6 +12516,11 @@ static void mavlink_test_utm_global_position(uint8_t system_id, uint8_t componen mavlink_msg_utm_global_position_send(MAVLINK_COMM_1 , packet1.time , packet1.uas_id , packet1.lat , packet1.lon , packet1.alt , packet1.relative_alt , packet1.vx , packet1.vy , packet1.vz , packet1.h_acc , packet1.v_acc , packet1.vel_acc , packet1.next_lat , packet1.next_lon , packet1.next_alt , packet1.update_rate , packet1.flight_state , packet1.flags ); mavlink_msg_utm_global_position_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("UTM_GLOBAL_POSITION") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_UTM_GLOBAL_POSITION) != NULL); +#endif } static void mavlink_test_debug_float_array(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -11705,6 +12578,11 @@ static void mavlink_test_debug_float_array(uint8_t system_id, uint8_t component_ mavlink_msg_debug_float_array_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.name , packet1.array_id , packet1.data ); mavlink_msg_debug_float_array_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("DEBUG_FLOAT_ARRAY") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY) != NULL); +#endif } static void mavlink_test_orbit_execution_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -11764,6 +12642,11 @@ static void mavlink_test_orbit_execution_status(uint8_t system_id, uint8_t compo mavlink_msg_orbit_execution_status_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.radius , packet1.frame , packet1.x , packet1.y , packet1.z ); mavlink_msg_orbit_execution_status_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("ORBIT_EXECUTION_STATUS") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS) != NULL); +#endif } static void mavlink_test_smart_battery_info(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -11778,7 +12661,7 @@ static void mavlink_test_smart_battery_info(uint8_t system_id, uint8_t component uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_smart_battery_info_t packet_in = { - 963497464,963497672,17651,17755,17859,17963,18067,187,254,65,"VWXYZABCDEFGHIJ","LMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGH" + 963497464,963497672,17651,17755,17859,17963,18067,187,254,65,"VWXYZABCDEFGHIJ","LMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGH",21759,80,963502144,963502352,"UVWXYZABCD" }; mavlink_smart_battery_info_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -11792,9 +12675,14 @@ static void mavlink_test_smart_battery_info(uint8_t system_id, uint8_t component packet1.id = packet_in.id; packet1.battery_function = packet_in.battery_function; packet1.type = packet_in.type; + packet1.charging_maximum_voltage = packet_in.charging_maximum_voltage; + packet1.cells_in_series = packet_in.cells_in_series; + packet1.discharge_maximum_current = packet_in.discharge_maximum_current; + packet1.discharge_maximum_burst_current = packet_in.discharge_maximum_burst_current; mav_array_memcpy(packet1.serial_number, packet_in.serial_number, sizeof(char)*16); mav_array_memcpy(packet1.device_name, packet_in.device_name, sizeof(char)*50); + mav_array_memcpy(packet1.manufacture_date, packet_in.manufacture_date, sizeof(char)*11); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -11808,12 +12696,12 @@ static void mavlink_test_smart_battery_info(uint8_t system_id, uint8_t component MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_smart_battery_info_pack(system_id, component_id, &msg , packet1.id , packet1.battery_function , packet1.type , packet1.capacity_full_specification , packet1.capacity_full , packet1.cycle_count , packet1.serial_number , packet1.device_name , packet1.weight , packet1.discharge_minimum_voltage , packet1.charging_minimum_voltage , packet1.resting_minimum_voltage ); + mavlink_msg_smart_battery_info_pack(system_id, component_id, &msg , packet1.id , packet1.battery_function , packet1.type , packet1.capacity_full_specification , packet1.capacity_full , packet1.cycle_count , packet1.serial_number , packet1.device_name , packet1.weight , packet1.discharge_minimum_voltage , packet1.charging_minimum_voltage , packet1.resting_minimum_voltage , packet1.charging_maximum_voltage , packet1.cells_in_series , packet1.discharge_maximum_current , packet1.discharge_maximum_burst_current , packet1.manufacture_date ); mavlink_msg_smart_battery_info_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_smart_battery_info_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.id , packet1.battery_function , packet1.type , packet1.capacity_full_specification , packet1.capacity_full , packet1.cycle_count , packet1.serial_number , packet1.device_name , packet1.weight , packet1.discharge_minimum_voltage , packet1.charging_minimum_voltage , packet1.resting_minimum_voltage ); + mavlink_msg_smart_battery_info_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.id , packet1.battery_function , packet1.type , packet1.capacity_full_specification , packet1.capacity_full , packet1.cycle_count , packet1.serial_number , packet1.device_name , packet1.weight , packet1.discharge_minimum_voltage , packet1.charging_minimum_voltage , packet1.resting_minimum_voltage , packet1.charging_maximum_voltage , packet1.cells_in_series , packet1.discharge_maximum_current , packet1.discharge_maximum_burst_current , packet1.manufacture_date ); mavlink_msg_smart_battery_info_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -11826,9 +12714,158 @@ static void mavlink_test_smart_battery_info(uint8_t system_id, uint8_t component MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_smart_battery_info_send(MAVLINK_COMM_1 , packet1.id , packet1.battery_function , packet1.type , packet1.capacity_full_specification , packet1.capacity_full , packet1.cycle_count , packet1.serial_number , packet1.device_name , packet1.weight , packet1.discharge_minimum_voltage , packet1.charging_minimum_voltage , packet1.resting_minimum_voltage ); + mavlink_msg_smart_battery_info_send(MAVLINK_COMM_1 , packet1.id , packet1.battery_function , packet1.type , packet1.capacity_full_specification , packet1.capacity_full , packet1.cycle_count , packet1.serial_number , packet1.device_name , packet1.weight , packet1.discharge_minimum_voltage , packet1.charging_minimum_voltage , packet1.resting_minimum_voltage , packet1.charging_maximum_voltage , packet1.cells_in_series , packet1.discharge_maximum_current , packet1.discharge_maximum_burst_current , packet1.manufacture_date ); mavlink_msg_smart_battery_info_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("SMART_BATTERY_INFO") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_SMART_BATTERY_INFO) != NULL); +#endif +} + +static void mavlink_test_fuel_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0); + if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_FUEL_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_fuel_status_t packet_in = { + 17.0,45.0,73.0,101.0,129.0,963498504,77,144 + }; + mavlink_fuel_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.maximum_fuel = packet_in.maximum_fuel; + packet1.consumed_fuel = packet_in.consumed_fuel; + packet1.remaining_fuel = packet_in.remaining_fuel; + packet1.flow_rate = packet_in.flow_rate; + packet1.temperature = packet_in.temperature; + packet1.fuel_type = packet_in.fuel_type; + packet1.id = packet_in.id; + packet1.percent_remaining = packet_in.percent_remaining; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_FUEL_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_FUEL_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_fuel_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_fuel_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_fuel_status_pack(system_id, component_id, &msg , packet1.id , packet1.maximum_fuel , packet1.consumed_fuel , packet1.remaining_fuel , packet1.percent_remaining , packet1.flow_rate , packet1.temperature , packet1.fuel_type ); + mavlink_msg_fuel_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_fuel_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.id , packet1.maximum_fuel , packet1.consumed_fuel , packet1.remaining_fuel , packet1.percent_remaining , packet1.flow_rate , packet1.temperature , packet1.fuel_type ); + mavlink_msg_fuel_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_BATTERY_INFO >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_battery_info_t packet_in = { + 17.0,45.0,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,19315,19419,137,204,15,82,149,"XYZABCDE","GHIJKLMNOPQRSTUVWXYZABCDEFGHIJK","MNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHI" + }; + mavlink_battery_info_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.discharge_minimum_voltage = packet_in.discharge_minimum_voltage; + packet1.charging_minimum_voltage = packet_in.charging_minimum_voltage; + packet1.resting_minimum_voltage = packet_in.resting_minimum_voltage; + packet1.charging_maximum_voltage = packet_in.charging_maximum_voltage; + packet1.charging_maximum_current = packet_in.charging_maximum_current; + packet1.nominal_voltage = packet_in.nominal_voltage; + packet1.discharge_maximum_current = packet_in.discharge_maximum_current; + packet1.discharge_maximum_burst_current = packet_in.discharge_maximum_burst_current; + packet1.design_capacity = packet_in.design_capacity; + packet1.full_charge_capacity = packet_in.full_charge_capacity; + packet1.cycle_count = packet_in.cycle_count; + packet1.weight = packet_in.weight; + packet1.id = packet_in.id; + packet1.battery_function = packet_in.battery_function; + packet1.type = packet_in.type; + packet1.state_of_health = packet_in.state_of_health; + packet1.cells_in_series = packet_in.cells_in_series; + + mav_array_memcpy(packet1.manufacture_date, packet_in.manufacture_date, sizeof(char)*9); + mav_array_memcpy(packet1.serial_number, packet_in.serial_number, sizeof(char)*32); + mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*50); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_BATTERY_INFO_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_BATTERY_INFO_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_battery_info_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_battery_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_battery_info_pack(system_id, component_id, &msg , packet1.id , packet1.battery_function , packet1.type , packet1.state_of_health , packet1.cells_in_series , packet1.cycle_count , packet1.weight , packet1.discharge_minimum_voltage , packet1.charging_minimum_voltage , packet1.resting_minimum_voltage , packet1.charging_maximum_voltage , packet1.charging_maximum_current , packet1.nominal_voltage , packet1.discharge_maximum_current , packet1.discharge_maximum_burst_current , packet1.design_capacity , packet1.full_charge_capacity , packet1.manufacture_date , packet1.serial_number , packet1.name ); + mavlink_msg_battery_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_battery_info_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.id , packet1.battery_function , packet1.type , packet1.state_of_health , packet1.cells_in_series , packet1.cycle_count , packet1.weight , packet1.discharge_minimum_voltage , packet1.charging_minimum_voltage , packet1.resting_minimum_voltage , packet1.charging_maximum_voltage , packet1.charging_maximum_current , packet1.nominal_voltage , packet1.discharge_maximum_current , packet1.discharge_maximum_burst_current , packet1.design_capacity , packet1.full_charge_capacity , packet1.manufacture_date , packet1.serial_number , packet1.name ); + mavlink_msg_battery_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CAN_FRAME >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_can_frame_t packet_in = { + 963497464,17,84,151,218,{ 29, 30, 31, 32, 33, 34, 35, 36 } + }; + mavlink_can_frame_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.id = packet_in.id; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.bus = packet_in.bus; + packet1.len = packet_in.len; + + mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*8); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CAN_FRAME_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CAN_FRAME_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_can_frame_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_can_frame_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_can_frame_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.bus , packet1.len , packet1.id , packet1.data ); + mavlink_msg_can_frame_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_can_frame_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.bus , packet1.len , packet1.id , packet1.data ); + mavlink_msg_can_frame_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -12176,12 +13301,12 @@ static void mavlink_test_component_information(uint8_t system_id, uint8_t compon MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_component_information_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.metadata_type , packet1.metadata_uid , packet1.metadata_uri , packet1.translation_uid , packet1.translation_uri ); + mavlink_msg_component_information_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.general_metadata_file_crc , packet1.general_metadata_uri , packet1.peripherals_metadata_file_crc , packet1.peripherals_metadata_uri ); mavlink_msg_component_information_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_component_information_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.metadata_type , packet1.metadata_uid , packet1.metadata_uri , packet1.translation_uid , packet1.translation_uri ); + mavlink_msg_component_information_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.general_metadata_file_crc , packet1.general_metadata_uri , packet1.peripherals_metadata_file_crc , packet1.peripherals_metadata_uri ); mavlink_msg_component_information_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -12194,52 +13319,61 @@ static void mavlink_test_component_information(uint8_t system_id, uint8_t compon MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_component_information_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.metadata_type , packet1.metadata_uid , packet1.metadata_uri , packet1.translation_uid , packet1.translation_uri ); + mavlink_msg_component_information_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.general_metadata_file_crc , packet1.general_metadata_uri , packet1.peripherals_metadata_file_crc , packet1.peripherals_metadata_uri ); mavlink_msg_component_information_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("COMPONENT_INFORMATION") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_COMPONENT_INFORMATION) != NULL); +#endif } -static void mavlink_test_play_tune_v2(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +static void mavlink_test_component_information_basic(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0); - if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PLAY_TUNE_V2 >= 256) { + if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC >= 256) { return; } #endif mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_play_tune_v2_t packet_in = { - 963497464,17,84,"GHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRS" + mavlink_component_information_basic_t packet_in = { + 93372036854775807ULL,963497880,963498088,"QRSTUVWXYZABCDEFGHIJKLMNOPQRSTU","WXYZABCDEFGHIJKLMNOPQRSTUVWXYZA","CDEFGHIJKLMNOPQRSTUVWXY","ABCDEFGHIJKLMNOPQRSTUVW","YZABCDEFGHIJKLMNOPQRSTUVWXYZABC" }; - mavlink_play_tune_v2_t packet1, packet2; + mavlink_component_information_basic_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.format = packet_in.format; - packet1.target_system = packet_in.target_system; - packet1.target_component = packet_in.target_component; + packet1.capabilities = packet_in.capabilities; + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.time_manufacture_s = packet_in.time_manufacture_s; - mav_array_memcpy(packet1.tune, packet_in.tune, sizeof(char)*248); + mav_array_memcpy(packet1.vendor_name, packet_in.vendor_name, sizeof(char)*32); + mav_array_memcpy(packet1.model_name, packet_in.model_name, sizeof(char)*32); + mav_array_memcpy(packet1.software_version, packet_in.software_version, sizeof(char)*24); + mav_array_memcpy(packet1.hardware_version, packet_in.hardware_version, sizeof(char)*24); + mav_array_memcpy(packet1.serial_number, packet_in.serial_number, sizeof(char)*32); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { // cope with extensions - memset(MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN); + memset(MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_COMPONENT_INFORMATION_BASIC_MIN_LEN); } #endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_play_tune_v2_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_play_tune_v2_decode(&msg, &packet2); + mavlink_msg_component_information_basic_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_component_information_basic_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_play_tune_v2_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.format , packet1.tune ); - mavlink_msg_play_tune_v2_decode(&msg, &packet2); + mavlink_msg_component_information_basic_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.capabilities , packet1.time_manufacture_s , packet1.vendor_name , packet1.model_name , packet1.software_version , packet1.hardware_version , packet1.serial_number ); + mavlink_msg_component_information_basic_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_play_tune_v2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.format , packet1.tune ); - mavlink_msg_play_tune_v2_decode(&msg, &packet2); + mavlink_msg_component_information_basic_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.capabilities , packet1.time_manufacture_s , packet1.vendor_name , packet1.model_name , packet1.software_version , packet1.hardware_version , packet1.serial_number ); + mavlink_msg_component_information_basic_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); @@ -12247,55 +13381,60 @@ static void mavlink_test_play_tune_v2(uint8_t system_id, uint8_t component_id, m for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_SUPPORTED_TUNES >= 256) { + if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_COMPONENT_METADATA >= 256) { return; } #endif mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_supported_tunes_t packet_in = { - 963497464,17,84 + mavlink_component_metadata_t packet_in = { + 963497464,963497672,"IJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABC" }; - mavlink_supported_tunes_t packet1, packet2; + mavlink_component_metadata_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.format = packet_in.format; - packet1.target_system = packet_in.target_system; - packet1.target_component = packet_in.target_component; + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.file_crc = packet_in.file_crc; + mav_array_memcpy(packet1.uri, packet_in.uri, sizeof(char)*100); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { // cope with extensions - memset(MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN); + memset(MAVLINK_MSG_ID_COMPONENT_METADATA_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_COMPONENT_METADATA_MIN_LEN); } #endif memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_supported_tunes_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_supported_tunes_decode(&msg, &packet2); + mavlink_msg_component_metadata_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_component_metadata_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_supported_tunes_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.format ); - mavlink_msg_supported_tunes_decode(&msg, &packet2); + mavlink_msg_component_metadata_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.file_crc , packet1.uri ); + mavlink_msg_component_metadata_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_supported_tunes_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.format ); - mavlink_msg_supported_tunes_decode(&msg, &packet2); + mavlink_msg_component_metadata_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.file_crc , packet1.uri ); + mavlink_msg_component_metadata_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); @@ -12303,35 +13442,794 @@ static void mavlink_test_supported_tunes(uint8_t system_id, uint8_t component_id for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_WHEEL_DISTANCE >= 256) { + if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PLAY_TUNE_V2 >= 256) { return; } #endif mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_wheel_distance_t packet_in = { - 93372036854775807ULL,{ 179.0, 180.0, 181.0, 182.0, 183.0, 184.0, 185.0, 186.0, 187.0, 188.0, 189.0, 190.0, 191.0, 192.0, 193.0, 194.0 },157 + mavlink_play_tune_v2_t packet_in = { + 963497464,17,84,"GHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRS" }; - mavlink_wheel_distance_t packet1, packet2; + mavlink_play_tune_v2_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); - packet1.time_usec = packet_in.time_usec; - packet1.count = packet_in.count; + packet1.format = packet_in.format; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; - mav_array_memcpy(packet1.distance, packet_in.distance, sizeof(double)*16); + mav_array_memcpy(packet1.tune, packet_in.tune, sizeof(char)*248); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_play_tune_v2_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_play_tune_v2_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_play_tune_v2_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.format , packet1.tune ); + mavlink_msg_play_tune_v2_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_play_tune_v2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.format , packet1.tune ); + mavlink_msg_play_tune_v2_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_SUPPORTED_TUNES >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_supported_tunes_t packet_in = { + 963497464,17,84 + }; + mavlink_supported_tunes_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.format = packet_in.format; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_supported_tunes_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_supported_tunes_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_supported_tunes_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.format ); + mavlink_msg_supported_tunes_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_supported_tunes_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.format ); + mavlink_msg_supported_tunes_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_EVENT >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_event_t packet_in = { + 963497464,963497672,17651,163,230,41,{ 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147 } + }; + mavlink_event_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.id = packet_in.id; + packet1.event_time_boot_ms = packet_in.event_time_boot_ms; + packet1.sequence = packet_in.sequence; + packet1.destination_component = packet_in.destination_component; + packet1.destination_system = packet_in.destination_system; + packet1.log_levels = packet_in.log_levels; + + mav_array_memcpy(packet1.arguments, packet_in.arguments, sizeof(uint8_t)*40); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_EVENT_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_EVENT_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_event_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_event_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_event_pack(system_id, component_id, &msg , packet1.destination_component , packet1.destination_system , packet1.id , packet1.event_time_boot_ms , packet1.sequence , packet1.log_levels , packet1.arguments ); + mavlink_msg_event_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_event_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.destination_component , packet1.destination_system , packet1.id , packet1.event_time_boot_ms , packet1.sequence , packet1.log_levels , packet1.arguments ); + mavlink_msg_event_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_current_event_sequence_t packet_in = { + 17235,139 + }; + mavlink_current_event_sequence_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.sequence = packet_in.sequence; + packet1.flags = packet_in.flags; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_current_event_sequence_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_current_event_sequence_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_current_event_sequence_pack(system_id, component_id, &msg , packet1.sequence , packet1.flags ); + mavlink_msg_current_event_sequence_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_current_event_sequence_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.sequence , packet1.flags ); + mavlink_msg_current_event_sequence_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_REQUEST_EVENT >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_request_event_t packet_in = { + 17235,17339,17,84 + }; + mavlink_request_event_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.first_sequence = packet_in.first_sequence; + packet1.last_sequence = packet_in.last_sequence; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_REQUEST_EVENT_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_REQUEST_EVENT_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_request_event_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_request_event_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_request_event_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.first_sequence , packet1.last_sequence ); + mavlink_msg_request_event_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_request_event_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.first_sequence , packet1.last_sequence ); + mavlink_msg_request_event_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_response_event_error_t packet_in = { + 17235,17339,17,84,151 + }; + mavlink_response_event_error_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.sequence = packet_in.sequence; + packet1.sequence_oldest_available = packet_in.sequence_oldest_available; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.reason = packet_in.reason; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_response_event_error_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_response_event_error_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_response_event_error_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.sequence , packet1.sequence_oldest_available , packet1.reason ); + mavlink_msg_response_event_error_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_response_event_error_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.sequence , packet1.sequence_oldest_available , packet1.reason ); + mavlink_msg_response_event_error_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_AVAILABLE_MODES >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_available_modes_t packet_in = { + 963497464,963497672,29,96,163,"LMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRS" + }; + mavlink_available_modes_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.custom_mode = packet_in.custom_mode; + packet1.properties = packet_in.properties; + packet1.number_modes = packet_in.number_modes; + packet1.mode_index = packet_in.mode_index; + packet1.standard_mode = packet_in.standard_mode; + + mav_array_memcpy(packet1.mode_name, packet_in.mode_name, sizeof(char)*35); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_AVAILABLE_MODES_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_AVAILABLE_MODES_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_available_modes_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_available_modes_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_available_modes_pack(system_id, component_id, &msg , packet1.number_modes , packet1.mode_index , packet1.standard_mode , packet1.custom_mode , packet1.properties , packet1.mode_name ); + mavlink_msg_available_modes_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_available_modes_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.number_modes , packet1.mode_index , packet1.standard_mode , packet1.custom_mode , packet1.properties , packet1.mode_name ); + mavlink_msg_available_modes_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CURRENT_MODE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_current_mode_t packet_in = { + 963497464,963497672,29 + }; + mavlink_current_mode_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.custom_mode = packet_in.custom_mode; + packet1.intended_custom_mode = packet_in.intended_custom_mode; + packet1.standard_mode = packet_in.standard_mode; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CURRENT_MODE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CURRENT_MODE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_current_mode_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_current_mode_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_current_mode_pack(system_id, component_id, &msg , packet1.standard_mode , packet1.custom_mode , packet1.intended_custom_mode ); + mavlink_msg_current_mode_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_current_mode_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.standard_mode , packet1.custom_mode , packet1.intended_custom_mode ); + mavlink_msg_current_mode_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_available_modes_monitor_t packet_in = { + 5 + }; + mavlink_available_modes_monitor_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.seq = packet_in.seq; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_available_modes_monitor_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_available_modes_monitor_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_available_modes_monitor_pack(system_id, component_id, &msg , packet1.seq ); + mavlink_msg_available_modes_monitor_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_available_modes_monitor_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.seq ); + mavlink_msg_available_modes_monitor_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ILLUMINATOR_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_illuminator_status_t packet_in = { + 963497464,963497672,73.0,101.0,129.0,157.0,185.0,213.0,101,168,235 + }; + mavlink_illuminator_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.uptime_ms = packet_in.uptime_ms; + packet1.error_status = packet_in.error_status; + packet1.brightness = packet_in.brightness; + packet1.strobe_period = packet_in.strobe_period; + packet1.strobe_duty_cycle = packet_in.strobe_duty_cycle; + packet1.temp_c = packet_in.temp_c; + packet1.min_strobe_period = packet_in.min_strobe_period; + packet1.max_strobe_period = packet_in.max_strobe_period; + packet1.enable = packet_in.enable; + packet1.mode_bitmask = packet_in.mode_bitmask; + packet1.mode = packet_in.mode; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ILLUMINATOR_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ILLUMINATOR_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_illuminator_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_illuminator_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_illuminator_status_pack(system_id, component_id, &msg , packet1.uptime_ms , packet1.enable , packet1.mode_bitmask , packet1.error_status , packet1.mode , packet1.brightness , packet1.strobe_period , packet1.strobe_duty_cycle , packet1.temp_c , packet1.min_strobe_period , packet1.max_strobe_period ); + mavlink_msg_illuminator_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_illuminator_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.uptime_ms , packet1.enable , packet1.mode_bitmask , packet1.error_status , packet1.mode , packet1.brightness , packet1.strobe_period , packet1.strobe_duty_cycle , packet1.temp_c , packet1.min_strobe_period , packet1.max_strobe_period ); + mavlink_msg_illuminator_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CANFD_FRAME >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_canfd_frame_t packet_in = { + 963497464,17,84,151,218,{ 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92 } + }; + mavlink_canfd_frame_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.id = packet_in.id; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.bus = packet_in.bus; + packet1.len = packet_in.len; + + mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*64); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CANFD_FRAME_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CANFD_FRAME_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_canfd_frame_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_canfd_frame_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_canfd_frame_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.bus , packet1.len , packet1.id , packet1.data ); + mavlink_msg_canfd_frame_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_canfd_frame_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.bus , packet1.len , packet1.id , packet1.data ); + mavlink_msg_canfd_frame_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CAN_FILTER_MODIFY >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_can_filter_modify_t packet_in = { + { 17235, 17236, 17237, 17238, 17239, 17240, 17241, 17242, 17243, 17244, 17245, 17246, 17247, 17248, 17249, 17250 },101,168,235,46,113 + }; + mavlink_can_filter_modify_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.bus = packet_in.bus; + packet1.operation = packet_in.operation; + packet1.num_ids = packet_in.num_ids; + + mav_array_memcpy(packet1.ids, packet_in.ids, sizeof(uint16_t)*16); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CAN_FILTER_MODIFY_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CAN_FILTER_MODIFY_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_can_filter_modify_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_can_filter_modify_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_can_filter_modify_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.bus , packet1.operation , packet1.num_ids , packet1.ids ); + mavlink_msg_can_filter_modify_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_can_filter_modify_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.bus , packet1.operation , packet1.num_ids , packet1.ids ); + mavlink_msg_can_filter_modify_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_WHEEL_DISTANCE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_wheel_distance_t packet_in = { + 93372036854775807ULL,{ 179.0, 180.0, 181.0, 182.0, 183.0, 184.0, 185.0, 186.0, 187.0, 188.0, 189.0, 190.0, 191.0, 192.0, 193.0, 194.0 },157 + }; + mavlink_wheel_distance_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.count = packet_in.count; + + mav_array_memcpy(packet1.distance, packet_in.distance, sizeof(double)*16); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -12366,6 +14264,11 @@ static void mavlink_test_wheel_distance(uint8_t system_id, uint8_t component_id, mavlink_msg_wheel_distance_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.count , packet1.distance ); mavlink_msg_wheel_distance_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("WHEEL_DISTANCE") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_WHEEL_DISTANCE) != NULL); +#endif } static void mavlink_test_winch_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -12427,6 +14330,11 @@ static void mavlink_test_winch_status(uint8_t system_id, uint8_t component_id, m mavlink_msg_winch_status_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.line_length , packet1.speed , packet1.tension , packet1.voltage , packet1.current , packet1.temperature , packet1.status ); mavlink_msg_winch_status_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("WINCH_STATUS") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_WINCH_STATUS) != NULL); +#endif } static void mavlink_test_open_drone_id_basic_id(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -12486,6 +14394,11 @@ static void mavlink_test_open_drone_id_basic_id(uint8_t system_id, uint8_t compo mavlink_msg_open_drone_id_basic_id_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.id_type , packet1.ua_type , packet1.uas_id ); mavlink_msg_open_drone_id_basic_id_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("OPEN_DRONE_ID_BASIC_ID") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID) != NULL); +#endif } static void mavlink_test_open_drone_id_location(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -12558,6 +14471,11 @@ static void mavlink_test_open_drone_id_location(uint8_t system_id, uint8_t compo mavlink_msg_open_drone_id_location_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.status , packet1.direction , packet1.speed_horizontal , packet1.speed_vertical , packet1.latitude , packet1.longitude , packet1.altitude_barometric , packet1.altitude_geodetic , packet1.height_reference , packet1.height , packet1.horizontal_accuracy , packet1.vertical_accuracy , packet1.barometer_accuracy , packet1.speed_accuracy , packet1.timestamp , packet1.timestamp_accuracy ); mavlink_msg_open_drone_id_location_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("OPEN_DRONE_ID_LOCATION") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION) != NULL); +#endif } static void mavlink_test_open_drone_id_authentication(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -12581,7 +14499,7 @@ static void mavlink_test_open_drone_id_authentication(uint8_t system_id, uint8_t packet1.target_component = packet_in.target_component; packet1.authentication_type = packet_in.authentication_type; packet1.data_page = packet_in.data_page; - packet1.page_count = packet_in.page_count; + packet1.last_page_index = packet_in.last_page_index; packet1.length = packet_in.length; mav_array_memcpy(packet1.id_or_mac, packet_in.id_or_mac, sizeof(uint8_t)*20); @@ -12599,12 +14517,12 @@ static void mavlink_test_open_drone_id_authentication(uint8_t system_id, uint8_t MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_open_drone_id_authentication_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.authentication_type , packet1.data_page , packet1.page_count , packet1.length , packet1.timestamp , packet1.authentication_data ); + mavlink_msg_open_drone_id_authentication_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.authentication_type , packet1.data_page , packet1.last_page_index , packet1.length , packet1.timestamp , packet1.authentication_data ); mavlink_msg_open_drone_id_authentication_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_open_drone_id_authentication_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.authentication_type , packet1.data_page , packet1.page_count , packet1.length , packet1.timestamp , packet1.authentication_data ); + mavlink_msg_open_drone_id_authentication_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.authentication_type , packet1.data_page , packet1.last_page_index , packet1.length , packet1.timestamp , packet1.authentication_data ); mavlink_msg_open_drone_id_authentication_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -12617,9 +14535,14 @@ static void mavlink_test_open_drone_id_authentication(uint8_t system_id, uint8_t MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_open_drone_id_authentication_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.authentication_type , packet1.data_page , packet1.page_count , packet1.length , packet1.timestamp , packet1.authentication_data ); + mavlink_msg_open_drone_id_authentication_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.authentication_type , packet1.data_page , packet1.last_page_index , packet1.length , packet1.timestamp , packet1.authentication_data ); mavlink_msg_open_drone_id_authentication_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("OPEN_DRONE_ID_AUTHENTICATION") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION) != NULL); +#endif } static void mavlink_test_open_drone_id_self_id(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -12678,6 +14601,11 @@ static void mavlink_test_open_drone_id_self_id(uint8_t system_id, uint8_t compon mavlink_msg_open_drone_id_self_id_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.description_type , packet1.description ); mavlink_msg_open_drone_id_self_id_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("OPEN_DRONE_ID_SELF_ID") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID) != NULL); +#endif } static void mavlink_test_open_drone_id_system(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -12692,7 +14620,7 @@ static void mavlink_test_open_drone_id_system(uint8_t system_id, uint8_t compone uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_open_drone_id_system_t packet_in = { - 963497464,963497672,73.0,101.0,18067,18171,65,132,{ 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218 },3,70,137,204 + 963497464,963497672,73.0,101.0,129.0,963498504,18483,18587,89,156,{ 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242 },27,94,161,228 }; mavlink_open_drone_id_system_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -12700,6 +14628,8 @@ static void mavlink_test_open_drone_id_system(uint8_t system_id, uint8_t compone packet1.operator_longitude = packet_in.operator_longitude; packet1.area_ceiling = packet_in.area_ceiling; packet1.area_floor = packet_in.area_floor; + packet1.operator_altitude_geo = packet_in.operator_altitude_geo; + packet1.timestamp = packet_in.timestamp; packet1.area_count = packet_in.area_count; packet1.area_radius = packet_in.area_radius; packet1.target_system = packet_in.target_system; @@ -12723,12 +14653,12 @@ static void mavlink_test_open_drone_id_system(uint8_t system_id, uint8_t compone MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_open_drone_id_system_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.operator_location_type , packet1.classification_type , packet1.operator_latitude , packet1.operator_longitude , packet1.area_count , packet1.area_radius , packet1.area_ceiling , packet1.area_floor , packet1.category_eu , packet1.class_eu ); + mavlink_msg_open_drone_id_system_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.operator_location_type , packet1.classification_type , packet1.operator_latitude , packet1.operator_longitude , packet1.area_count , packet1.area_radius , packet1.area_ceiling , packet1.area_floor , packet1.category_eu , packet1.class_eu , packet1.operator_altitude_geo , packet1.timestamp ); mavlink_msg_open_drone_id_system_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_open_drone_id_system_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.operator_location_type , packet1.classification_type , packet1.operator_latitude , packet1.operator_longitude , packet1.area_count , packet1.area_radius , packet1.area_ceiling , packet1.area_floor , packet1.category_eu , packet1.class_eu ); + mavlink_msg_open_drone_id_system_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.operator_location_type , packet1.classification_type , packet1.operator_latitude , packet1.operator_longitude , packet1.area_count , packet1.area_radius , packet1.area_ceiling , packet1.area_floor , packet1.category_eu , packet1.class_eu , packet1.operator_altitude_geo , packet1.timestamp ); mavlink_msg_open_drone_id_system_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -12741,9 +14671,14 @@ static void mavlink_test_open_drone_id_system(uint8_t system_id, uint8_t compone MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_open_drone_id_system_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.operator_location_type , packet1.classification_type , packet1.operator_latitude , packet1.operator_longitude , packet1.area_count , packet1.area_radius , packet1.area_ceiling , packet1.area_floor , packet1.category_eu , packet1.class_eu ); + mavlink_msg_open_drone_id_system_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.operator_location_type , packet1.classification_type , packet1.operator_latitude , packet1.operator_longitude , packet1.area_count , packet1.area_radius , packet1.area_ceiling , packet1.area_floor , packet1.category_eu , packet1.class_eu , packet1.operator_altitude_geo , packet1.timestamp ); mavlink_msg_open_drone_id_system_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("OPEN_DRONE_ID_SYSTEM") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM) != NULL); +#endif } static void mavlink_test_open_drone_id_operator_id(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -12802,6 +14737,11 @@ static void mavlink_test_open_drone_id_operator_id(uint8_t system_id, uint8_t co mavlink_msg_open_drone_id_operator_id_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.operator_id_type , packet1.operator_id ); mavlink_msg_open_drone_id_operator_id_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("OPEN_DRONE_ID_OPERATOR_ID") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID) != NULL); +#endif } static void mavlink_test_open_drone_id_message_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -12816,7 +14756,7 @@ static void mavlink_test_open_drone_id_message_pack(uint8_t system_id, uint8_t c uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_open_drone_id_message_pack_t packet_in = { - 5,72,139,206,{ 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 } + 5,72,{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158 },199,10,{ 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45 } }; mavlink_open_drone_id_message_pack_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -12825,7 +14765,8 @@ static void mavlink_test_open_drone_id_message_pack(uint8_t system_id, uint8_t c packet1.single_message_size = packet_in.single_message_size; packet1.msg_pack_size = packet_in.msg_pack_size; - mav_array_memcpy(packet1.messages, packet_in.messages, sizeof(uint8_t)*250); + mav_array_memcpy(packet1.id_or_mac, packet_in.id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet1.messages, packet_in.messages, sizeof(uint8_t)*225); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -12839,12 +14780,12 @@ static void mavlink_test_open_drone_id_message_pack(uint8_t system_id, uint8_t c MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_open_drone_id_message_pack_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.single_message_size , packet1.msg_pack_size , packet1.messages ); + mavlink_msg_open_drone_id_message_pack_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.single_message_size , packet1.msg_pack_size , packet1.messages ); mavlink_msg_open_drone_id_message_pack_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_open_drone_id_message_pack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.single_message_size , packet1.msg_pack_size , packet1.messages ); + mavlink_msg_open_drone_id_message_pack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.single_message_size , packet1.msg_pack_size , packet1.messages ); mavlink_msg_open_drone_id_message_pack_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -12857,9 +14798,199 @@ static void mavlink_test_open_drone_id_message_pack(uint8_t system_id, uint8_t c MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_open_drone_id_message_pack_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.single_message_size , packet1.msg_pack_size , packet1.messages ); + mavlink_msg_open_drone_id_message_pack_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.single_message_size , packet1.msg_pack_size , packet1.messages ); mavlink_msg_open_drone_id_message_pack_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("OPEN_DRONE_ID_MESSAGE_PACK") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK) != NULL); +#endif +} + +static void mavlink_test_open_drone_id_arm_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0); + if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_open_drone_id_arm_status_t packet_in = { + 5,"BCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWX" + }; + mavlink_open_drone_id_arm_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.status = packet_in.status; + + mav_array_memcpy(packet1.error, packet_in.error, sizeof(char)*50); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_arm_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_open_drone_id_arm_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_arm_status_pack(system_id, component_id, &msg , packet1.status , packet1.error ); + mavlink_msg_open_drone_id_arm_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_arm_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.status , packet1.error ); + mavlink_msg_open_drone_id_arm_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_open_drone_id_system_update_t packet_in = { + 963497464,963497672,73.0,963498088,53,120 + }; + mavlink_open_drone_id_system_update_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.operator_latitude = packet_in.operator_latitude; + packet1.operator_longitude = packet_in.operator_longitude; + packet1.operator_altitude_geo = packet_in.operator_altitude_geo; + packet1.timestamp = packet_in.timestamp; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_UPDATE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_system_update_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_open_drone_id_system_update_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_system_update_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.operator_latitude , packet1.operator_longitude , packet1.operator_altitude_geo , packet1.timestamp ); + mavlink_msg_open_drone_id_system_update_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_system_update_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.operator_latitude , packet1.operator_longitude , packet1.operator_altitude_geo , packet1.timestamp ); + mavlink_msg_open_drone_id_system_update_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_HYGROMETER_SENSOR >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_hygrometer_sensor_t packet_in = { + 17235,17339,17 + }; + mavlink_hygrometer_sensor_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.temperature = packet_in.temperature; + packet1.humidity = packet_in.humidity; + packet1.id = packet_in.id; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_HYGROMETER_SENSOR_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_HYGROMETER_SENSOR_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_hygrometer_sensor_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_hygrometer_sensor_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_hygrometer_sensor_pack(system_id, component_id, &msg , packet1.id , packet1.temperature , packet1.humidity ); + mavlink_msg_hygrometer_sensor_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_hygrometer_sensor_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.id , packet1.temperature , packet1.humidity ); + mavlink_msg_hygrometer_sensor_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i mavlink_message_info[mid].msgid) { - low = mid; - continue; - } - low = mid; - break; - } - if (mavlink_message_info[low].msgid == msgid) { - return &mavlink_message_info[low]; - } - return NULL; + const uint32_t count = sizeof(mavlink_message_info)/sizeof(mavlink_message_info[0]); + if (count == 0) { + return NULL; + } + uint32_t low=0, high=count-1; + while (low < high) { + uint32_t mid = (low+high)/2; + if (msgid < mavlink_message_info[mid].msgid) { + high = mid; + continue; + } + if (msgid > mavlink_message_info[mid].msgid) { + low = mid+1; + continue; + } + return &mavlink_message_info[mid]; + } + if (mavlink_message_info[low].msgid == msgid) { + return &mavlink_message_info[low]; + } + return NULL; } /* @@ -47,24 +50,33 @@ MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info(const mavl MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info_by_name(const char *name) { static const struct { const char *name; uint32_t msgid; } mavlink_message_names[] = MAVLINK_MESSAGE_NAMES; - /* + /* use a bisection search to find the right entry. A perfect hash may be better Note that this assumes the table is sorted with primary key name */ - uint32_t low=0, high=sizeof(mavlink_message_names)/sizeof(mavlink_message_names[0]); - while (low < high) { - uint32_t mid = (low+1+high)/2; - int cmp = strcmp(mavlink_message_names[mid].name, name); - if (cmp == 0) { - return mavlink_get_message_info_by_id(mavlink_message_names[mid].msgid); - } - if (cmp > 0) { - high = mid-1; - } else { - low = mid; - } - } - return NULL; + const uint32_t count = sizeof(mavlink_message_names)/sizeof(mavlink_message_names[0]); + if (count == 0) { + return NULL; + } + uint32_t low=0, high=count-1; + while (low < high) { + uint32_t mid = (low+high)/2; + int cmp = strcmp(mavlink_message_names[mid].name, name); + if (cmp > 0) { + high = mid; + continue; + } + if (cmp < 0) { + low = mid+1; + continue; + } + low = mid; + break; + } + if (strcmp(mavlink_message_names[low].name, name) == 0) { + return mavlink_get_message_info_by_id(mavlink_message_names[low].msgid); + } + return NULL; } #endif // MAVLINK_USE_MESSAGE_INFO diff --git a/lib/main/MAVLink/mavlink_helpers.h b/lib/main/MAVLink/mavlink_helpers.h index b99f41ccab4..4935ee04b56 100755 --- a/lib/main/MAVLink/mavlink_helpers.h +++ b/lib/main/MAVLink/mavlink_helpers.h @@ -66,6 +66,7 @@ MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan) status->parse_state = MAVLINK_PARSE_STATE_IDLE; } +#ifndef MAVLINK_NO_SIGN_PACKET /** * @brief create a signature block for a packet */ @@ -98,6 +99,7 @@ MAVLINK_HELPER uint8_t mavlink_sign_packet(mavlink_signing_t *signing, return MAVLINK_SIGNATURE_BLOCK_LEN; } +#endif /** * @brief Trim payload of any trailing zero-populated bytes (MAVLink 2 only). @@ -114,6 +116,7 @@ MAVLINK_HELPER uint8_t _mav_trim_payload(const char *payload, uint8_t length) return length; } +#ifndef MAVLINK_NO_SIGNATURE_CHECK /** * @brief check a signature block for a packet */ @@ -133,11 +136,13 @@ MAVLINK_HELPER bool mavlink_signature_check(mavlink_signing_t *signing, mavlink_sha256_init(&ctx); mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key)); - mavlink_sha256_update(&ctx, p, MAVLINK_CORE_HEADER_LEN+1+msg->len); + mavlink_sha256_update(&ctx, p, MAVLINK_NUM_HEADER_BYTES); + mavlink_sha256_update(&ctx, _MAV_PAYLOAD(msg), msg->len); mavlink_sha256_update(&ctx, msg->ck, 2); mavlink_sha256_update(&ctx, psig, 1+6); mavlink_sha256_final_48(&ctx, signature); - if (memcmp(signature, incoming_signature, 6) != 0) { + if (memcmp(signature, incoming_signature, 6) != 0) { + signing->last_status = MAVLINK_SIGNING_STATUS_BAD_SIGNATURE; return false; } @@ -151,7 +156,8 @@ MAVLINK_HELPER bool mavlink_signature_check(mavlink_signing_t *signing, memcpy(tstamp.t8, psig+1, 6); if (signing_streams == NULL) { - return false; + signing->last_status = MAVLINK_SIGNING_STATUS_NO_STREAMS; + return false; } // find stream @@ -165,11 +171,13 @@ MAVLINK_HELPER bool mavlink_signature_check(mavlink_signing_t *signing, if (i == signing_streams->num_signing_streams) { if (signing_streams->num_signing_streams >= MAVLINK_MAX_SIGNING_STREAMS) { // over max number of streams - return false; + signing->last_status = MAVLINK_SIGNING_STATUS_TOO_MANY_STREAMS; + return false; } // new stream. Only accept if timestamp is not more than 1 minute old if (tstamp.t64 + 6000*1000UL < signing->timestamp) { - return false; + signing->last_status = MAVLINK_SIGNING_STATUS_OLD_TIMESTAMP; + return false; } // add new stream signing_streams->stream[i].sysid = msg->sysid; @@ -182,7 +190,8 @@ MAVLINK_HELPER bool mavlink_signature_check(mavlink_signing_t *signing, memcpy(last_tstamp.t8, signing_streams->stream[i].timestamp_bytes, 6); if (tstamp.t64 <= last_tstamp.t64) { // repeating old timestamp - return false; + signing->last_status = MAVLINK_SIGNING_STATUS_REPLAY; + return false; } } @@ -193,8 +202,10 @@ MAVLINK_HELPER bool mavlink_signature_check(mavlink_signing_t *signing, if (tstamp.t64 > signing->timestamp) { signing->timestamp = tstamp.t64; } - return true; + signing->last_status = MAVLINK_SIGNING_STATUS_OK; + return true; } +#endif /** @@ -213,7 +224,11 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, mavlink_status_t* status, uint8_t min_length, uint8_t length, uint8_t crc_extra) { bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0; +#ifndef MAVLINK_NO_SIGN_PACKET bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING); +#else + bool signing = false; +#endif uint8_t signature_len = signing? MAVLINK_SIGNATURE_BLOCK_LEN : 0; uint8_t header_len = MAVLINK_CORE_HEADER_LEN+1; uint8_t buf[MAVLINK_CORE_HEADER_LEN+1]; @@ -261,6 +276,7 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, msg->checksum = checksum; +#ifndef MAVLINK_NO_SIGN_PACKET if (signing) { mavlink_sign_packet(status->signing, msg->signature, @@ -268,7 +284,8 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, (const uint8_t *)_MAV_PAYLOAD(msg), msg->len, (const uint8_t *)_MAV_PAYLOAD(msg)+(uint16_t)msg->len); } - +#endif + return msg->len + header_len + 2 + signature_len; } @@ -351,12 +368,14 @@ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint ck[0] = (uint8_t)(checksum & 0xFF); ck[1] = (uint8_t)(checksum >> 8); +#ifndef MAVLINK_NO_SIGN_PACKET if (signing) { // possibly add a signature signature_len = mavlink_sign_packet(status->signing, signature, buf, header_len+1, (const uint8_t *)packet, length, ck); } - +#endif + MAVLINK_START_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len); _mavlink_send_uart(chan, (const char *)buf, header_len+1); _mavlink_send_uart(chan, packet, length); @@ -574,6 +593,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) { + status->msg_received = MAVLINK_FRAMING_INCOMPLETE; switch (status->parse_state) @@ -608,7 +628,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, { status->buffer_overrun++; _mav_parse_error(status); - status->msg_received = 0; + status->msg_received = MAVLINK_FRAMING_INCOMPLETE; status->parse_state = MAVLINK_PARSE_STATE_IDLE; } else @@ -632,7 +652,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, if ((rxmsg->incompat_flags & ~MAVLINK_IFLAG_MASK) != 0) { // message includes an incompatible feature flag _mav_parse_error(status); - status->msg_received = 0; + status->msg_received = MAVLINK_FRAMING_INCOMPLETE; status->parse_state = MAVLINK_PARSE_STATE_IDLE; break; } @@ -687,7 +707,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, break; case MAVLINK_PARSE_STATE_GOT_MSGID1: - rxmsg->msgid |= c<<8; + rxmsg->msgid |= ((uint32_t)c)<<8; mavlink_update_checksum(rxmsg, c); status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID2; break; @@ -722,18 +742,24 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, case MAVLINK_PARSE_STATE_GOT_PAYLOAD: { const mavlink_msg_entry_t *e = mavlink_get_msg_entry(rxmsg->msgid); - uint8_t crc_extra = e?e->crc_extra:0; - mavlink_update_checksum(rxmsg, crc_extra); - if (c != (rxmsg->checksum & 0xFF)) { + if (e == NULL) { + // Message not found in CRC_EXTRA table. status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1; + rxmsg->ck[0] = c; } else { - status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; - } - rxmsg->ck[0] = c; + uint8_t crc_extra = e->crc_extra; + mavlink_update_checksum(rxmsg, crc_extra); + if (c != (rxmsg->checksum & 0xFF)) { + status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1; + } else { + status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; + } + rxmsg->ck[0] = c; - // zero-fill the packet to cope with short incoming packets - if (e && status->packet_idx < e->max_msg_len) { - memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->max_msg_len - status->packet_idx); + // zero-fill the packet to cope with short incoming packets + if (e && status->packet_idx < e->max_msg_len) { + memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->max_msg_len - status->packet_idx); + } } break; } @@ -750,14 +776,15 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, rxmsg->ck[1] = c; if (rxmsg->incompat_flags & MAVLINK_IFLAG_SIGNED) { - status->parse_state = MAVLINK_PARSE_STATE_SIGNATURE_WAIT; - status->signature_wait = MAVLINK_SIGNATURE_BLOCK_LEN; - - // If the CRC is already wrong, don't overwrite msg_received, - // otherwise we can end up with garbage flagged as valid. - if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) { - status->msg_received = MAVLINK_FRAMING_INCOMPLETE; + if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) { + // If the CRC is already wrong, don't overwrite msg_received, + // otherwise we can end up with garbage flagged as valid. + status->parse_state = MAVLINK_PARSE_STATE_SIGNATURE_WAIT_BAD_CRC; + } else { + status->parse_state = MAVLINK_PARSE_STATE_SIGNATURE_WAIT; + status->msg_received = MAVLINK_FRAMING_INCOMPLETE; } + status->signature_wait = MAVLINK_SIGNATURE_BLOCK_LEN; } else { if (status->signing && (status->signing->accept_unsigned_callback == NULL || @@ -775,21 +802,28 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, } break; case MAVLINK_PARSE_STATE_SIGNATURE_WAIT: + case MAVLINK_PARSE_STATE_SIGNATURE_WAIT_BAD_CRC: rxmsg->signature[MAVLINK_SIGNATURE_BLOCK_LEN-status->signature_wait] = c; status->signature_wait--; if (status->signature_wait == 0) { // we have the whole signature, check it is OK +#ifndef MAVLINK_NO_SIGNATURE_CHECK bool sig_ok = mavlink_signature_check(status->signing, status->signing_streams, rxmsg); +#else + bool sig_ok = true; +#endif if (!sig_ok && (status->signing->accept_unsigned_callback && status->signing->accept_unsigned_callback(status, rxmsg->msgid))) { // accepted via application level override sig_ok = true; } - if (sig_ok) { - status->msg_received = MAVLINK_FRAMING_OK; + if (status->parse_state == MAVLINK_PARSE_STATE_SIGNATURE_WAIT_BAD_CRC) { + status->msg_received = MAVLINK_FRAMING_BAD_CRC; + } else if (sig_ok) { + status->msg_received = MAVLINK_FRAMING_OK; } else { - status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE; + status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE; } status->parse_state = MAVLINK_PARSE_STATE_IDLE; if (r_message !=NULL) { @@ -799,7 +833,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, break; } - // If a message has been sucessfully decoded, check index + // If a message has been successfully decoded, check index if (status->msg_received == MAVLINK_FRAMING_OK) { //while(status->current_seq != rxmsg->seq) diff --git a/lib/main/MAVLink/mavlink_types.h b/lib/main/MAVLink/mavlink_types.h index 8bdf0b51c75..8b97cb818fa 100755 --- a/lib/main/MAVLink/mavlink_types.h +++ b/lib/main/MAVLink/mavlink_types.h @@ -199,7 +199,8 @@ typedef enum { MAVLINK_PARSE_STATE_GOT_PAYLOAD, MAVLINK_PARSE_STATE_GOT_CRC1, MAVLINK_PARSE_STATE_GOT_BAD_CRC1, - MAVLINK_PARSE_STATE_SIGNATURE_WAIT + MAVLINK_PARSE_STATE_SIGNATURE_WAIT, + MAVLINK_PARSE_STATE_SIGNATURE_WAIT_BAD_CRC } mavlink_parse_state_t; ///< The state machine for the comm parser typedef enum { @@ -242,6 +243,16 @@ typedef bool (*mavlink_accept_unsigned_t)(const mavlink_status_t *status, uint32 */ #define MAVLINK_SIGNING_FLAG_SIGN_OUTGOING 1 ///< Enable outgoing signing +typedef enum { + MAVLINK_SIGNING_STATUS_NONE=0, + MAVLINK_SIGNING_STATUS_OK=1, + MAVLINK_SIGNING_STATUS_BAD_SIGNATURE=2, + MAVLINK_SIGNING_STATUS_NO_STREAMS=3, + MAVLINK_SIGNING_STATUS_TOO_MANY_STREAMS=4, + MAVLINK_SIGNING_STATUS_OLD_TIMESTAMP=5, + MAVLINK_SIGNING_STATUS_REPLAY=6, +} mavlink_signing_status_t; + /* state of MAVLink signing for this channel */ @@ -251,6 +262,7 @@ typedef struct __mavlink_signing { uint64_t timestamp; ///< Timestamp, in microseconds since UNIX epoch GMT uint8_t secret_key[32]; mavlink_accept_unsigned_t accept_unsigned_callback; + mavlink_signing_status_t last_status; } mavlink_signing_t; /* diff --git a/lib/main/MAVLink/minimal/mavlink.h b/lib/main/MAVLink/minimal/mavlink.h index 32c6c97c684..817557b1eec 100644 --- a/lib/main/MAVLink/minimal/mavlink.h +++ b/lib/main/MAVLink/minimal/mavlink.h @@ -6,7 +6,7 @@ #ifndef MAVLINK_H #define MAVLINK_H -#define MAVLINK_PRIMARY_XML_IDX 1 +#define MAVLINK_PRIMARY_XML_HASH 154102757936060511 #ifndef MAVLINK_STX #define MAVLINK_STX 253 diff --git a/lib/main/MAVLink/minimal/mavlink_msg_heartbeat.h b/lib/main/MAVLink/minimal/mavlink_msg_heartbeat.h index 7774a1006b9..cf527aa51a9 100644 --- a/lib/main/MAVLink/minimal/mavlink_msg_heartbeat.h +++ b/lib/main/MAVLink/minimal/mavlink_msg_heartbeat.h @@ -92,6 +92,53 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); } +/** + * @brief Pack a heartbeat message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param type Vehicle or component type. For a flight controller component the vehicle type (quadrotor, helicopter, etc.). For other components the component type (e.g. camera, gimbal, etc.). This should be used in preference to component id for identifying the component type. + * @param autopilot Autopilot type / class. Use MAV_AUTOPILOT_INVALID for components that are not flight controllers. + * @param base_mode System mode bitmap. + * @param custom_mode A bitfield for use for autopilot-specific flags + * @param system_status System status flag. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_heartbeat_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN]; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 5, autopilot); + _mav_put_uint8_t(buf, 6, base_mode); + _mav_put_uint8_t(buf, 7, system_status); + _mav_put_uint8_t(buf, 8, 3); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#else + mavlink_heartbeat_t packet; + packet.custom_mode = custom_mode; + packet.type = type; + packet.autopilot = autopilot; + packet.base_mode = base_mode; + packet.system_status = system_status; + packet.mavlink_version = 3; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif +} + /** * @brief Pack a heartbeat message on a channel * @param system_id ID of this system @@ -162,6 +209,20 @@ static inline uint16_t mavlink_msg_heartbeat_encode_chan(uint8_t system_id, uint return mavlink_msg_heartbeat_pack_chan(system_id, component_id, chan, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status); } +/** + * @brief Encode a heartbeat struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param heartbeat C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_heartbeat_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat) +{ + return mavlink_msg_heartbeat_pack_status(system_id, component_id, _status, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status); +} + /** * @brief Send a heartbeat message * @param chan MAVLink channel to send the message @@ -215,7 +276,7 @@ static inline void mavlink_msg_heartbeat_send_struct(mavlink_channel_t chan, con #if MAVLINK_MSG_ID_HEARTBEAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an diff --git a/lib/main/MAVLink/minimal/mavlink_msg_protocol_version.h b/lib/main/MAVLink/minimal/mavlink_msg_protocol_version.h index d795ff28c6d..efc35ab8eb0 100644 --- a/lib/main/MAVLink/minimal/mavlink_msg_protocol_version.h +++ b/lib/main/MAVLink/minimal/mavlink_msg_protocol_version.h @@ -64,6 +64,45 @@ typedef struct __mavlink_protocol_version_t { static inline uint16_t mavlink_msg_protocol_version_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t version, uint16_t min_version, uint16_t max_version, const uint8_t *spec_version_hash, const uint8_t *library_version_hash) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN]; + _mav_put_uint16_t(buf, 0, version); + _mav_put_uint16_t(buf, 2, min_version); + _mav_put_uint16_t(buf, 4, max_version); + _mav_put_uint8_t_array(buf, 6, spec_version_hash, 8); + _mav_put_uint8_t_array(buf, 14, library_version_hash, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); +#else + mavlink_protocol_version_t packet; + packet.version = version; + packet.min_version = min_version; + packet.max_version = max_version; + mav_array_assign_uint8_t(packet.spec_version_hash, spec_version_hash, 8); + mav_array_assign_uint8_t(packet.library_version_hash, library_version_hash, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PROTOCOL_VERSION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); +} + +/** + * @brief Pack a protocol_version message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param version Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc. + * @param min_version Minimum MAVLink version supported + * @param max_version Maximum MAVLink version supported (set to the same value as version by default) + * @param spec_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. + * @param library_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_protocol_version_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint16_t version, uint16_t min_version, uint16_t max_version, const uint8_t *spec_version_hash, const uint8_t *library_version_hash) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN]; _mav_put_uint16_t(buf, 0, version); @@ -83,7 +122,11 @@ static inline uint16_t mavlink_msg_protocol_version_pack(uint8_t system_id, uint #endif msg->msgid = MAVLINK_MSG_ID_PROTOCOL_VERSION; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); +#endif } /** @@ -116,8 +159,8 @@ static inline uint16_t mavlink_msg_protocol_version_pack_chan(uint8_t system_id, packet.version = version; packet.min_version = min_version; packet.max_version = max_version; - mav_array_memcpy(packet.spec_version_hash, spec_version_hash, sizeof(uint8_t)*8); - mav_array_memcpy(packet.library_version_hash, library_version_hash, sizeof(uint8_t)*8); + mav_array_assign_uint8_t(packet.spec_version_hash, spec_version_hash, 8); + mav_array_assign_uint8_t(packet.library_version_hash, library_version_hash, 8); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); #endif @@ -152,6 +195,20 @@ static inline uint16_t mavlink_msg_protocol_version_encode_chan(uint8_t system_i return mavlink_msg_protocol_version_pack_chan(system_id, component_id, chan, msg, protocol_version->version, protocol_version->min_version, protocol_version->max_version, protocol_version->spec_version_hash, protocol_version->library_version_hash); } +/** + * @brief Encode a protocol_version struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param protocol_version C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_protocol_version_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_protocol_version_t* protocol_version) +{ + return mavlink_msg_protocol_version_pack_status(system_id, component_id, _status, msg, protocol_version->version, protocol_version->min_version, protocol_version->max_version, protocol_version->spec_version_hash, protocol_version->library_version_hash); +} + /** * @brief Send a protocol_version message * @param chan MAVLink channel to send the message @@ -179,8 +236,8 @@ static inline void mavlink_msg_protocol_version_send(mavlink_channel_t chan, uin packet.version = version; packet.min_version = min_version; packet.max_version = max_version; - mav_array_memcpy(packet.spec_version_hash, spec_version_hash, sizeof(uint8_t)*8); - mav_array_memcpy(packet.library_version_hash, library_version_hash, sizeof(uint8_t)*8); + mav_array_assign_uint8_t(packet.spec_version_hash, spec_version_hash, 8); + mav_array_assign_uint8_t(packet.library_version_hash, library_version_hash, 8); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PROTOCOL_VERSION, (const char *)&packet, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); #endif } @@ -201,7 +258,7 @@ static inline void mavlink_msg_protocol_version_send_struct(mavlink_channel_t ch #if MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -222,8 +279,8 @@ static inline void mavlink_msg_protocol_version_send_buf(mavlink_message_t *msgb packet->version = version; packet->min_version = min_version; packet->max_version = max_version; - mav_array_memcpy(packet->spec_version_hash, spec_version_hash, sizeof(uint8_t)*8); - mav_array_memcpy(packet->library_version_hash, library_version_hash, sizeof(uint8_t)*8); + mav_array_assign_uint8_t(packet->spec_version_hash, spec_version_hash, 8); + mav_array_assign_uint8_t(packet->library_version_hash, library_version_hash, 8); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PROTOCOL_VERSION, (const char *)packet, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); #endif } diff --git a/lib/main/MAVLink/minimal/minimal.h b/lib/main/MAVLink/minimal/minimal.h index 755eeba8889..b6c950e08b5 100644 --- a/lib/main/MAVLink/minimal/minimal.h +++ b/lib/main/MAVLink/minimal/minimal.h @@ -10,8 +10,7 @@ #error Wrong include order: MAVLINK_MINIMAL.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call. #endif -#undef MAVLINK_THIS_XML_IDX -#define MAVLINK_THIS_XML_IDX 1 +#define MAVLINK_MINIMAL_XML_HASH 154102757936060511 #ifdef __cplusplus extern "C" { @@ -59,7 +58,8 @@ typedef enum MAV_AUTOPILOT MAV_AUTOPILOT_ASLUAV=17, /* ASLUAV autopilot -- http://www.asl.ethz.ch | */ MAV_AUTOPILOT_SMARTAP=18, /* SmartAP Autopilot - http://sky-drones.com | */ MAV_AUTOPILOT_AIRRAILS=19, /* AirRails - http://uaventure.com | */ - MAV_AUTOPILOT_ENUM_END=20, /* | */ + MAV_AUTOPILOT_REFLEX=20, /* Fusion Reflex - https://fusion.engineering | */ + MAV_AUTOPILOT_ENUM_END=21, /* | */ } MAV_AUTOPILOT; #endif @@ -87,12 +87,12 @@ typedef enum MAV_TYPE MAV_TYPE_FLAPPING_WING=16, /* Flapping wing | */ MAV_TYPE_KITE=17, /* Kite | */ MAV_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */ - MAV_TYPE_VTOL_DUOROTOR=19, /* Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter. | */ - MAV_TYPE_VTOL_QUADROTOR=20, /* Quad-rotor VTOL using a V-shaped quad config in vertical operation. Tailsitter. | */ - MAV_TYPE_VTOL_TILTROTOR=21, /* Tiltrotor VTOL | */ - MAV_TYPE_VTOL_RESERVED2=22, /* VTOL reserved 2 | */ - MAV_TYPE_VTOL_RESERVED3=23, /* VTOL reserved 3 | */ - MAV_TYPE_VTOL_RESERVED4=24, /* VTOL reserved 4 | */ + MAV_TYPE_VTOL_TAILSITTER_DUOROTOR=19, /* Two-rotor Tailsitter VTOL that additionally uses control surfaces in vertical operation. Note, value previously named MAV_TYPE_VTOL_DUOROTOR. | */ + MAV_TYPE_VTOL_TAILSITTER_QUADROTOR=20, /* Quad-rotor Tailsitter VTOL using a V-shaped quad config in vertical operation. Note: value previously named MAV_TYPE_VTOL_QUADROTOR. | */ + MAV_TYPE_VTOL_TILTROTOR=21, /* Tiltrotor VTOL. Fuselage and wings stay (nominally) horizontal in all flight phases. It able to tilt (some) rotors to provide thrust in cruise flight. | */ + MAV_TYPE_VTOL_FIXEDROTOR=22, /* VTOL with separate fixed rotors for hover and cruise flight. Fuselage and wings stay (nominally) horizontal in all flight phases. | */ + MAV_TYPE_VTOL_TAILSITTER=23, /* Tailsitter VTOL. Fuselage and wings orientation changes depending on flight phase: vertical for hover, horizontal for cruise. Use more specific VTOL MAV_TYPE_VTOL_TAILSITTER_DUOROTOR or MAV_TYPE_VTOL_TAILSITTER_QUADROTOR if appropriate. | */ + MAV_TYPE_VTOL_TILTWING=24, /* Tiltwing VTOL. Fuselage stays horizontal in all flight phases. The whole wing, along with any attached engine, can tilt between vertical and horizontal mode. | */ MAV_TYPE_VTOL_RESERVED5=25, /* VTOL reserved 5 | */ MAV_TYPE_GIMBAL=26, /* Gimbal | */ MAV_TYPE_ADSB=27, /* ADSB system | */ @@ -104,16 +104,30 @@ typedef enum MAV_TYPE MAV_TYPE_SERVO=33, /* Servo | */ MAV_TYPE_ODID=34, /* Open Drone ID. See https://mavlink.io/en/services/opendroneid.html. | */ MAV_TYPE_DECAROTOR=35, /* Decarotor | */ - MAV_TYPE_ENUM_END=36, /* | */ + MAV_TYPE_BATTERY=36, /* Battery | */ + MAV_TYPE_PARACHUTE=37, /* Parachute | */ + MAV_TYPE_LOG=38, /* Log | */ + MAV_TYPE_OSD=39, /* OSD | */ + MAV_TYPE_IMU=40, /* IMU | */ + MAV_TYPE_GPS=41, /* GPS | */ + MAV_TYPE_WINCH=42, /* Winch | */ + MAV_TYPE_GENERIC_MULTIROTOR=43, /* Generic multirotor that does not fit into a specific type or whose type is unknown | */ + MAV_TYPE_ILLUMINATOR=44, /* Illuminator. An illuminator is a light source that is used for lighting up dark areas external to the system: e.g. a torch or searchlight (as opposed to a light source for illuminating the system itself, e.g. an indicator light). | */ + MAV_TYPE_SPACECRAFT_ORBITER=45, /* Orbiter spacecraft. Includes satellites orbiting terrestrial and extra-terrestrial bodies. Follows NASA Spacecraft Classification. | */ + MAV_TYPE_GROUND_QUADRUPED=46, /* A generic four-legged ground vehicle (e.g., a robot dog). | */ + MAV_TYPE_VTOL_GYRODYNE=47, /* VTOL hybrid of helicopter and autogyro. It has a main rotor for lift and separate propellers for forward flight. The rotor must be powered for hover but can autorotate in cruise flight. See: https://en.wikipedia.org/wiki/Gyrodyne | */ + MAV_TYPE_GRIPPER=48, /* Gripper | */ + MAV_TYPE_RADIO=49, /* Radio | */ + MAV_TYPE_ENUM_END=50, /* | */ } MAV_TYPE; #endif -/** @brief These flags encode the MAV mode. */ +/** @brief These flags encode the MAV mode, see MAV_MODE enum for useful combinations. */ #ifndef HAVE_ENUM_MAV_MODE_FLAG #define HAVE_ENUM_MAV_MODE_FLAG typedef enum MAV_MODE_FLAG { - MAV_MODE_FLAG_CUSTOM_MODE_ENABLED=1, /* 0b00000001 Reserved for future use. | */ + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED=1, /* 0b00000001 system-specific custom mode is enabled. When using this flag to enable a custom mode all other flags should be ignored. | */ MAV_MODE_FLAG_TEST_ENABLED=2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ MAV_MODE_FLAG_AUTO_ENABLED=4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ MAV_MODE_FLAG_GUIDED_ENABLED=8, /* 0b00001000 guided mode enabled, system flies waypoints / mission items. | */ @@ -152,17 +166,26 @@ typedef enum MAV_STATE MAV_STATE_CALIBRATING=2, /* System is calibrating and not flight-ready. | */ MAV_STATE_STANDBY=3, /* System is grounded and on standby. It can be launched any time. | */ MAV_STATE_ACTIVE=4, /* System is active and might be already airborne. Motors are engaged. | */ - MAV_STATE_CRITICAL=5, /* System is in a non-normal flight mode. It can however still navigate. | */ - MAV_STATE_EMERGENCY=6, /* System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. | */ + MAV_STATE_CRITICAL=5, /* System is in a non-normal flight mode (failsafe). It can however still navigate. | */ + MAV_STATE_EMERGENCY=6, /* System is in a non-normal flight mode (failsafe). It lost control over parts or over the whole airframe. It is in mayday and going down. | */ MAV_STATE_POWEROFF=7, /* System just initialized its power-down sequence, will shut down now. | */ - MAV_STATE_FLIGHT_TERMINATION=8, /* System is terminating itself. | */ + MAV_STATE_FLIGHT_TERMINATION=8, /* System is terminating itself (failsafe or commanded). | */ MAV_STATE_ENUM_END=9, /* | */ } MAV_STATE; #endif -/** @brief Component ids (values) for the different types and instances of onboard hardware/software that might make up a MAVLink system (autopilot, cameras, servos, GPS systems, avoidance systems etc.). - Components must use the appropriate ID in their source address when sending messages. Components can also use IDs to determine if they are the intended recipient of an incoming message. The MAV_COMP_ID_ALL value is used to indicate messages that must be processed by all components. - When creating new entries, components that can have multiple instances (e.g. cameras, servos etc.) should be allocated sequential values. An appropriate number of values should be left free after these components to allow the number of instances to be expanded. */ +/** @brief Legacy component ID values for particular types of hardware/software that might make up a MAVLink system (autopilot, cameras, servos, avoidance systems etc.). + + Components are not required or expected to use IDs with names that correspond to their type or function, but may choose to do so. + Using an ID that matches the type may slightly reduce the chances of component id clashes, as, for historical reasons, it is less likely to be used by some other type of component. + System integration will still need to ensure that all components have unique IDs. + + Component IDs are used for addressing messages to a particular component within a system. + A component can use any unique ID between 1 and 255 (MAV_COMP_ID_ALL value is the broadcast address, used to send to all components). + + Historically component ID were also used for identifying the type of component. + New code must not use component IDs to infer the component type, but instead check the MAV_TYPE in the HEARTBEAT message! + */ #ifndef HAVE_ENUM_MAV_COMPONENT #define HAVE_ENUM_MAV_COMPONENT typedef enum MAV_COMPONENT @@ -250,6 +273,9 @@ typedef enum MAV_COMPONENT MAV_COMP_ID_CAMERA4=103, /* Camera #4. | */ MAV_COMP_ID_CAMERA5=104, /* Camera #5. | */ MAV_COMP_ID_CAMERA6=105, /* Camera #6. | */ + MAV_COMP_ID_RADIO=110, /* Radio #1. | */ + MAV_COMP_ID_RADIO2=111, /* Radio #2. | */ + MAV_COMP_ID_RADIO3=112, /* Radio #3. | */ MAV_COMP_ID_SERVO1=140, /* Servo #1. | */ MAV_COMP_ID_SERVO2=141, /* Servo #2. | */ MAV_COMP_ID_SERVO3=142, /* Servo #3. | */ @@ -271,13 +297,21 @@ typedef enum MAV_COMPONENT MAV_COMP_ID_PERIPHERAL=158, /* Generic autopilot peripheral component ID. Meant for devices that do not implement the parameter microservice. | */ MAV_COMP_ID_QX1_GIMBAL=159, /* Gimbal ID for QX1. | */ MAV_COMP_ID_FLARM=160, /* FLARM collision alert component. | */ + MAV_COMP_ID_PARACHUTE=161, /* Parachute component. | */ + MAV_COMP_ID_WINCH=169, /* Winch component. | */ MAV_COMP_ID_GIMBAL2=171, /* Gimbal #2. | */ MAV_COMP_ID_GIMBAL3=172, /* Gimbal #3. | */ MAV_COMP_ID_GIMBAL4=173, /* Gimbal #4 | */ MAV_COMP_ID_GIMBAL5=174, /* Gimbal #5. | */ MAV_COMP_ID_GIMBAL6=175, /* Gimbal #6. | */ + MAV_COMP_ID_BATTERY=180, /* Battery #1. | */ + MAV_COMP_ID_BATTERY2=181, /* Battery #2. | */ + MAV_COMP_ID_MAVCAN=189, /* CAN over MAVLink client. | */ MAV_COMP_ID_MISSIONPLANNER=190, /* Component that can generate/supply a mission flight plan (e.g. GCS or developer API). | */ MAV_COMP_ID_ONBOARD_COMPUTER=191, /* Component that lives on the onboard computer (companion computer) and has some generic functionalities, such as settings system parameters and monitoring the status of some processes that don't directly speak mavlink and so on. | */ + MAV_COMP_ID_ONBOARD_COMPUTER2=192, /* Component that lives on the onboard computer (companion computer) and has some generic functionalities, such as settings system parameters and monitoring the status of some processes that don't directly speak mavlink and so on. | */ + MAV_COMP_ID_ONBOARD_COMPUTER3=193, /* Component that lives on the onboard computer (companion computer) and has some generic functionalities, such as settings system parameters and monitoring the status of some processes that don't directly speak mavlink and so on. | */ + MAV_COMP_ID_ONBOARD_COMPUTER4=194, /* Component that lives on the onboard computer (companion computer) and has some generic functionalities, such as settings system parameters and monitoring the status of some processes that don't directly speak mavlink and so on. | */ MAV_COMP_ID_PATHPLANNER=195, /* Component that finds an optimal path between points based on a certain constraint (e.g. minimum snap, shortest path, cost, etc.). | */ MAV_COMP_ID_OBSTACLE_AVOIDANCE=196, /* Component that plans a collision free path between two points. | */ MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY=197, /* Component that provides position estimates using VIO techniques. | */ @@ -293,7 +327,8 @@ typedef enum MAV_COMPONENT MAV_COMP_ID_UDP_BRIDGE=240, /* Component to bridge MAVLink to UDP (i.e. from a UART). | */ MAV_COMP_ID_UART_BRIDGE=241, /* Component to bridge to UART (i.e. from UDP). | */ MAV_COMP_ID_TUNNEL_NODE=242, /* Component handling TUNNEL messages (e.g. vendor specific GUI of a component). | */ - MAV_COMP_ID_SYSTEM_CONTROL=250, /* Component for handling system messages (e.g. to ARM, takeoff, etc.). | */ + MAV_COMP_ID_ILLUMINATOR=243, /* Illuminator | */ + MAV_COMP_ID_SYSTEM_CONTROL=250, /* Deprecated, don't use. Component for handling system messages (e.g. to ARM, takeoff, etc.). | */ MAV_COMPONENT_ENUM_END=251, /* | */ } MAV_COMPONENT; #endif @@ -316,10 +351,8 @@ typedef enum MAV_COMPONENT // base include -#undef MAVLINK_THIS_XML_IDX -#define MAVLINK_THIS_XML_IDX 1 -#if MAVLINK_THIS_XML_IDX == MAVLINK_PRIMARY_XML_IDX +#if MAVLINK_MINIMAL_XML_HASH == MAVLINK_PRIMARY_XML_HASH # define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_PROTOCOL_VERSION} # define MAVLINK_MESSAGE_NAMES {{ "HEARTBEAT", 0 }, { "PROTOCOL_VERSION", 300 }} # if MAVLINK_COMMAND_24BIT diff --git a/lib/main/MAVLink/minimal/testsuite.h b/lib/main/MAVLink/minimal/testsuite.h index fd9c37f4145..ad2471cd396 100644 --- a/lib/main/MAVLink/minimal/testsuite.h +++ b/lib/main/MAVLink/minimal/testsuite.h @@ -1,6 +1,6 @@ /** @file * @brief MAVLink comm protocol testsuite generated from minimal.xml - * @see http://qgroundcontrol.org/mavlink/ + * @see https://mavlink.io/en/ */ #pragma once #ifndef MINIMAL_TESTSUITE_H @@ -82,6 +82,11 @@ static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id, mavl mavlink_msg_heartbeat_send(MAVLINK_COMM_1 , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); mavlink_msg_heartbeat_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("HEARTBEAT") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_HEARTBEAT) != NULL); +#endif } static void mavlink_test_protocol_version(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) @@ -140,6 +145,11 @@ static void mavlink_test_protocol_version(uint8_t system_id, uint8_t component_i mavlink_msg_protocol_version_send(MAVLINK_COMM_1 , packet1.version , packet1.min_version , packet1.max_version , packet1.spec_version_hash , packet1.library_version_hash ); mavlink_msg_protocol_version_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + +#ifdef MAVLINK_HAVE_GET_MESSAGE_INFO + MAVLINK_ASSERT(mavlink_get_message_info_by_name("PROTOCOL_VERSION") != NULL); + MAVLINK_ASSERT(mavlink_get_message_info_by_id(MAVLINK_MSG_ID_PROTOCOL_VERSION) != NULL); +#endif } static void mavlink_test_minimal(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) diff --git a/lib/main/MAVLink/minimal/version.h b/lib/main/MAVLink/minimal/version.h index b80bc639e96..3064b37b971 100644 --- a/lib/main/MAVLink/minimal/version.h +++ b/lib/main/MAVLink/minimal/version.h @@ -7,7 +7,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Sun Feb 28 2021" +#define MAVLINK_BUILD_DATE "Fri Aug 29 2025" #define MAVLINK_WIRE_PROTOCOL_VERSION "2.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 22 diff --git a/lib/main/MAVLink/protocol.h b/lib/main/MAVLink/protocol.h index 6feca0104c5..a496e265f43 100755 --- a/lib/main/MAVLink/protocol.h +++ b/lib/main/MAVLink/protocol.h @@ -43,6 +43,8 @@ MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan); #endif MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan); + MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + mavlink_status_t* status, uint8_t min_length, uint8_t length, uint8_t crc_extra); MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra); MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, @@ -177,13 +179,43 @@ static void mav_array_memcpy(void *dest, const void *src, size_t n) } } +/* + * Array direct assignment, for use when fields align and no byte swapping + * is required. Most are directly #defined to mav_array_memcpy, except for + * mav_array_assign_char, which uses strncpy instead. + */ +#if !MAVLINK_NEED_BYTE_SWAP && MAVLINK_ALIGNED_FIELDS +static void mav_array_assign_char(char *dest, const char *src, size_t n) +{ + if (src == NULL) { + memset(dest, 0, n); + } else { + /* strncpy will zero pad dest array up to n bytes */ + strncpy(dest, src, n); + } +} +#define mav_array_assign_uint8_t(DEST,SRC,N) mav_array_memcpy(DEST,SRC,N*sizeof(uint8_t)) +#define mav_array_assign_int8_t(DEST,SRC,N) mav_array_memcpy(DEST,SRC,N*sizeof(int8_t)) +#define mav_array_assign_uint16_t(DEST,SRC,N) mav_array_memcpy(DEST,SRC,N*sizeof(uint16_t)) +#define mav_array_assign_int16_t(DEST,SRC,N) mav_array_memcpy(DEST,SRC,N*sizeof(int16_t)) +#define mav_array_assign_uint32_t(DEST,SRC,N) mav_array_memcpy(DEST,SRC,N*sizeof(uint32_t)) +#define mav_array_assign_int32_t(DEST,SRC,N) mav_array_memcpy(DEST,SRC,N*sizeof(int32_t)) +#define mav_array_assign_uint64_t(DEST,SRC,N) mav_array_memcpy(DEST,SRC,N*sizeof(uint64_t)) +#define mav_array_assign_int64_t(DEST,SRC,N) mav_array_memcpy(DEST,SRC,N*sizeof(int64_t)) +#define mav_array_assign_float(DEST,SRC,N) mav_array_memcpy(DEST,SRC,N*sizeof(float)) +#define mav_array_assign_double(DEST,SRC,N) mav_array_memcpy(DEST,SRC,N*sizeof(double)) +#endif + /* * Place a char array into a buffer */ static void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length) { - mav_array_memcpy(&buf[wire_offset], b, array_length); - + if (b == NULL) { + memset(&buf[wire_offset], 0, array_length); + } else { + strncpy(&buf[wire_offset], b, array_length); + } } /* diff --git a/lib/main/MAVLink/standard/mavlink.h b/lib/main/MAVLink/standard/mavlink.h new file mode 100644 index 00000000000..59bf5ee3d4b --- /dev/null +++ b/lib/main/MAVLink/standard/mavlink.h @@ -0,0 +1,34 @@ +/** @file + * @brief MAVLink comm protocol built from standard.xml + * @see http://mavlink.org + */ +#pragma once +#ifndef MAVLINK_H +#define MAVLINK_H + +#define MAVLINK_PRIMARY_XML_HASH 2116097011577996616 + +#ifndef MAVLINK_STX +#define MAVLINK_STX 253 +#endif + +#ifndef MAVLINK_ENDIAN +#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN +#endif + +#ifndef MAVLINK_ALIGNED_FIELDS +#define MAVLINK_ALIGNED_FIELDS 1 +#endif + +#ifndef MAVLINK_CRC_EXTRA +#define MAVLINK_CRC_EXTRA 1 +#endif + +#ifndef MAVLINK_COMMAND_24BIT +#define MAVLINK_COMMAND_24BIT 1 +#endif + +#include "version.h" +#include "standard.h" + +#endif // MAVLINK_H diff --git a/lib/main/MAVLink/common/mavlink_msg_autopilot_version.h b/lib/main/MAVLink/standard/mavlink_msg_autopilot_version.h old mode 100755 new mode 100644 similarity index 73% rename from lib/main/MAVLink/common/mavlink_msg_autopilot_version.h rename to lib/main/MAVLink/standard/mavlink_msg_autopilot_version.h index 37b87be38fd..7840793170f --- a/lib/main/MAVLink/common/mavlink_msg_autopilot_version.h +++ b/lib/main/MAVLink/standard/mavlink_msg_autopilot_version.h @@ -7,10 +7,12 @@ typedef struct __mavlink_autopilot_version_t { uint64_t capabilities; /*< Bitmap of capabilities*/ uint64_t uid; /*< UID if provided by hardware (see uid2)*/ - uint32_t flight_sw_version; /*< Firmware version number*/ + uint32_t flight_sw_version; /*< Firmware version number. + The field must be encoded as 4 bytes, where each byte (shown from MSB to LSB) is part of a semantic version: (major) (minor) (patch) (FIRMWARE_VERSION_TYPE). + */ uint32_t middleware_sw_version; /*< Middleware version number*/ uint32_t os_sw_version; /*< Operating system version number*/ - uint32_t board_version; /*< HW / board version (last 8 bytes should be silicon ID, if any)*/ + uint32_t board_version; /*< HW / board version (last 8 bits should be silicon ID, if any). The first 16 bits of this field specify a board type from an enumeration stored at https://github.com/PX4/PX4-Bootloader/blob/master/board_types.txt and with extensive additions at https://github.com/ArduPilot/ardupilot/blob/master/Tools/AP_Bootloader/board_types.txt*/ uint16_t vendor_id; /*< ID of the board vendor*/ uint16_t product_id; /*< ID of the product*/ uint8_t flight_custom_version[8]; /*< Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases.*/ @@ -78,10 +80,12 @@ typedef struct __mavlink_autopilot_version_t { * @param msg The MAVLink message to compress the data into * * @param capabilities Bitmap of capabilities - * @param flight_sw_version Firmware version number + * @param flight_sw_version Firmware version number. + The field must be encoded as 4 bytes, where each byte (shown from MSB to LSB) is part of a semantic version: (major) (minor) (patch) (FIRMWARE_VERSION_TYPE). + * @param middleware_sw_version Middleware version number * @param os_sw_version Operating system version number - * @param board_version HW / board version (last 8 bytes should be silicon ID, if any) + * @param board_version HW / board version (last 8 bits should be silicon ID, if any). The first 16 bits of this field specify a board type from an enumeration stored at https://github.com/PX4/PX4-Bootloader/blob/master/board_types.txt and with extensive additions at https://github.com/ArduPilot/ardupilot/blob/master/Tools/AP_Bootloader/board_types.txt * @param flight_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. * @param middleware_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. * @param os_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. @@ -94,6 +98,68 @@ typedef struct __mavlink_autopilot_version_t { static inline uint16_t mavlink_msg_autopilot_version_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, const uint8_t *flight_custom_version, const uint8_t *middleware_custom_version, const uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid, const uint8_t *uid2) { +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN]; + _mav_put_uint64_t(buf, 0, capabilities); + _mav_put_uint64_t(buf, 8, uid); + _mav_put_uint32_t(buf, 16, flight_sw_version); + _mav_put_uint32_t(buf, 20, middleware_sw_version); + _mav_put_uint32_t(buf, 24, os_sw_version); + _mav_put_uint32_t(buf, 28, board_version); + _mav_put_uint16_t(buf, 32, vendor_id); + _mav_put_uint16_t(buf, 34, product_id); + _mav_put_uint8_t_array(buf, 36, flight_custom_version, 8); + _mav_put_uint8_t_array(buf, 44, middleware_custom_version, 8); + _mav_put_uint8_t_array(buf, 52, os_custom_version, 8); + _mav_put_uint8_t_array(buf, 60, uid2, 18); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#else + mavlink_autopilot_version_t packet; + packet.capabilities = capabilities; + packet.uid = uid; + packet.flight_sw_version = flight_sw_version; + packet.middleware_sw_version = middleware_sw_version; + packet.os_sw_version = os_sw_version; + packet.board_version = board_version; + packet.vendor_id = vendor_id; + packet.product_id = product_id; + mav_array_assign_uint8_t(packet.flight_custom_version, flight_custom_version, 8); + mav_array_assign_uint8_t(packet.middleware_custom_version, middleware_custom_version, 8); + mav_array_assign_uint8_t(packet.os_custom_version, os_custom_version, 8); + mav_array_assign_uint8_t(packet.uid2, uid2, 18); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AUTOPILOT_VERSION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); +} + +/** + * @brief Pack a autopilot_version message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * + * @param capabilities Bitmap of capabilities + * @param flight_sw_version Firmware version number. + The field must be encoded as 4 bytes, where each byte (shown from MSB to LSB) is part of a semantic version: (major) (minor) (patch) (FIRMWARE_VERSION_TYPE). + + * @param middleware_sw_version Middleware version number + * @param os_sw_version Operating system version number + * @param board_version HW / board version (last 8 bits should be silicon ID, if any). The first 16 bits of this field specify a board type from an enumeration stored at https://github.com/PX4/PX4-Bootloader/blob/master/board_types.txt and with extensive additions at https://github.com/ArduPilot/ardupilot/blob/master/Tools/AP_Bootloader/board_types.txt + * @param flight_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + * @param middleware_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + * @param os_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. + * @param vendor_id ID of the board vendor + * @param product_id ID of the product + * @param uid UID if provided by hardware (see uid2) + * @param uid2 UID if provided by hardware (supersedes the uid field. If this is non-zero, use this field, otherwise use uid) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_autopilot_version_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg, + uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, const uint8_t *flight_custom_version, const uint8_t *middleware_custom_version, const uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid, const uint8_t *uid2) +{ #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN]; _mav_put_uint64_t(buf, 0, capabilities); @@ -127,7 +193,11 @@ static inline uint16_t mavlink_msg_autopilot_version_pack(uint8_t system_id, uin #endif msg->msgid = MAVLINK_MSG_ID_AUTOPILOT_VERSION; - return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); +#else + return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); +#endif } /** @@ -137,10 +207,12 @@ static inline uint16_t mavlink_msg_autopilot_version_pack(uint8_t system_id, uin * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param capabilities Bitmap of capabilities - * @param flight_sw_version Firmware version number + * @param flight_sw_version Firmware version number. + The field must be encoded as 4 bytes, where each byte (shown from MSB to LSB) is part of a semantic version: (major) (minor) (patch) (FIRMWARE_VERSION_TYPE). + * @param middleware_sw_version Middleware version number * @param os_sw_version Operating system version number - * @param board_version HW / board version (last 8 bytes should be silicon ID, if any) + * @param board_version HW / board version (last 8 bits should be silicon ID, if any). The first 16 bits of this field specify a board type from an enumeration stored at https://github.com/PX4/PX4-Bootloader/blob/master/board_types.txt and with extensive additions at https://github.com/ArduPilot/ardupilot/blob/master/Tools/AP_Bootloader/board_types.txt * @param flight_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. * @param middleware_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. * @param os_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. @@ -179,10 +251,10 @@ static inline uint16_t mavlink_msg_autopilot_version_pack_chan(uint8_t system_id packet.board_version = board_version; packet.vendor_id = vendor_id; packet.product_id = product_id; - mav_array_memcpy(packet.flight_custom_version, flight_custom_version, sizeof(uint8_t)*8); - mav_array_memcpy(packet.middleware_custom_version, middleware_custom_version, sizeof(uint8_t)*8); - mav_array_memcpy(packet.os_custom_version, os_custom_version, sizeof(uint8_t)*8); - mav_array_memcpy(packet.uid2, uid2, sizeof(uint8_t)*18); + mav_array_assign_uint8_t(packet.flight_custom_version, flight_custom_version, 8); + mav_array_assign_uint8_t(packet.middleware_custom_version, middleware_custom_version, 8); + mav_array_assign_uint8_t(packet.os_custom_version, os_custom_version, 8); + mav_array_assign_uint8_t(packet.uid2, uid2, 18); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); #endif @@ -217,15 +289,31 @@ static inline uint16_t mavlink_msg_autopilot_version_encode_chan(uint8_t system_ return mavlink_msg_autopilot_version_pack_chan(system_id, component_id, chan, msg, autopilot_version->capabilities, autopilot_version->flight_sw_version, autopilot_version->middleware_sw_version, autopilot_version->os_sw_version, autopilot_version->board_version, autopilot_version->flight_custom_version, autopilot_version->middleware_custom_version, autopilot_version->os_custom_version, autopilot_version->vendor_id, autopilot_version->product_id, autopilot_version->uid, autopilot_version->uid2); } +/** + * @brief Encode a autopilot_version struct with provided status structure + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param status MAVLink status structure + * @param msg The MAVLink message to compress the data into + * @param autopilot_version C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_autopilot_version_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_autopilot_version_t* autopilot_version) +{ + return mavlink_msg_autopilot_version_pack_status(system_id, component_id, _status, msg, autopilot_version->capabilities, autopilot_version->flight_sw_version, autopilot_version->middleware_sw_version, autopilot_version->os_sw_version, autopilot_version->board_version, autopilot_version->flight_custom_version, autopilot_version->middleware_custom_version, autopilot_version->os_custom_version, autopilot_version->vendor_id, autopilot_version->product_id, autopilot_version->uid, autopilot_version->uid2); +} + /** * @brief Send a autopilot_version message * @param chan MAVLink channel to send the message * * @param capabilities Bitmap of capabilities - * @param flight_sw_version Firmware version number + * @param flight_sw_version Firmware version number. + The field must be encoded as 4 bytes, where each byte (shown from MSB to LSB) is part of a semantic version: (major) (minor) (patch) (FIRMWARE_VERSION_TYPE). + * @param middleware_sw_version Middleware version number * @param os_sw_version Operating system version number - * @param board_version HW / board version (last 8 bytes should be silicon ID, if any) + * @param board_version HW / board version (last 8 bits should be silicon ID, if any). The first 16 bits of this field specify a board type from an enumeration stored at https://github.com/PX4/PX4-Bootloader/blob/master/board_types.txt and with extensive additions at https://github.com/ArduPilot/ardupilot/blob/master/Tools/AP_Bootloader/board_types.txt * @param flight_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. * @param middleware_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. * @param os_custom_version Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases. @@ -263,10 +351,10 @@ static inline void mavlink_msg_autopilot_version_send(mavlink_channel_t chan, ui packet.board_version = board_version; packet.vendor_id = vendor_id; packet.product_id = product_id; - mav_array_memcpy(packet.flight_custom_version, flight_custom_version, sizeof(uint8_t)*8); - mav_array_memcpy(packet.middleware_custom_version, middleware_custom_version, sizeof(uint8_t)*8); - mav_array_memcpy(packet.os_custom_version, os_custom_version, sizeof(uint8_t)*8); - mav_array_memcpy(packet.uid2, uid2, sizeof(uint8_t)*18); + mav_array_assign_uint8_t(packet.flight_custom_version, flight_custom_version, 8); + mav_array_assign_uint8_t(packet.middleware_custom_version, middleware_custom_version, 8); + mav_array_assign_uint8_t(packet.os_custom_version, os_custom_version, 8); + mav_array_assign_uint8_t(packet.uid2, uid2, 18); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, (const char *)&packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); #endif } @@ -287,7 +375,7 @@ static inline void mavlink_msg_autopilot_version_send_struct(mavlink_channel_t c #if MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN <= MAVLINK_MAX_PAYLOAD_LEN /* - This varient of _send() can be used to save stack space by re-using + This variant of _send() can be used to save stack space by reusing memory from the receive buffer. The caller provides a mavlink_message_t which is the size of a full mavlink message. This is usually the receive buffer for the channel, and allows a reply to an @@ -320,10 +408,10 @@ static inline void mavlink_msg_autopilot_version_send_buf(mavlink_message_t *msg packet->board_version = board_version; packet->vendor_id = vendor_id; packet->product_id = product_id; - mav_array_memcpy(packet->flight_custom_version, flight_custom_version, sizeof(uint8_t)*8); - mav_array_memcpy(packet->middleware_custom_version, middleware_custom_version, sizeof(uint8_t)*8); - mav_array_memcpy(packet->os_custom_version, os_custom_version, sizeof(uint8_t)*8); - mav_array_memcpy(packet->uid2, uid2, sizeof(uint8_t)*18); + mav_array_assign_uint8_t(packet->flight_custom_version, flight_custom_version, 8); + mav_array_assign_uint8_t(packet->middleware_custom_version, middleware_custom_version, 8); + mav_array_assign_uint8_t(packet->os_custom_version, os_custom_version, 8); + mav_array_assign_uint8_t(packet->uid2, uid2, 18); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, (const char *)packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); #endif } @@ -347,7 +435,9 @@ static inline uint64_t mavlink_msg_autopilot_version_get_capabilities(const mavl /** * @brief Get field flight_sw_version from autopilot_version message * - * @return Firmware version number + * @return Firmware version number. + The field must be encoded as 4 bytes, where each byte (shown from MSB to LSB) is part of a semantic version: (major) (minor) (patch) (FIRMWARE_VERSION_TYPE). + */ static inline uint32_t mavlink_msg_autopilot_version_get_flight_sw_version(const mavlink_message_t* msg) { @@ -377,7 +467,7 @@ static inline uint32_t mavlink_msg_autopilot_version_get_os_sw_version(const mav /** * @brief Get field board_version from autopilot_version message * - * @return HW / board version (last 8 bytes should be silicon ID, if any) + * @return HW / board version (last 8 bits should be silicon ID, if any). The first 16 bits of this field specify a board type from an enumeration stored at https://github.com/PX4/PX4-Bootloader/blob/master/board_types.txt and with extensive additions at https://github.com/ArduPilot/ardupilot/blob/master/Tools/AP_Bootloader/board_types.txt */ static inline uint32_t mavlink_msg_autopilot_version_get_board_version(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/standard/standard.h b/lib/main/MAVLink/standard/standard.h new file mode 100644 index 00000000000..cbc52bb4e1b --- /dev/null +++ b/lib/main/MAVLink/standard/standard.h @@ -0,0 +1,129 @@ +/** @file + * @brief MAVLink comm protocol generated from standard.xml + * @see http://mavlink.org + */ +#pragma once +#ifndef MAVLINK_STANDARD_H +#define MAVLINK_STANDARD_H + +#ifndef MAVLINK_H + #error Wrong include order: MAVLINK_STANDARD.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call. +#endif + +#define MAVLINK_STANDARD_XML_HASH 2116097011577996616 + +#ifdef __cplusplus +extern "C" { +#endif + +// MESSAGE LENGTHS AND CRCS + +#ifndef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS {} +#endif + +#ifndef MAVLINK_MESSAGE_CRCS +#define MAVLINK_MESSAGE_CRCS {{0, 50, 9, 9, 0, 0, 0}, {148, 178, 60, 78, 0, 0, 0}, {300, 217, 22, 22, 0, 0, 0}} +#endif + +#include "../protocol.h" + +#define MAVLINK_ENABLED_STANDARD + +// ENUM DEFINITIONS + + +/** @brief Enum used to indicate true or false (also: success or failure, enabled or disabled, active or inactive). */ +#ifndef HAVE_ENUM_MAV_BOOL +#define HAVE_ENUM_MAV_BOOL +typedef enum MAV_BOOL +{ + MAV_BOOL_FALSE=0, /* False. | */ + MAV_BOOL_TRUE=1, /* True. | */ + MAV_BOOL_ENUM_END=2, /* | */ +} MAV_BOOL; +#endif + +/** @brief Bitmask of (optional) autopilot capabilities (64 bit). If a bit is set, the autopilot supports this capability. */ +#ifndef HAVE_ENUM_MAV_PROTOCOL_CAPABILITY +#define HAVE_ENUM_MAV_PROTOCOL_CAPABILITY +typedef enum MAV_PROTOCOL_CAPABILITY +{ + MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT=1, /* Autopilot supports the MISSION_ITEM float message type. + Note that MISSION_ITEM is deprecated, and autopilots should use MISSION_INT instead. + | */ + MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT=2, /* Autopilot supports the new param float message type. | */ + MAV_PROTOCOL_CAPABILITY_MISSION_INT=4, /* Autopilot supports MISSION_ITEM_INT scaled integer message type. + Note that this flag must always be set if missions are supported, because missions must always use MISSION_ITEM_INT (rather than MISSION_ITEM, which is deprecated). + | */ + MAV_PROTOCOL_CAPABILITY_COMMAND_INT=8, /* Autopilot supports COMMAND_INT scaled integer message type. | */ + MAV_PROTOCOL_CAPABILITY_PARAM_ENCODE_BYTEWISE=16, /* Parameter protocol uses byte-wise encoding of parameter values into param_value (float) fields: https://mavlink.io/en/services/parameter.html#parameter-encoding. + Note that either this flag or MAV_PROTOCOL_CAPABILITY_PARAM_ENCODE_C_CAST should be set if the parameter protocol is supported. + | */ + MAV_PROTOCOL_CAPABILITY_FTP=32, /* Autopilot supports the File Transfer Protocol v1: https://mavlink.io/en/services/ftp.html. | */ + MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET=64, /* Autopilot supports commanding attitude offboard. | */ + MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED=128, /* Autopilot supports commanding position and velocity targets in local NED frame. | */ + MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT=256, /* Autopilot supports commanding position and velocity targets in global scaled integers. | */ + MAV_PROTOCOL_CAPABILITY_TERRAIN=512, /* Autopilot supports terrain protocol / data handling. | */ + MAV_PROTOCOL_CAPABILITY_RESERVED3=1024, /* Reserved for future use. | */ + MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION=2048, /* Autopilot supports the MAV_CMD_DO_FLIGHTTERMINATION command (flight termination). | */ + MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION=4096, /* Autopilot supports onboard compass calibration. | */ + MAV_PROTOCOL_CAPABILITY_MAVLINK2=8192, /* Autopilot supports MAVLink version 2. | */ + MAV_PROTOCOL_CAPABILITY_MISSION_FENCE=16384, /* Autopilot supports mission fence protocol. | */ + MAV_PROTOCOL_CAPABILITY_MISSION_RALLY=32768, /* Autopilot supports mission rally point protocol. | */ + MAV_PROTOCOL_CAPABILITY_RESERVED2=65536, /* Reserved for future use. | */ + MAV_PROTOCOL_CAPABILITY_PARAM_ENCODE_C_CAST=131072, /* Parameter protocol uses C-cast of parameter values to set the param_value (float) fields: https://mavlink.io/en/services/parameter.html#parameter-encoding. + Note that either this flag or MAV_PROTOCOL_CAPABILITY_PARAM_ENCODE_BYTEWISE should be set if the parameter protocol is supported. + | */ + MAV_PROTOCOL_CAPABILITY_COMPONENT_IMPLEMENTS_GIMBAL_MANAGER=262144, /* This component implements/is a gimbal manager. This means the GIMBAL_MANAGER_INFORMATION, and other messages can be requested. + | */ + MAV_PROTOCOL_CAPABILITY_COMPONENT_ACCEPTS_GCS_CONTROL=524288, /* Component supports locking control to a particular GCS independent of its system (via MAV_CMD_REQUEST_OPERATOR_CONTROL). | */ + MAV_PROTOCOL_CAPABILITY_GRIPPER=1048576, /* Autopilot has a connected gripper. MAVLink Grippers would set MAV_TYPE_GRIPPER instead. | */ + MAV_PROTOCOL_CAPABILITY_ENUM_END=1048577, /* | */ +} MAV_PROTOCOL_CAPABILITY; +#endif + +/** @brief These values define the type of firmware release. These values indicate the first version or release of this type. For example the first alpha release would be 64, the second would be 65. */ +#ifndef HAVE_ENUM_FIRMWARE_VERSION_TYPE +#define HAVE_ENUM_FIRMWARE_VERSION_TYPE +typedef enum FIRMWARE_VERSION_TYPE +{ + FIRMWARE_VERSION_TYPE_DEV=0, /* development release | */ + FIRMWARE_VERSION_TYPE_ALPHA=64, /* alpha release | */ + FIRMWARE_VERSION_TYPE_BETA=128, /* beta release | */ + FIRMWARE_VERSION_TYPE_RC=192, /* release candidate | */ + FIRMWARE_VERSION_TYPE_OFFICIAL=255, /* official stable release | */ + FIRMWARE_VERSION_TYPE_ENUM_END=256, /* | */ +} FIRMWARE_VERSION_TYPE; +#endif + +// MAVLINK VERSION + +#ifndef MAVLINK_VERSION +#define MAVLINK_VERSION 2 +#endif + +#if (MAVLINK_VERSION == 0) +#undef MAVLINK_VERSION +#define MAVLINK_VERSION 2 +#endif + +// MESSAGE DEFINITIONS +#include "./mavlink_msg_autopilot_version.h" + +// base include +#include "../minimal/minimal.h" + + +#if MAVLINK_STANDARD_XML_HASH == MAVLINK_PRIMARY_XML_HASH +# define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION, MAVLINK_MESSAGE_INFO_PROTOCOL_VERSION} +# define MAVLINK_MESSAGE_NAMES {{ "AUTOPILOT_VERSION", 148 }, { "HEARTBEAT", 0 }, { "PROTOCOL_VERSION", 300 }} +# if MAVLINK_COMMAND_24BIT +# include "../mavlink_get_info.h" +# endif +#endif + +#ifdef __cplusplus +} +#endif // __cplusplus +#endif // MAVLINK_STANDARD_H diff --git a/lib/main/MAVLink/standard/testsuite.h b/lib/main/MAVLink/standard/testsuite.h new file mode 100644 index 00000000000..b7dad4178b0 --- /dev/null +++ b/lib/main/MAVLink/standard/testsuite.h @@ -0,0 +1,106 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from standard.xml + * @see https://mavlink.io/en/ + */ +#pragma once +#ifndef STANDARD_TESTSUITE_H +#define STANDARD_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAVLINK_TEST_ALL +#define MAVLINK_TEST_ALL +static void mavlink_test_minimal(uint8_t, uint8_t, mavlink_message_t *last_msg); +static void mavlink_test_standard(uint8_t, uint8_t, mavlink_message_t *last_msg); + +static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_test_minimal(system_id, component_id, last_msg); + mavlink_test_standard(system_id, component_id, last_msg); +} +#endif + +#include "../minimal/testsuite.h" + + +static void mavlink_test_autopilot_version(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0); + if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_AUTOPILOT_VERSION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_autopilot_version_t packet_in = { + 93372036854775807ULL,93372036854776311ULL,963498296,963498504,963498712,963498920,18899,19003,{ 113, 114, 115, 116, 117, 118, 119, 120 },{ 137, 138, 139, 140, 141, 142, 143, 144 },{ 161, 162, 163, 164, 165, 166, 167, 168 },{ 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202 } + }; + mavlink_autopilot_version_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.capabilities = packet_in.capabilities; + packet1.uid = packet_in.uid; + packet1.flight_sw_version = packet_in.flight_sw_version; + packet1.middleware_sw_version = packet_in.middleware_sw_version; + packet1.os_sw_version = packet_in.os_sw_version; + packet1.board_version = packet_in.board_version; + packet1.vendor_id = packet_in.vendor_id; + packet1.product_id = packet_in.product_id; + + mav_array_memcpy(packet1.flight_custom_version, packet_in.flight_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet1.middleware_custom_version, packet_in.middleware_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet1.os_custom_version, packet_in.os_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet1.uid2, packet_in.uid2, sizeof(uint8_t)*18); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_autopilot_version_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_autopilot_version_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_autopilot_version_pack(system_id, component_id, &msg , packet1.capabilities , packet1.flight_sw_version , packet1.middleware_sw_version , packet1.os_sw_version , packet1.board_version , packet1.flight_custom_version , packet1.middleware_custom_version , packet1.os_custom_version , packet1.vendor_id , packet1.product_id , packet1.uid , packet1.uid2 ); + mavlink_msg_autopilot_version_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_autopilot_version_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.capabilities , packet1.flight_sw_version , packet1.middleware_sw_version , packet1.os_sw_version , packet1.board_version , packet1.flight_custom_version , packet1.middleware_custom_version , packet1.os_custom_version , packet1.vendor_id , packet1.product_id , packet1.uid , packet1.uid2 ); + mavlink_msg_autopilot_version_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i INAV no longer accepts targets based on STM32 F411 MCU. -> INAV 7 was the last INAV official release available for F411 based flight controllers. INAV 8 is not officially available for F411 boards. +> INAV 7 was the last INAV official release available for F411 based flight controllers. INAV 8 is not officially available for F411 boards and the team has not tested either. Issues that can't be reproduced on other MCUs may not be fixed and the targets for F411 targets may eventually be completelly removed from future releases. # ICM426xx IMUs PSA diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index f1966c45f60..101c7223372 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -152,6 +152,8 @@ main_sources(COMMON_SRC drivers/compass/compass_mpu9250.h drivers/compass/compass_qmc5883l.c drivers/compass/compass_qmc5883l.h + drivers/compass/compass_qmc5883p.c + drivers/compass/compass_qmc5883p.h drivers/compass/compass_rm3100.c drivers/compass/compass_rm3100.h drivers/compass/compass_vcm5883.c @@ -177,8 +179,8 @@ main_sources(COMMON_SRC drivers/flash.h drivers/flash_m25p16.c drivers/flash_m25p16.h - drivers/flash_w25n01g.c - drivers/flash_w25n01g.h + drivers/flash_w25n.c + drivers/flash_w25n.h drivers/gimbal_common.h drivers/gimbal_common.c drivers/headtracker_common.h @@ -278,9 +280,9 @@ main_sources(COMMON_SRC fc/cli.h fc/config.c fc/config.h - fc/controlrate_profile.c - fc/controlrate_profile.h - fc/controlrate_profile_config_struct.h + fc/control_profile.c + fc/control_profile.h + fc/control_profile_config_struct.h fc/fc_core.c fc/fc_core.h fc/fc_init.c diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 4341ea0c34d..1027f25379d 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -48,7 +48,7 @@ #include "drivers/pwm_output.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/fc_core.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" @@ -99,13 +99,14 @@ #define BLACKBOX_INVERTED_CARD_DETECTION 0 #endif -PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 4); PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, .device = DEFAULT_BLACKBOX_DEVICE, .rate_num = SETTING_BLACKBOX_RATE_NUM_DEFAULT, .rate_denom = SETTING_BLACKBOX_RATE_DENOM_DEFAULT, .invertedCardDetection = BLACKBOX_INVERTED_CARD_DETECTION, + .arm_control = SETTING_BLACKBOX_ARM_CONTROL_DEFAULT, .includeFlags = BLACKBOX_FEATURE_NAV_PID | BLACKBOX_FEATURE_NAV_POS | BLACKBOX_FEATURE_MAG | BLACKBOX_FEATURE_ACC | BLACKBOX_FEATURE_ATTITUDE | BLACKBOX_FEATURE_RC_DATA | BLACKBOX_FEATURE_RC_COMMAND | @@ -469,21 +470,6 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { #endif }; -typedef enum BlackboxState { - BLACKBOX_STATE_DISABLED = 0, - BLACKBOX_STATE_STOPPED, - BLACKBOX_STATE_PREPARE_LOG_FILE, - BLACKBOX_STATE_SEND_HEADER, - BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER, - BLACKBOX_STATE_SEND_GPS_H_HEADER, - BLACKBOX_STATE_SEND_GPS_G_HEADER, - BLACKBOX_STATE_SEND_SLOW_HEADER, - BLACKBOX_STATE_SEND_SYSINFO, - BLACKBOX_STATE_PAUSED, - BLACKBOX_STATE_RUNNING, - BLACKBOX_STATE_SHUTTING_DOWN -} BlackboxState; - #define BLACKBOX_FIRST_HEADER_SENDING_STATE BLACKBOX_STATE_SEND_HEADER #define BLACKBOX_LAST_HEADER_SENDING_STATE BLACKBOX_STATE_SEND_SYSINFO @@ -1741,9 +1727,10 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->rssi = getRSSI(); + const uint8_t minServoIndex = getMinServoIndex(); const int servoCount = getServoCount(); for (int i = 0; i < servoCount; i++) { - blackboxCurrent->servo[i] = servo[i]; + blackboxCurrent->servo[i] = servo[i + minServoIndex]; } blackboxCurrent->navState = navCurrentState; @@ -1947,15 +1934,15 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", getLooptime()); BLACKBOX_PRINT_HEADER_LINE("rc_rate", "%d", 100); //For compatibility reasons write rc_rate 100 - BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlRateProfile->stabilized.rcExpo8); - BLACKBOX_PRINT_HEADER_LINE("rc_yaw_expo", "%d", currentControlRateProfile->stabilized.rcYawExpo8); - BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlRateProfile->throttle.rcMid8); - BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlRateProfile->throttle.rcExpo8); - BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlRateProfile->throttle.dynPID); - BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlRateProfile->throttle.pa_breakpoint); - BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlRateProfile->stabilized.rates[ROLL], - currentControlRateProfile->stabilized.rates[PITCH], - currentControlRateProfile->stabilized.rates[YAW]); + BLACKBOX_PRINT_HEADER_LINE("rc_expo", "%d", currentControlProfile->stabilized.rcExpo8); + BLACKBOX_PRINT_HEADER_LINE("rc_yaw_expo", "%d", currentControlProfile->stabilized.rcYawExpo8); + BLACKBOX_PRINT_HEADER_LINE("thr_mid", "%d", currentControlProfile->throttle.rcMid8); + BLACKBOX_PRINT_HEADER_LINE("thr_expo", "%d", currentControlProfile->throttle.rcExpo8); + BLACKBOX_PRINT_HEADER_LINE("tpa_rate", "%d", currentControlProfile->throttle.dynPID); + BLACKBOX_PRINT_HEADER_LINE("tpa_breakpoint", "%d", currentControlProfile->throttle.pa_breakpoint); + BLACKBOX_PRINT_HEADER_LINE("rates", "%d,%d,%d", currentControlProfile->stabilized.rates[ROLL], + currentControlProfile->stabilized.rates[PITCH], + currentControlProfile->stabilized.rates[YAW]); BLACKBOX_PRINT_HEADER_LINE("rollPID", "%d,%d,%d,%d", pidBank()->pid[PID_ROLL].P, pidBank()->pid[PID_ROLL].I, pidBank()->pid[PID_ROLL].D, @@ -2324,6 +2311,11 @@ static bool canUseBlackboxWithCurrentConfiguration(void) return feature(FEATURE_BLACKBOX); } +BlackboxState getBlackboxState(void) +{ + return blackboxState; +} + /** * Call during system startup to initialize the blackbox. */ diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index a329c11df12..1901201fa22 100644 --- a/src/main/blackbox/blackbox.h +++ b/src/main/blackbox/blackbox.h @@ -37,12 +37,29 @@ typedef enum { BLACKBOX_FEATURE_GYRO_PEAKS_YAW = 1 << 12, BLACKBOX_FEATURE_SERVOS = 1 << 13, } blackboxFeatureMask_e; + +typedef enum BlackboxState { + BLACKBOX_STATE_DISABLED = 0, + BLACKBOX_STATE_STOPPED, + BLACKBOX_STATE_PREPARE_LOG_FILE, + BLACKBOX_STATE_SEND_HEADER, + BLACKBOX_STATE_SEND_MAIN_FIELD_HEADER, + BLACKBOX_STATE_SEND_GPS_H_HEADER, + BLACKBOX_STATE_SEND_GPS_G_HEADER, + BLACKBOX_STATE_SEND_SLOW_HEADER, + BLACKBOX_STATE_SEND_SYSINFO, + BLACKBOX_STATE_PAUSED, + BLACKBOX_STATE_RUNNING, + BLACKBOX_STATE_SHUTTING_DOWN +} BlackboxState; + typedef struct blackboxConfig_s { uint16_t rate_num; uint16_t rate_denom; uint8_t device; uint8_t invertedCardDetection; uint32_t includeFlags; + int8_t arm_control; } blackboxConfig_t; PG_DECLARE(blackboxConfig_t, blackboxConfig); @@ -57,3 +74,4 @@ bool blackboxMayEditConfig(void); void blackboxIncludeFlagSet(uint32_t mask); void blackboxIncludeFlagClear(uint32_t mask); bool blackboxIncludeFlag(uint32_t mask); +BlackboxState getBlackboxState(void); diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 1f8eb9bb55e..2747697d222 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -37,7 +37,7 @@ #include "flight/pid.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/rc_controls.h" #include "fc/settings.h" @@ -492,7 +492,7 @@ static const OSD_Entry cmsx_menuImuEntries[] = OSD_SUBMENU_ENTRY("MECHANICS", &cmsx_menuMechanics), // Rate profile dependent - OSD_UINT8_CALLBACK_ENTRY("RATE PROF", cmsx_profileIndexOnChange, (&(const OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_CONTROL_RATE_PROFILE_COUNT, 1})), + OSD_UINT8_CALLBACK_ENTRY("RATE PROF", cmsx_profileIndexOnChange, (&(const OSD_UINT8_t){ &tmpProfileIndex, 1, MAX_CONTROL_PROFILE_COUNT, 1})), OSD_SUBMENU_ENTRY("RATE", &cmsx_menuRateProfile), OSD_SUBMENU_ENTRY("MANU RATE", &cmsx_menuManualRateProfile), diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index 0e85d0e656d..00918a7112e 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -217,6 +217,8 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("AZIMUTH", OSD_AZIMUTH), OSD_ELEMENT_ENTRY("GRD COURSE", OSD_GROUND_COURSE), OSD_ELEMENT_ENTRY("X TRACK ERR", OSD_CROSS_TRACK_ERROR), + OSD_ELEMENT_ENTRY("ADSB WARNING", OSD_ADSB_WARNING), + OSD_ELEMENT_ENTRY("ADSB INFO", OSD_ADSB_INFO), #endif // GPS OSD_ELEMENT_ENTRY("HEADING", OSD_HEADING), OSD_ELEMENT_ENTRY("HEADING GR.", OSD_HEADING_GRAPH), @@ -224,7 +226,7 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("CRS HLD ADJ", OSD_COURSE_HOLD_ADJUSTMENT), #if defined(USE_BARO) || defined(USE_GPS) OSD_ELEMENT_ENTRY("VARIO", OSD_VARIO), - OSD_ELEMENT_ENTRY("VARIO NUM", OSD_VARIO_NUM), + OSD_ELEMENT_ENTRY("V SPEED", OSD_VERTICAL_SPEED_INDICATOR), #endif // defined OSD_ELEMENT_ENTRY("ALTITUDE", OSD_ALTITUDE), OSD_ELEMENT_ENTRY("ALTITUDE MSL", OSD_ALTITUDE_MSL), diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 2dbb53c0a1d..675ff56c2ca 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -35,49 +35,51 @@ #define RAD (M_PIf / 180.0f) -#define DEGREES_TO_CENTIDEGREES(angle) ((angle) * 100) -#define CENTIDEGREES_TO_DEGREES(angle) ((angle) * 0.01f) +#define DEGREES_TO_CENTIDEGREES(angle) ((angle) * 100) +#define CENTIDEGREES_TO_DEGREES(angle) ((angle) * 0.01f) -#define CENTIDEGREES_TO_DECIDEGREES(angle) ((angle) / 10.0f) -#define DECIDEGREES_TO_CENTIDEGREES(angle) ((angle) * 10) +#define CENTIDEGREES_TO_DECIDEGREES(angle) ((angle) / 10.0f) +#define DECIDEGREES_TO_CENTIDEGREES(angle) ((angle) * 10) -#define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10) -#define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10.0f) +#define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10) +#define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10.0f) -#define DEGREES_PER_DEKADEGREE 10 -#define DEGREES_TO_DEKADEGREES(angle) ((angle) / DEGREES_PER_DEKADEGREE) -#define DEKADEGREES_TO_DEGREES(angle) ((angle) * DEGREES_PER_DEKADEGREE) +#define DEGREES_PER_DEKADEGREE 10 +#define DEGREES_TO_DEKADEGREES(angle) ((angle) / DEGREES_PER_DEKADEGREE) +#define DEKADEGREES_TO_DEGREES(angle) ((angle) * DEGREES_PER_DEKADEGREE) -#define DEGREES_TO_RADIANS(angle) ((angle) * RAD) -#define RADIANS_TO_DEGREES(angle) ((angle) / RAD) -#define DECIDEGREES_TO_RADIANS(angle) (((angle) / 10.0f) * RAD) -#define RADIANS_TO_DECIDEGREES(angle) (((angle) * 10.0f) / RAD) +#define DEGREES_TO_RADIANS(angle) ((angle) * RAD) +#define RADIANS_TO_DEGREES(angle) ((angle) / RAD) +#define DECIDEGREES_TO_RADIANS(angle) (((angle) / 10.0f) * RAD) +#define RADIANS_TO_DECIDEGREES(angle) (((angle) * 10.0f) / RAD) -#define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD) -#define CENTIDEGREES_TO_RADIANS(angle) (((angle) * 0.01f) * RAD) +#define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD) +#define CENTIDEGREES_TO_RADIANS(angle) (((angle) * 0.01f) * RAD) -#define MILLIMETERS_TO_CENTIMETERS(mm) (mm / 10.0f) +#define MILLIMETERS_TO_CENTIMETERS(mm) (mm / 10.0f) -#define CENTIMETERS_TO_CENTIFEET(cm) (cm / 0.3048f) -#define CENTIMETERS_TO_FEET(cm) (cm / 30.48f) -#define CENTIMETERS_TO_METERS(cm) (cm * 0.01f) +#define CENTIMETERS_TO_CENTIFEET(cm) (cm / 0.3048f) +#define CENTIMETERS_TO_FEET(cm) (cm / 30.48f) +#define CENTIMETERS_TO_METERS(cm) (cm * 0.01f) -#define METERS_TO_CENTIMETERS(m) (m * 100) -#define METERS_TO_KILOMETERS(m) (m / 1000.0f) -#define METERS_TO_MILES(m) (m / 1609.344f) -#define METERS_TO_NAUTICALMILES(m) (m / 1852.00f) +#define METERS_TO_CENTIMETERS(m) (m * 100) +#define METERS_TO_KILOMETERS(m) (m / 1000.0f) +#define METERS_TO_MILES(m) (m / 1609.344f) +#define METERS_TO_NAUTICALMILES(m) (m / 1852.00f) -#define MWH_TO_WH(mWh) (mWh / 1000.0f) +#define MWH_TO_WH(mWh) (mWh / 1000.0f) -#define CMSEC_TO_CENTIMPH(cms) (cms * 2.2369363f) -#define CMSEC_TO_CENTIKPH(cms) (cms * 3.6f) -#define CMSEC_TO_CENTIKNOTS(cms) (cms * 1.943845f) +#define CMSEC_TO_CENTIMPH(cms) (cms * 2.2369363f) +#define CMSEC_TO_CENTIKPH(cms) (cms * 3.6f) +#define CMSEC_TO_CENTIKNOTS(cms) (cms * 1.943845f) -#define CMSEC_TO_MPH(cms) (CMSEC_TO_CENTIMPH(cms) / 100.0f) -#define CMSEC_TO_KPH(cms) (CMSEC_TO_CENTIKPH(cms) / 100.0f) -#define CMSEC_TO_KNOTS(cms) (CMSEC_TO_CENTIKNOTS(cms) / 100.0f) +#define CMSEC_TO_MPH(cms) (CMSEC_TO_CENTIMPH(cms) / 100.0f) +#define CMSEC_TO_KPH(cms) (CMSEC_TO_CENTIKPH(cms) / 100.0f) +#define CMSEC_TO_KNOTS(cms) (CMSEC_TO_CENTIKNOTS(cms) / 100.0f) -#define C_TO_KELVIN(temp) (temp + 273.15f) +#define KMH_TO_MS(kmh) (kmh / 3.6f) + +#define C_TO_KELVIN(temp) (temp + 273.15f) // Standard Sea Level values // Ref:https://en.wikipedia.org/wiki/Standard_sea_level diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 26715ab3cec..2acb9c8172e 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -28,7 +28,7 @@ //#define PG_SENSOR_TRIMS 9 -- NOT USED in INAV #define PG_GYRO_CONFIG 10 #define PG_BATTERY_PROFILES 11 -#define PG_CONTROL_RATE_PROFILES 12 +#define PG_CONTROL_PROFILES 12 #define PG_SERIAL_CONFIG 13 #define PG_PID_PROFILE 14 //#define PG_GTUNE_CONFIG 15 diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 6c2cedaff02..5a3e6dba453 100644 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -107,6 +107,7 @@ typedef enum { DEVHW_IST8310_1, DEVHW_IST8308, DEVHW_QMC5883, + DEVHW_QMC5883P, DEVHW_MAG3110, DEVHW_LIS3MDL, DEVHW_RM3100, @@ -141,7 +142,7 @@ typedef enum { DEVHW_MS4525, // Pitot meter DEVHW_DLVR, // Pitot meter DEVHW_M25P16, // SPI NOR flash - DEVHW_W25N01G, // SPI 128MB flash + DEVHW_W25N, // SPI 128MB or 256MB flash from Winbond W25N family DEVHW_UG2864, // I2C OLED display DEVHW_SDCARD, // Generic SD-Card DEVHW_IRLOCK, // IR-Lock visual positioning hardware diff --git a/src/main/drivers/compass/compass_qmc5883l.c b/src/main/drivers/compass/compass_qmc5883l.c index 381bb3b48d5..aca7b1ef995 100644 --- a/src/main/drivers/compass/compass_qmc5883l.c +++ b/src/main/drivers/compass/compass_qmc5883l.c @@ -103,7 +103,7 @@ static bool qmc5883Read(magDev_t * mag) mag->magADCRaw[Z] = 0; bool ack = busRead(mag->busDev, QMC5883L_REG_STATUS, &status); - if (!ack || (status & 0x04) == 0) { + if (!ack || (status & 0x01) == 0) { return false; } diff --git a/src/main/drivers/compass/compass_qmc5883p.c b/src/main/drivers/compass/compass_qmc5883p.c new file mode 100644 index 00000000000..e5c21afb74a --- /dev/null +++ b/src/main/drivers/compass/compass_qmc5883p.c @@ -0,0 +1,203 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 http://www.gnu.org/licenses/. + */ + +#include +#include + +#include + +#include "platform.h" + +#ifdef USE_MAG_QMC5883P + +#include "build/build_config.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/time.h" +#include "drivers/bus_i2c.h" + +#include "sensors/boardalignment.h" +#include "sensors/sensors.h" + +#include "drivers/sensor.h" +#include "drivers/compass/compass.h" + +#include "drivers/compass/compass_qmc5883p.h" + +#define QMC5883P_MAG_I2C_ADDRESS 0x2C + +#define QMC5883P_REG_ID 0x00 +#define QMC5883P_ID_VAL 0x80 + +#define QMC5883P_REG_DATA_OUTPUT_X 0x01 +#define QMC5883P_DATA_BYTES 6 + +#define QMC5883P_REG_CONF1 0x0A + +#define QMC5883P_CONF1_MODE_SUSPEND 0x00 +#define QMC5883P_CONF1_MODE_NORMAL 0x01 +#define QMC5883P_CONF1_MODE_SINGLE 0x02 +#define QMC5883P_CONF1_MODE_CONTINUOUS 0x03 + +#define QMC5883P_CONF1_ODR_10HZ (0x00 << 2) +#define QMC5883P_CONF1_ODR_50HZ (0x01 << 2) +#define QMC5883P_CONF1_ODR_100HZ (0x02 << 2) +#define QMC5883P_CONF1_ODR_200HZ (0x03 << 2) + +#define QMC5883P_CONF1_OSR1_8 (0x00 << 4) +#define QMC5883P_CONF1_OSR1_4 (0x01 << 4) +#define QMC5883P_CONF1_OSR1_2 (0x02 << 4) +#define QMC5883P_CONF1_OSR1_1 (0x03 << 4) + +#define QMC5883P_CONF1_OSR2_1 (0x00 << 6) +#define QMC5883P_CONF1_OSR2_2 (0x01 << 6) +#define QMC5883P_CONF1_OSR2_4 (0x02 << 6) +#define QMC5883P_CONF1_OSR2_8 (0x03 << 6) + + +#define QMC5883P_REG_CONF2 0x0B + +#define QMC5883P_CONF2_SET_RESET_ON 0x00 +#define QMC5883P_CONF2_SET_ON_RESET_OFF 0x01 +#define QMC5883P_CONF2_SET_RESET_OFF 0x02 + +#define QMC5883P_CONF2_RNG_30G (0x00 << 2) +#define QMC5883P_CONF2_RNG_12G (0x01 << 2) +#define QMC5883P_CONF2_RNG_8G (0x02 << 2) +#define QMC5883P_CONF2_RNG_2G (0x03 << 2) + +#define QMC5883P_CONF2_SELF_TEST 0x40 + +#define QMC5883P_CONF2_RESET 0x80 + + +#define QMC5883P_REG_STATUS 0x09 +#define QMC5883P_STATUS_DRDY_MASK 0x01 +#define QMC5883P_STATUS_OVFL_MASK 0x02 + +// This register has no definition in the datasheet and only mentioned in an example +#define QMC5883P_REG_DATA_SIGN 0x29 +#define QMC5883P_DATA_SIGN_MAGIC_VALUE 0x06 + +static bool qmc5883pInit(magDev_t * mag) +{ + bool ack = true; + + ack = ack && busWrite(mag->busDev, QMC5883P_REG_CONF2, QMC5883P_CONF2_RESET); + + delay(30); + + ack = ack && busWrite(mag->busDev, QMC5883P_REG_DATA_SIGN, QMC5883P_DATA_SIGN_MAGIC_VALUE); + ack = ack && busWrite(mag->busDev, QMC5883P_REG_CONF2, QMC5883P_CONF2_RNG_8G); + ack = ack && busWrite(mag->busDev, + QMC5883P_REG_CONF1, + QMC5883P_CONF1_MODE_CONTINUOUS | + QMC5883P_CONF1_ODR_200HZ | + QMC5883P_CONF1_OSR1_8 | + QMC5883P_CONF1_OSR2_8); + + return ack; +} + +static bool qmc5883pRead(magDev_t * mag) +{ + uint8_t status; + uint8_t buf[QMC5883P_DATA_BYTES]; + + // set magData to zero for case of failed read + mag->magADCRaw[X] = 0; + mag->magADCRaw[Y] = 0; + mag->magADCRaw[Z] = 0; + + bool ack = busRead(mag->busDev, QMC5883P_REG_STATUS, &status); + if (!ack || (status & QMC5883P_STATUS_DRDY_MASK) == 0) { + return false; + } + + ack = busReadBuf(mag->busDev, QMC5883P_REG_DATA_OUTPUT_X, buf, QMC5883P_DATA_BYTES); + if (!ack) { + return false; + } + + /* + Initially, this sensor provided the data like this: + mag->magADCRaw[X] = (int16_t)(buf[1] << 8 | buf[0]); + mag->magADCRaw[Y] = (int16_t)(buf[3] << 8 | buf[2]); + mag->magADCRaw[Z] = (int16_t)(buf[5] << 8 | buf[4]); + + But QMC5883P has a different reference point and axis directions, compared to QMC5883L. + As QMC5883P is designed to be a drop-in replacement for QMC5883L, apply alignment at + readout to obtain the same readings in the same position. In particular, it does + the same transformation to the data, as the CW270_DEG_FLIP option: + dest[X] = -y; + dest[Y] = -x; + dest[Z] = -z; + */ + mag->magADCRaw[X] = -(int16_t)(buf[3] << 8 | buf[2]); + mag->magADCRaw[Y] = -(int16_t)(buf[1] << 8 | buf[0]); + mag->magADCRaw[Z] = -(int16_t)(buf[5] << 8 | buf[4]); + + return true; +} + +#define DETECTION_MAX_RETRY_COUNT 5 +static bool deviceDetect(magDev_t * mag) +{ + for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) { + // Must write reset first - don't care about the result + busWrite(mag->busDev, QMC5883P_REG_CONF2, QMC5883P_CONF2_RESET); + delay(30); + + uint8_t sig = 0; + bool ack = busRead(mag->busDev, QMC5883P_REG_ID, &sig); + + if (ack && sig == QMC5883P_ID_VAL) { + return true; + } + } + + return false; +} + +bool qmc5883pDetect(magDev_t * mag) +{ + mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_QMC5883P, mag->magSensorToUse, OWNER_COMPASS); + if (mag->busDev == NULL) { + return false; + } + + if (!deviceDetect(mag)) { + busDeviceDeInit(mag->busDev); + return false; + } + + mag->init = qmc5883pInit; + mag->read = qmc5883pRead; + + return true; +} +#endif diff --git a/src/main/drivers/compass/compass_qmc5883p.h b/src/main/drivers/compass/compass_qmc5883p.h new file mode 100644 index 00000000000..9f20c183c89 --- /dev/null +++ b/src/main/drivers/compass/compass_qmc5883p.h @@ -0,0 +1,27 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 http://www.gnu.org/licenses/. + */ + +#pragma once + +bool qmc5883pDetect(magDev_t *mag); diff --git a/src/main/drivers/flash.c b/src/main/drivers/flash.c index 145aeafd56b..392c7680519 100644 --- a/src/main/drivers/flash.c +++ b/src/main/drivers/flash.c @@ -30,7 +30,7 @@ #include "flash.h" #include "flash_m25p16.h" -#include "flash_w25n01g.h" +#include "flash_w25n.h" #include "common/time.h" @@ -56,17 +56,17 @@ static flashDriver_t flashDrivers[] = { }, #endif -#ifdef USE_FLASH_W25N01G +#if defined(USE_FLASH_W25N01G) || defined(USE_FLASH_W25N02K) { - .init = w25n01g_init, - .isReady = w25n01g_isReady, - .waitForReady = w25n01g_waitForReady, - .eraseSector = w25n01g_eraseSector, - .eraseCompletely = w25n01g_eraseCompletely, - .pageProgram = w25n01g_pageProgram, - .readBytes = w25n01g_readBytes, - .getGeometry = w25n01g_getGeometry, - .flush = w25n01g_flush + .init = w25n_init, + .isReady = w25n_isReady, + .waitForReady = w25n_waitForReady, + .eraseSector = w25n_eraseSector, + .eraseCompletely = w25n_eraseCompletely, + .pageProgram = w25n_pageProgram, + .readBytes = w25n_readBytes, + .getGeometry = w25n_getGeometry, + .flush = w25n_flush }, #endif diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c index 9842a80995d..2db61a3a166 100644 --- a/src/main/drivers/flash_m25p16.c +++ b/src/main/drivers/flash_m25p16.c @@ -99,6 +99,9 @@ struct { // Winbond W25Q128_DTR // Datasheet: https://www.winbond.com/resource-files/w25q128jv%20dtr%20revb%2011042016.pdf {0xEF7018, 256, 256}, + // Puya PY25Q128HA + // Datasheet: https://www.puyasemi.com/cpzx3/info_271_itemid_87.html + {0x856018, 256, 256}, // Winbond W25Q256 // Datasheet: https://www.winbond.com/resource-files/w25q256jv%20spi%20revb%2009202016.pdf {0xEF4019, 512, 256}, diff --git a/src/main/drivers/flash_w25n.c b/src/main/drivers/flash_w25n.c new file mode 100644 index 00000000000..2791d3d89e1 --- /dev/null +++ b/src/main/drivers/flash_w25n.c @@ -0,0 +1,555 @@ +/* + * This file is part of INAV. + * + * INAV are free software. You can redistribute + * this software and/or modify this software 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. + * + * INAV are distributed in the hope that they + * 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 software. + * + * If not, see . + */ + +#include +#include + +#include "platform.h" + +#if defined(USE_FLASH_W25N01G) || defined(USE_FLASH_W25N02K) + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/time.h" + +#include "flash_w25n.h" + +// Device size parameters +#define W25N_PAGE_SIZE 2048 +#define W25N_PAGES_PER_BLOCK 64 + +#define W25N01GV_BLOCKS_PER_DIE 1024 +#define W25N02KV_BLOCKS_PER_DIE 2048 + + + +// BB replacement area +#define W25N_BB_MARKER_BLOCKS 1 +#define W25N_BB_REPLACEMENT_BLOCKS 20 +#define W25N_BB_MANAGEMENT_BLOCKS (W25N_BB_REPLACEMENT_BLOCKS + W25N_BB_MARKER_BLOCKS) + +// blocks are zero-based index +#define W25N_BB_REPLACEMENT_START_BLOCK (W25N_BLOCKS_PER_DIE - W25N_BB_REPLACEMENT_BLOCKS) +#define W25N_BB_MANAGEMENT_START_BLOCK (W25N_BLOCKS_PER_DIE - W25N_BB_MANAGEMENT_BLOCKS) +#define W25N_BB_MARKER_BLOCK (W25N_BB_REPLACEMENT_START_BLOCK - W25N_BB_MARKER_BLOCKS) + +// Instructions +#define W25N_INSTRUCTION_RDID 0x9F +#define W25N_INSTRUCTION_DEVICE_RESET 0xFF +#define W25N_INSTRUCTION_READ_STATUS_REG 0x05 +#define W25N_INSTRUCTION_READ_STATUS_ALTERNATE_REG 0x0F +#define W25N_INSTRUCTION_WRITE_STATUS_REG 0x01 +#define W25N_INSTRUCTION_WRITE_STATUS_ALTERNATE_REG 0x1F +#define W25N_INSTRUCTION_WRITE_ENABLE 0x06 +#define W25N_INSTRUCTION_DIE_SELECT 0xC2 +#define W25N_INSTRUCTION_BLOCK_ERASE 0xD8 +#define W25N_INSTRUCTION_READ_BBM_LUT 0xA5 +#define W25N_INSTRUCTION_BB_MANAGEMENT 0xA1 +#define W25N_INSTRUCTION_PROGRAM_DATA_LOAD 0x02 +#define W25N_INSTRUCTION_RANDOM_PROGRAM_DATA_LOAD 0x84 +#define W25N_INSTRUCTION_PROGRAM_EXECUTE 0x10 +#define W25N_INSTRUCTION_PAGE_DATA_READ 0x13 +#define W25N_INSTRUCTION_READ_DATA 0x03 +#define W25N_INSTRUCTION_FAST_READ 0x1B +#define W25N_INSTRUCTION_FAST_READ_QUAD_OUTPUT 0x6B + +// Config/status register addresses +#define W25N_PROT_REG 0xA0 +#define W25N_CONF_REG 0xB0 +#define W25N_STAT_REG 0xC0 + +// Bits in config/status register 1 (W25N_PROT_REG) +#define W25N_PROT_CLEAR (0) +#define W25N_PROT_SRP1_ENABLE (1 << 0) +#define W25N_PROT_WP_E_ENABLE (1 << 1) +#define W25N_PROT_TB_ENABLE (1 << 2) +#define W25N_PROT_PB0_ENABLE (1 << 3) +#define W25N_PROT_PB1_ENABLE (1 << 4) +#define W25N_PROT_PB2_ENABLE (1 << 5) +#define W25N_PROT_PB3_ENABLE (1 << 6) +#define W25N_PROT_SRP2_ENABLE (1 << 7) + +// Bits in config/status register 2 (W25N_CONF_REG) +#define W25N_CONFIG_ECC_ENABLE (1 << 4) +#define W25N_CONFIG_BUFFER_READ_MODE (1 << 3) + +// Bits in config/status register 3 (W25N_STATREG) +#define W25N_STATUS_BBM_LUT_FULL (1 << 6) +#define W25N_STATUS_FLAG_ECC_POS 4 +#define W25N_STATUS_FLAG_ECC_MASK ((1 << 5)|(1 << 4)) +#define W25N_STATUS_FLAG_ECC(status) (((status) & W25N_STATUS_FLAG_ECC_MASK) >> 4) +#define W25N_STATUS_PROGRAM_FAIL (1 << 3) +#define W25N_STATUS_ERASE_FAIL (1 << 2) +#define W25N_STATUS_FLAG_WRITE_ENABLED (1 << 1) +#define W25N_STATUS_FLAG_BUSY (1 << 0) +#define W25N_BBLUT_TABLE_ENTRY_COUNT 20 +#define W25N_BBLUT_TABLE_ENTRY_SIZE 4 // in bytes + +// Bits in LBA for BB LUT +#define W25N_BBLUT_STATUS_ENABLED (1 << 15) +#define W25N_BBLUT_STATUS_INVALID (1 << 14) +#define W25N_BBLUT_STATUS_MASK (W25N_BBLUT_STATUS_ENABLED | W25N_BBLUT_STATUS_INVALID) + +// Some useful defs and macros +#define W25N_LINEAR_TO_COLUMN(laddr) ((laddr) % W25N_PAGE_SIZE) +#define W25N_LINEAR_TO_PAGE(laddr) ((laddr) / W25N_PAGE_SIZE) +#define W25N_LINEAR_TO_BLOCK(laddr) (W25N_LINEAR_TO_PAGE(laddr) / W25N_PAGES_PER_BLOCK) +#define W25N_BLOCK_TO_PAGE(block) ((block) * W25N_PAGES_PER_BLOCK) +#define W25N_BLOCK_TO_LINEAR(block) (W25N_BLOCK_TO_PAGE(block) * W25N_PAGE_SIZE) + +// IMPORTANT: Timeout values are currently required to be set to the highest value required by any of the supported flash chips by this driver +// The timeout values (2ms minimum to avoid 1 tick advance in consecutive calls to millis). +#define W25N_TIMEOUT_PAGE_READ_MS 2 // tREmax = 60us (ECC enabled) +#define W25N_TIMEOUT_PAGE_PROGRAM_MS 2 // tPPmax = 700us +#define W25N_TIMEOUT_BLOCK_ERASE_MS 15 // tBEmax = 10ms +#define W25N_TIMEOUT_RESET_MS 500 // tRSTmax = 500ms + +// Sizes (in bits) +#define W28N_STATUS_REGISTER_SIZE 8 +#define W28N_STATUS_PAGE_ADDRESS_SIZE 16 +#define W28N_STATUS_COLUMN_ADDRESS_SIZE 16 + +// JEDEC ID +#define JEDEC_ID_WINBOND_W25N01GV 0xEFAA21 +#define JEDEC_ID_WINBOND_W25N02KV 0xEFAA22 + +static busDevice_t *busDev = NULL; +static flashGeometry_t geometry; + +/* + * Whether we've performed an action that could have made the device busy for writes. + * + * This allows us to avoid polling for writable status when it is definitely ready already. + */ +static bool couldBeBusy = false; + +static timeMs_t timeoutAt = 0; + +static bool w25n_waitForReadyInternal(void); + +static void w25n_setTimeout(timeMs_t timeoutMillis) +{ + timeMs_t now = millis(); + timeoutAt = now + timeoutMillis; + couldBeBusy = true; +} + +/** + * Send the given command byte to the device. + */ +static void w25n_performOneByteCommand(uint8_t command) +{ + busTransfer(busDev, NULL, &command, 1); +} + +static void w25n_performCommandWithPageAddress(uint8_t command, uint32_t pageAddress) +{ + uint8_t cmd[4] = { command, 0, (pageAddress >> 8) & 0xff, (pageAddress >> 0) & 0xff}; + busTransfer(busDev, NULL, cmd, sizeof(cmd)); +} + +static uint8_t w25n_readRegister(uint8_t reg) +{ + uint8_t command[3] = { W25N_INSTRUCTION_READ_STATUS_REG, reg, 0 }; + uint8_t in[3]; + + busTransfer(busDev, in, command, sizeof(command)); + + return in[2]; +} + +static void w25n_writeRegister(uint8_t reg, uint8_t data) +{ + uint8_t cmd[3] = { W25N_INSTRUCTION_WRITE_STATUS_REG, reg, data }; + busTransfer(busDev, NULL, cmd, sizeof(cmd)); +} + +static void w25n_deviceReset(void) +{ + w25n_performOneByteCommand(W25N_INSTRUCTION_DEVICE_RESET); + w25n_setTimeout(W25N_TIMEOUT_RESET_MS); + w25n_waitForReadyInternal(); + // Protection for upper 1/32 (BP[3:0] = 0101, TB=0), WP-E on; to protect bad block replacement area + // DON'T DO THIS. This will prevent writes through the bblut as well. + // w25n_writeRegister(busdev, W25N_PROT_REG, W25N_PROT_PB0_ENABLE|W25N_PROT_PB2_ENABLE|W25N_PROT_WP_E_ENABLE); + // No protection, WP-E off, WP-E prevents use of IO2 + w25n_writeRegister(W25N_PROT_REG, W25N_PROT_CLEAR); + // Buffered read mode (BUF = 1), ECC enabled (ECC = 1) + w25n_writeRegister(W25N_CONF_REG, W25N_CONFIG_ECC_ENABLE | W25N_CONFIG_BUFFER_READ_MODE); +} + +bool w25n_isReady(void) +{ + uint8_t status = w25n_readRegister(W25N_STAT_REG); + + // If couldBeBusy is false, don't bother to poll the flash chip for its status + couldBeBusy = couldBeBusy && ((status & W25N_STATUS_FLAG_BUSY) != 0); + + return !couldBeBusy; +} + +static bool w25n_waitForReadyInternal(void) +{ + while (!w25n_isReady()) { + timeMs_t now = millis(); + if (cmp32(now, timeoutAt) >= 0) { + return false; + } + } + timeoutAt = 0; + return true; +} + +bool w25n_waitForReady(timeMs_t timeoutMillis) +{ + w25n_setTimeout(timeoutMillis); + return w25n_waitForReadyInternal(); +} + +/** + * The flash requires this write enable command to be sent before commands that would cause + * a write like program and erase. + */ +static void w25n_writeEnable(void) +{ + w25n_performOneByteCommand(W25N_INSTRUCTION_WRITE_ENABLE); + + // Assume that we're about to do some writing, so the device is just about to become busy + couldBeBusy = true; +} + +bool w25n_detect(uint32_t chipID) +{ + switch (chipID) { + case JEDEC_ID_WINBOND_W25N01GV: + geometry.sectors = W25N01GV_BLOCKS_PER_DIE; // Blocks + geometry.pagesPerSector = W25N_PAGES_PER_BLOCK; // Pages/Blocks + geometry.pageSize = W25N_PAGE_SIZE; + break; + case JEDEC_ID_WINBOND_W25N02KV: + geometry.sectors = W25N02KV_BLOCKS_PER_DIE; // Blocks + geometry.pagesPerSector = W25N_PAGES_PER_BLOCK; // Pages/Blocks + geometry.pageSize = W25N_PAGE_SIZE; + break; + + default: + // Unsupported chip + geometry.sectors = 0; + geometry.pagesPerSector = 0; + geometry.sectorSize = 0; + geometry.totalSize = 0; + return false; + } + + geometry.flashType = FLASH_TYPE_NAND; + geometry.sectorSize = geometry.pagesPerSector * geometry.pageSize; + geometry.totalSize = geometry.sectorSize * geometry.sectors; + + /*flashPartitionSet(FLASH_PARTITION_TYPE_BADBLOCK_MANAGEMENT, + W25N_BB_MANAGEMENT_START_BLOCK, + W25N_BB_MANAGEMENT_START_BLOCK + W25N_BB_MANAGEMENT_BLOCKS - 1);*/ + + couldBeBusy = true; // Just for luck we'll assume the chip could be busy even though it isn't specced to be + + w25n_deviceReset(); + + // Upper 4MB (32 blocks * 128KB/block) will be used for bad block replacement area. + // Blocks in this area are only written through bad block LUT, + // and factory written bad block marker in unused blocks are retained. + // When a replacement block is required, + // (1) "Read BB LUT" command is used to obtain the last block mapped, + // (2) blocks after the last block is scanned for a good block, + // (3) the first good block is used for replacement, and the BB LUT is updated. + // There are only 20 BB LUT entries, and there are 32 replacement blocks. + // There will be a least chance of running out of replacement blocks. + // If it ever run out, the device becomes unusable. + + return true; +} + +/** + * Erase a sector full of bytes to all 1's at the given byte offset in the flash chip. + */ +void w25n_eraseSector(uint32_t address) +{ + w25n_waitForReadyInternal(); + w25n_writeEnable(); + w25n_performCommandWithPageAddress(W25N_INSTRUCTION_BLOCK_ERASE, W25N_LINEAR_TO_PAGE(address)); + w25n_setTimeout(W25N_TIMEOUT_BLOCK_ERASE_MS); +} + +// W25N does not support full chip erase. +// Call eraseSector repeatedly. +void w25n_eraseCompletely(void) +{ + for (uint32_t block = 0; block < geometry.sectors; block++) { + w25n_eraseSector(W25N_BLOCK_TO_LINEAR(block)); + } +} + +static void w25n_programDataLoad(uint16_t columnAddress, const uint8_t *data, int length) +{ + w25n_waitForReadyInternal(); + + uint8_t cmd[3] = {W25N_INSTRUCTION_PROGRAM_DATA_LOAD, columnAddress >> 8, columnAddress & 0xff}; + + busTransferDescriptor_t transferDescr[] = {{.length = sizeof(cmd), .rxBuf = NULL, .txBuf = cmd}, {.length = length, .rxBuf = NULL, .txBuf = (uint8_t *)data}}; + busTransferMultiple(busDev, transferDescr, ARRAYLEN(transferDescr)); + + w25n_setTimeout(W25N_TIMEOUT_PAGE_PROGRAM_MS); +} + +static void w25n_randomProgramDataLoad(uint16_t columnAddress, const uint8_t *data, int length) +{ + w25n_waitForReadyInternal(); + + uint8_t cmd[3] = {W25N_INSTRUCTION_RANDOM_PROGRAM_DATA_LOAD, columnAddress >> 8, columnAddress & 0xff}; + + busTransferDescriptor_t transferDescr[] = {{.length = sizeof(cmd), .rxBuf = NULL, .txBuf = cmd}, {.length = length, .rxBuf = NULL, .txBuf = (uint8_t *)data}}; + busTransferMultiple(busDev, transferDescr, ARRAYLEN(transferDescr)); + + w25n_setTimeout(W25N_TIMEOUT_PAGE_PROGRAM_MS); +} + +static void w25n_programExecute(uint32_t pageAddress) +{ + w25n_waitForReadyInternal(); + w25n_performCommandWithPageAddress(W25N_INSTRUCTION_PROGRAM_EXECUTE, pageAddress); + w25n_setTimeout(W25N_TIMEOUT_PAGE_PROGRAM_MS); +} + +// Writes are done in three steps: +// (1) Load internal data buffer with data to write +// - We use "Random Load Program Data", as "Load Program Data" resets unused data bytes in the buffer to 0xff. +// - Each "Random Load Program Data" instruction must be accompanied by at least a single data. +// - Each "Random Load Program Data" instruction terminates at the rising of CS. +// (2) Enable write +// (3) Issue "Execute Program" +// +/* +flashfs page program behavior +- Single program never crosses page boundary. +- Except for this characteristic, it program arbitral size. +- Write address is, naturally, not a page boundary. +To cope with this behavior. +pageProgramBegin: +If buffer is dirty and programLoadAddress != address, then the last page is a partial write; +issue PAGE_PROGRAM_EXECUTE to flash buffer contents, clear dirty and record the address as programLoadAddress and programStartAddress. +Else do nothing. +pageProgramContinue: +Mark buffer as dirty. +If programLoadAddress is on page boundary, then issue PROGRAM_LOAD_DATA, else issue RANDOM_PROGRAM_LOAD_DATA. +Update programLoadAddress. +Optionally observe the programLoadAddress, and if it's on page boundary, issue PAGE_PROGRAM_EXECUTE. +pageProgramFinish: +Observe programLoadAddress. If it's on page boundary, issue PAGE_PROGRAM_EXECUTE and clear dirty, else just return. +If pageProgramContinue observes the page boundary, then do nothing(?). +*/ +bool bufferDirty = false; +bool isProgramming = false; +static uint32_t programStartAddress; +static uint32_t programLoadAddress; +static uint32_t currentPage = UINT32_MAX; + +void w25n_pageProgramBegin(uint32_t address) +{ + if (bufferDirty) { + if (address != programLoadAddress) { + w25n_waitForReadyInternal(); + isProgramming = false; + w25n_writeEnable(); + w25n_programExecute(W25N_LINEAR_TO_PAGE(programStartAddress)); + bufferDirty = false; + isProgramming = true; + } + } else { + programStartAddress = programLoadAddress = address; + } +} + +void w25n_pageProgramContinue(const uint8_t *data, int length) +{ + // Check for page boundary overrun + w25n_waitForReadyInternal(); + w25n_writeEnable(); + isProgramming = false; + if (!bufferDirty) { + w25n_programDataLoad(W25N_LINEAR_TO_COLUMN(programLoadAddress), (uint8_t *)data, length); + } else { + w25n_randomProgramDataLoad(W25N_LINEAR_TO_COLUMN(programLoadAddress), (uint8_t *)data, length); + } + // XXX Test if write enable is reset after each data loading. + bufferDirty = true; + programLoadAddress += length; +} + +void w25n_pageProgramFinish(void) +{ + if (bufferDirty && W25N_LINEAR_TO_COLUMN(programLoadAddress) == 0) { + currentPage = W25N_LINEAR_TO_PAGE(programStartAddress); // reset page to the page being written + w25n_programExecute(W25N_LINEAR_TO_PAGE(programStartAddress)); + bufferDirty = false; + isProgramming = true; + programStartAddress = programLoadAddress; + } +} + +/* + * Write bytes to a flash page. Address must not cross a page boundary. + * + * Bits can only be set to zero, not from zero back to one again. In order to set bits to 1, use the erase command. + * + * Length must be smaller than the page size. + * + * This will wait for the flash to become ready before writing begins. + * + * Datasheet indicates typical programming time is 0.8ms for 256 bytes, 0.2ms for 64 bytes, 0.05ms for 16 bytes. + * (Although the maximum possible write time is noted as 5ms). + * + * If you want to write multiple buffers (whose sum of sizes is still not more than the page size) then you can + * break this operation up into one beginProgram call, one or more continueProgram calls, and one finishProgram call. + */ +uint32_t w25n_pageProgram(uint32_t address, const uint8_t *data, int length) +{ + w25n_pageProgramBegin(address); + w25n_pageProgramContinue((uint8_t *)data, length); + w25n_pageProgramFinish(); + + return address + length; +} + +void w25n_flush(void) +{ + if (bufferDirty) { + currentPage = W25N_LINEAR_TO_PAGE(programStartAddress); // reset page to the page being written + w25n_programExecute(W25N_LINEAR_TO_PAGE(programStartAddress)); + bufferDirty = false; + isProgramming = true; + } else { + isProgramming = false; + } +} + +void w25n_addError(uint32_t address, uint8_t code) +{ + UNUSED(address); + UNUSED(code); +} + +/* + * Read `length` bytes into the provided `buffer` from the flash starting from the given `address` (which need not lie + * on a page boundary). + * + * Waits up to W25N_TIMEOUT_PAGE_READ_MS milliseconds for the flash to become ready before reading. + * + * The number of bytes actually read is returned, which can be zero if an error or timeout occurred. + */ +// Continuous read mode (BUF = 0): +// (1) "Page Data Read" command is executed for the page pointed by address +// (2) "Read Data" command is executed for bytes not requested and data are discarded +// (3) "Read Data" command is executed and data are stored directly into caller's buffer +// +// Buffered read mode (BUF = 1), non-read ahead +// (1) If currentBufferPage != requested page, then issue PAGE_DATA_READ on requested page. +// (2) Compute transferLength as smaller of remaining length and requested length. +// (3) Issue READ_DATA on column address. +// (4) Return transferLength. +int w25n_readBytes(uint32_t address, uint8_t *buffer, int length) +{ + uint32_t targetPage = W25N_LINEAR_TO_PAGE(address); + + if (currentPage != targetPage) { + if (!w25n_waitForReadyInternal()) { + return 0; + } + currentPage = UINT32_MAX; + w25n_performCommandWithPageAddress(W25N_INSTRUCTION_PAGE_DATA_READ, targetPage); + if (!w25n_waitForReady(W25N_TIMEOUT_PAGE_READ_MS)) { + return 0; + } + currentPage = targetPage; + } + + uint16_t column = W25N_LINEAR_TO_COLUMN(address); + uint16_t transferLength; + + if (length > W25N_PAGE_SIZE - column) { + transferLength = W25N_PAGE_SIZE - column; + } else { + transferLength = length; + } + + const uint8_t cmd[4] = {W25N_INSTRUCTION_READ_DATA, (column >> 8) & 0xff, (column >> 0) & 0xff, 0}; + + busTransferDescriptor_t readDescr[] = {{.length = sizeof(cmd), .rxBuf = NULL, .txBuf = cmd}, {.length = transferLength, .rxBuf = buffer, .txBuf = NULL}}; + busTransferMultiple(busDev, readDescr, ARRAYLEN(readDescr)); + + if (!w25n_waitForReady(W25N_TIMEOUT_PAGE_READ_MS)) { + return 0; + } + + // Check ECC + uint8_t statReg = w25n_readRegister(W25N_STAT_REG); + uint8_t eccCode = W25N_STATUS_FLAG_ECC(statReg); + switch (eccCode) { + case 0: // Successful read, no ECC correction + break; + + case 1: // Successful read with ECC correction + case 2: // Uncorrectable ECC in a single page + case 3: // Uncorrectable ECC in multiple pages + w25n_addError(address, eccCode); + w25n_deviceReset(); + break; + } + + return transferLength; +} + +/** + * Fetch information about the detected flash chip layout. + * + * Can be called before calling w25n_init() (the result would have totalSize = 0). + */ +const flashGeometry_t* w25n_getGeometry(void) +{ + return &geometry; +} + +bool w25n_init(int flashNumToUse) +{ + busDev = busDeviceInit(BUSTYPE_SPI, DEVHW_W25N, flashNumToUse, OWNER_FLASH); + if (busDev == NULL) { + return false; + } + + uint8_t in[4] = { 0 }; + uint32_t chipID; + + delay(50); // short delay required after initialisation of SPI device instance. + + busReadBuf(busDev, W25N_INSTRUCTION_RDID, in, sizeof(in)); + + chipID = (in[1] << 16) | (in[2] << 8) | (in[3]); + + return w25n_detect(chipID); +} + +#endif \ No newline at end of file diff --git a/src/main/drivers/flash_w25n01g.h b/src/main/drivers/flash_w25n.h similarity index 65% rename from src/main/drivers/flash_w25n01g.h rename to src/main/drivers/flash_w25n.h index 032234005b6..516cefea8c5 100644 --- a/src/main/drivers/flash_w25n01g.h +++ b/src/main/drivers/flash_w25n.h @@ -24,18 +24,18 @@ #include "flash.h" #include "drivers/io_types.h" -bool w25n01g_init(int flashNumToUse); +bool w25n_init(int flashNumToUse); -void w25n01g_eraseSector(uint32_t address); -void w25n01g_eraseCompletely(void); +void w25n_eraseSector(uint32_t address); +void w25n_eraseCompletely(void); -uint32_t w25n01g_pageProgram(uint32_t address, const uint8_t *data, int length); +uint32_t w25n_pageProgram(uint32_t address, const uint8_t *data, int length); -void w25n01g_flush(void); +void w25n_flush(void); -int w25n01g_readBytes(uint32_t address, uint8_t *buffer, int length); +int w25n_readBytes(uint32_t address, uint8_t *buffer, int length); -bool w25n01g_isReady(void); -bool w25n01g_waitForReady(timeMs_t timeoutMillis); +bool w25n_isReady(void); +bool w25n_waitForReady(timeMs_t timeoutMillis); -const flashGeometry_t* w25n01g_getGeometry(void); \ No newline at end of file +const flashGeometry_t* w25n_getGeometry(void); \ No newline at end of file diff --git a/src/main/drivers/flash_w25n01g.c b/src/main/drivers/flash_w25n01g.c deleted file mode 100644 index fc287724616..00000000000 --- a/src/main/drivers/flash_w25n01g.c +++ /dev/null @@ -1,569 +0,0 @@ -/* - * This file is part of INAV. - * - * INAV are free software. You can redistribute - * this software and/or modify this software 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. - * - * INAV are distributed in the hope that they - * 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 software. - * - * If not, see . - */ - -#include -#include - -#include "platform.h" - -#ifdef USE_FLASH_W25N01G - -#include "drivers/bus.h" -#include "drivers/io.h" -#include "drivers/time.h" - -#include "flash_w25n01g.h" - -// Device size parameters -#define W25N01G_PAGE_SIZE 2048 -#define W25N01G_PAGES_PER_BLOCK 64 -#define W25N01G_BLOCKS_PER_DIE 1024 - -// BB replacement area -#define W25N01G_BB_MARKER_BLOCKS 1 -#define W25N01G_BB_REPLACEMENT_BLOCKS 20 -#define W25N01G_BB_MANAGEMENT_BLOCKS (W25N01G_BB_REPLACEMENT_BLOCKS + W25N01G_BB_MARKER_BLOCKS) - -// blocks are zero-based index -#define W25N01G_BB_REPLACEMENT_START_BLOCK (W25N01G_BLOCKS_PER_DIE - W25N01G_BB_REPLACEMENT_BLOCKS) -#define W25N01G_BB_MANAGEMENT_START_BLOCK (W25N01G_BLOCKS_PER_DIE - W25N01G_BB_MANAGEMENT_BLOCKS) -#define W25N01G_BB_MARKER_BLOCK (W25N01G_BB_REPLACEMENT_START_BLOCK - W25N01G_BB_MARKER_BLOCKS) - -// Instructions -#define W25N01G_INSTRUCTION_RDID 0x9F -#define W25N01G_INSTRUCTION_DEVICE_RESET 0xFF -#define W25N01G_INSTRUCTION_READ_STATUS_REG 0x05 -#define W25N01G_INSTRUCTION_READ_STATUS_ALTERNATE_REG 0x0F -#define W25N01G_INSTRUCTION_WRITE_STATUS_REG 0x01 -#define W25N01G_INSTRUCTION_WRITE_STATUS_ALTERNATE_REG 0x1F -#define W25N01G_INSTRUCTION_WRITE_ENABLE 0x06 -#define W25N01G_INSTRUCTION_DIE_SELECT 0xC2 -#define W25N01G_INSTRUCTION_BLOCK_ERASE 0xD8 -#define W25N01G_INSTRUCTION_READ_BBM_LUT 0xA5 -#define W25N01G_INSTRUCTION_BB_MANAGEMENT 0xA1 -#define W25N01G_INSTRUCTION_PROGRAM_DATA_LOAD 0x02 -#define W25N01G_INSTRUCTION_RANDOM_PROGRAM_DATA_LOAD 0x84 -#define W25N01G_INSTRUCTION_PROGRAM_EXECUTE 0x10 -#define W25N01G_INSTRUCTION_PAGE_DATA_READ 0x13 -#define W25N01G_INSTRUCTION_READ_DATA 0x03 -#define W25N01G_INSTRUCTION_FAST_READ 0x1B -#define W25N01G_INSTRUCTION_FAST_READ_QUAD_OUTPUT 0x6B - -// Config/status register addresses -#define W25N01G_PROT_REG 0xA0 -#define W25N01G_CONF_REG 0xB0 -#define W25N01G_STAT_REG 0xC0 - -// Bits in config/status register 1 (W25N01G_PROT_REG) -#define W25N01G_PROT_CLEAR (0) -#define W25N01G_PROT_SRP1_ENABLE (1 << 0) -#define W25N01G_PROT_WP_E_ENABLE (1 << 1) -#define W25N01G_PROT_TB_ENABLE (1 << 2) -#define W25N01G_PROT_PB0_ENABLE (1 << 3) -#define W25N01G_PROT_PB1_ENABLE (1 << 4) -#define W25N01G_PROT_PB2_ENABLE (1 << 5) -#define W25N01G_PROT_PB3_ENABLE (1 << 6) -#define W25N01G_PROT_SRP2_ENABLE (1 << 7) - -// Bits in config/status register 2 (W25N01G_CONF_REG) -#define W25N01G_CONFIG_ECC_ENABLE (1 << 4) -#define W25N01G_CONFIG_BUFFER_READ_MODE (1 << 3) - -// Bits in config/status register 3 (W25N01G_STATREG) -#define W25N01G_STATUS_BBM_LUT_FULL (1 << 6) -#define W25N01G_STATUS_FLAG_ECC_POS 4 -#define W25N01G_STATUS_FLAG_ECC_MASK ((1 << 5)|(1 << 4)) -#define W25N01G_STATUS_FLAG_ECC(status) (((status) & W25N01G_STATUS_FLAG_ECC_MASK) >> 4) -#define W25N01G_STATUS_PROGRAM_FAIL (1 << 3) -#define W25N01G_STATUS_ERASE_FAIL (1 << 2) -#define W25N01G_STATUS_FLAG_WRITE_ENABLED (1 << 1) -#define W25N01G_STATUS_FLAG_BUSY (1 << 0) -#define W25N01G_BBLUT_TABLE_ENTRY_COUNT 20 -#define W25N01G_BBLUT_TABLE_ENTRY_SIZE 4 // in bytes - -// Bits in LBA for BB LUT -#define W25N01G_BBLUT_STATUS_ENABLED (1 << 15) -#define W25N01G_BBLUT_STATUS_INVALID (1 << 14) -#define W25N01G_BBLUT_STATUS_MASK (W25N01G_BBLUT_STATUS_ENABLED | W25N01G_BBLUT_STATUS_INVALID) - -// Some useful defs and macros -#define W25N01G_LINEAR_TO_COLUMN(laddr) ((laddr) % W25N01G_PAGE_SIZE) -#define W25N01G_LINEAR_TO_PAGE(laddr) ((laddr) / W25N01G_PAGE_SIZE) -#define W25N01G_LINEAR_TO_BLOCK(laddr) (W25N01G_LINEAR_TO_PAGE(laddr) / W25N01G_PAGES_PER_BLOCK) -#define W25N01G_BLOCK_TO_PAGE(block) ((block) * W25N01G_PAGES_PER_BLOCK) -#define W25N01G_BLOCK_TO_LINEAR(block) (W25N01G_BLOCK_TO_PAGE(block) * W25N01G_PAGE_SIZE) - -// IMPORTANT: Timeout values are currently required to be set to the highest value required by any of the supported flash chips by this driver -// The timeout values (2ms minimum to avoid 1 tick advance in consecutive calls to millis). -#define W25N01G_TIMEOUT_PAGE_READ_MS 2 // tREmax = 60us (ECC enabled) -#define W25N01G_TIMEOUT_PAGE_PROGRAM_MS 2 // tPPmax = 700us -#define W25N01G_TIMEOUT_BLOCK_ERASE_MS 15 // tBEmax = 10ms -#define W25N01G_TIMEOUT_RESET_MS 500 // tRSTmax = 500ms - -// Sizes (in bits) -#define W28N01G_STATUS_REGISTER_SIZE 8 -#define W28N01G_STATUS_PAGE_ADDRESS_SIZE 16 -#define W28N01G_STATUS_COLUMN_ADDRESS_SIZE 16 - -// JEDEC ID -#define JEDEC_ID_WINBOND_W25N01GV 0xEFAA21 - -static busDevice_t *busDev = NULL; -static flashGeometry_t geometry; - -/* - * Whether we've performed an action that could have made the device busy for writes. - * - * This allows us to avoid polling for writable status when it is definitely ready already. - */ -static bool couldBeBusy = false; - -static timeMs_t timeoutAt = 0; - -static bool w25n01g_waitForReadyInternal(void); - -static void w25n01g_setTimeout(timeMs_t timeoutMillis) -{ - timeMs_t now = millis(); - timeoutAt = now + timeoutMillis; - couldBeBusy = true; -} - -/** - * Send the given command byte to the device. - */ -static void w25n01g_performOneByteCommand(uint8_t command) -{ - busTransfer(busDev, NULL, &command, 1); -} - -static void w25n01g_performCommandWithPageAddress(uint8_t command, uint32_t pageAddress) -{ - uint8_t cmd[4] = { command, 0, (pageAddress >> 8) & 0xff, (pageAddress >> 0) & 0xff}; - busTransfer(busDev, NULL, cmd, sizeof(cmd)); -} - -static uint8_t w25n01g_readRegister(uint8_t reg) -{ - uint8_t command[3] = { W25N01G_INSTRUCTION_READ_STATUS_REG, reg, 0 }; - uint8_t in[3]; - - busTransfer(busDev, in, command, sizeof(command)); - - return in[2]; -} - -static void w25n01g_writeRegister(uint8_t reg, uint8_t data) -{ - uint8_t cmd[3] = { W25N01G_INSTRUCTION_WRITE_STATUS_REG, reg, data }; - busTransfer(busDev, NULL, cmd, sizeof(cmd)); -} - -static void w25n01g_deviceReset(void) -{ - w25n01g_performOneByteCommand(W25N01G_INSTRUCTION_DEVICE_RESET); - w25n01g_setTimeout(W25N01G_TIMEOUT_RESET_MS); - w25n01g_waitForReadyInternal(); - // Protection for upper 1/32 (BP[3:0] = 0101, TB=0), WP-E on; to protect bad block replacement area - // DON'T DO THIS. This will prevent writes through the bblut as well. - // w25n01g_writeRegister(busdev, W25N01G_PROT_REG, W25N01G_PROT_PB0_ENABLE|W25N01G_PROT_PB2_ENABLE|W25N01G_PROT_WP_E_ENABLE); - // No protection, WP-E off, WP-E prevents use of IO2 - w25n01g_writeRegister(W25N01G_PROT_REG, W25N01G_PROT_CLEAR); - // Buffered read mode (BUF = 1), ECC enabled (ECC = 1) - w25n01g_writeRegister(W25N01G_CONF_REG, W25N01G_CONFIG_ECC_ENABLE | W25N01G_CONFIG_BUFFER_READ_MODE); -} - -bool w25n01g_isReady(void) -{ - uint8_t status = w25n01g_readRegister(W25N01G_STAT_REG); - - // If couldBeBusy is false, don't bother to poll the flash chip for its status - couldBeBusy = couldBeBusy && ((status & W25N01G_STATUS_FLAG_BUSY) != 0); - - return !couldBeBusy; -} - -static bool w25n01g_waitForReadyInternal(void) -{ - while (!w25n01g_isReady()) { - timeMs_t now = millis(); - if (cmp32(now, timeoutAt) >= 0) { - return false; - } - } - timeoutAt = 0; - return true; -} - -bool w25n01g_waitForReady(timeMs_t timeoutMillis) -{ - w25n01g_setTimeout(timeoutMillis); - return w25n01g_waitForReadyInternal(); -} - -/** - * The flash requires this write enable command to be sent before commands that would cause - * a write like program and erase. - */ -static void w25n01g_writeEnable(void) -{ - w25n01g_performOneByteCommand(W25N01G_INSTRUCTION_WRITE_ENABLE); - - // Assume that we're about to do some writing, so the device is just about to become busy - couldBeBusy = true; -} - -bool w25n01g_detect(uint32_t chipID) -{ - switch (chipID) { - case JEDEC_ID_WINBOND_W25N01GV: - geometry.sectors = 1024; // Blocks - geometry.pagesPerSector = 64; // Pages/Blocks - geometry.pageSize = 2048; - break; - - default: - // Unsupported chip - geometry.sectors = 0; - geometry.pagesPerSector = 0; - geometry.sectorSize = 0; - geometry.totalSize = 0; - return false; - } - - geometry.flashType = FLASH_TYPE_NAND; - geometry.sectorSize = geometry.pagesPerSector * geometry.pageSize; - geometry.totalSize = geometry.sectorSize * geometry.sectors; - - /*flashPartitionSet(FLASH_PARTITION_TYPE_BADBLOCK_MANAGEMENT, - W25N01G_BB_MANAGEMENT_START_BLOCK, - W25N01G_BB_MANAGEMENT_START_BLOCK + W25N01G_BB_MANAGEMENT_BLOCKS - 1);*/ - - couldBeBusy = true; // Just for luck we'll assume the chip could be busy even though it isn't specced to be - - w25n01g_deviceReset(); - - // Upper 4MB (32 blocks * 128KB/block) will be used for bad block replacement area. - // Blocks in this area are only written through bad block LUT, - // and factory written bad block marker in unused blocks are retained. - // When a replacement block is required, - // (1) "Read BB LUT" command is used to obtain the last block mapped, - // (2) blocks after the last block is scanned for a good block, - // (3) the first good block is used for replacement, and the BB LUT is updated. - // There are only 20 BB LUT entries, and there are 32 replacement blocks. - // There will be a least chance of running out of replacement blocks. - // If it ever run out, the device becomes unusable. - - return true; -} - -/** - * Erase a sector full of bytes to all 1's at the given byte offset in the flash chip. - */ -void w25n01g_eraseSector(uint32_t address) -{ - w25n01g_waitForReadyInternal(); - w25n01g_writeEnable(); - w25n01g_performCommandWithPageAddress(W25N01G_INSTRUCTION_BLOCK_ERASE, W25N01G_LINEAR_TO_PAGE(address)); - w25n01g_setTimeout(W25N01G_TIMEOUT_BLOCK_ERASE_MS); -} - -// W25N01G does not support full chip erase. -// Call eraseSector repeatedly. -void w25n01g_eraseCompletely(void) -{ - for (uint32_t block = 0; block < geometry.sectors; block++) { - w25n01g_eraseSector(W25N01G_BLOCK_TO_LINEAR(block)); - } -} - -static void w25n01g_programDataLoad(uint16_t columnAddress, const uint8_t *data, int length) -{ - w25n01g_waitForReadyInternal(); - - uint8_t cmd[3] = {W25N01G_INSTRUCTION_PROGRAM_DATA_LOAD, columnAddress >> 8, columnAddress & 0xff}; - - busTransferDescriptor_t transferDescr[] = {{.length = sizeof(cmd), .rxBuf = NULL, .txBuf = cmd}, {.length = length, .rxBuf = NULL, .txBuf = (uint8_t *)data}}; - busTransferMultiple(busDev, transferDescr, ARRAYLEN(transferDescr)); - - w25n01g_setTimeout(W25N01G_TIMEOUT_PAGE_PROGRAM_MS); -} - -static void w25n01g_randomProgramDataLoad(uint16_t columnAddress, const uint8_t *data, int length) -{ - w25n01g_waitForReadyInternal(); - - uint8_t cmd[3] = {W25N01G_INSTRUCTION_RANDOM_PROGRAM_DATA_LOAD, columnAddress >> 8, columnAddress & 0xff}; - - busTransferDescriptor_t transferDescr[] = {{.length = sizeof(cmd), .rxBuf = NULL, .txBuf = cmd}, {.length = length, .rxBuf = NULL, .txBuf = (uint8_t *)data}}; - busTransferMultiple(busDev, transferDescr, ARRAYLEN(transferDescr)); - - w25n01g_setTimeout(W25N01G_TIMEOUT_PAGE_PROGRAM_MS); -} - -static void w25n01g_programExecute(uint32_t pageAddress) -{ - w25n01g_waitForReadyInternal(); - w25n01g_performCommandWithPageAddress(W25N01G_INSTRUCTION_PROGRAM_EXECUTE, pageAddress); - w25n01g_setTimeout(W25N01G_TIMEOUT_PAGE_PROGRAM_MS); -} - -// Writes are done in three steps: -// (1) Load internal data buffer with data to write -// - We use "Random Load Program Data", as "Load Program Data" resets unused data bytes in the buffer to 0xff. -// - Each "Random Load Program Data" instruction must be accompanied by at least a single data. -// - Each "Random Load Program Data" instruction terminates at the rising of CS. -// (2) Enable write -// (3) Issue "Execute Program" -// -/* -flashfs page program behavior -- Single program never crosses page boundary. -- Except for this characteristic, it program arbitral size. -- Write address is, naturally, not a page boundary. -To cope with this behavior. -pageProgramBegin: -If buffer is dirty and programLoadAddress != address, then the last page is a partial write; -issue PAGE_PROGRAM_EXECUTE to flash buffer contents, clear dirty and record the address as programLoadAddress and programStartAddress. -Else do nothing. -pageProgramContinue: -Mark buffer as dirty. -If programLoadAddress is on page boundary, then issue PROGRAM_LOAD_DATA, else issue RANDOM_PROGRAM_LOAD_DATA. -Update programLoadAddress. -Optionally observe the programLoadAddress, and if it's on page boundary, issue PAGE_PROGRAM_EXECUTE. -pageProgramFinish: -Observe programLoadAddress. If it's on page boundary, issue PAGE_PROGRAM_EXECUTE and clear dirty, else just return. -If pageProgramContinue observes the page boundary, then do nothing(?). -*/ -bool bufferDirty = false; -bool isProgramming = false; -static uint32_t programStartAddress; -static uint32_t programLoadAddress; -static uint32_t currentPage = UINT32_MAX; - -void w25n01g_pageProgramBegin(uint32_t address) -{ - if (bufferDirty) { - if (address != programLoadAddress) { - w25n01g_waitForReadyInternal(); - isProgramming = false; - w25n01g_writeEnable(); - w25n01g_programExecute(W25N01G_LINEAR_TO_PAGE(programStartAddress)); - bufferDirty = false; - isProgramming = true; - } - } else { - programStartAddress = programLoadAddress = address; - } -} - -void w25n01g_pageProgramContinue(const uint8_t *data, int length) -{ - // Check for page boundary overrun - w25n01g_waitForReadyInternal(); - w25n01g_writeEnable(); - isProgramming = false; - if (!bufferDirty) { - w25n01g_programDataLoad(W25N01G_LINEAR_TO_COLUMN(programLoadAddress), (uint8_t *)data, length); - } else { - w25n01g_randomProgramDataLoad(W25N01G_LINEAR_TO_COLUMN(programLoadAddress), (uint8_t *)data, length); - } - // XXX Test if write enable is reset after each data loading. - bufferDirty = true; - programLoadAddress += length; -} - -void w25n01g_pageProgramFinish(void) -{ - if (bufferDirty && W25N01G_LINEAR_TO_COLUMN(programLoadAddress) == 0) { - currentPage = W25N01G_LINEAR_TO_PAGE(programStartAddress); // reset page to the page being written - w25n01g_programExecute(W25N01G_LINEAR_TO_PAGE(programStartAddress)); - bufferDirty = false; - isProgramming = true; - programStartAddress = programLoadAddress; - } -} - -/* - * Write bytes to a flash page. Address must not cross a page boundary. - * - * Bits can only be set to zero, not from zero back to one again. In order to set bits to 1, use the erase command. - * - * Length must be smaller than the page size. - * - * This will wait for the flash to become ready before writing begins. - * - * Datasheet indicates typical programming time is 0.8ms for 256 bytes, 0.2ms for 64 bytes, 0.05ms for 16 bytes. - * (Although the maximum possible write time is noted as 5ms). - * - * If you want to write multiple buffers (whose sum of sizes is still not more than the page size) then you can - * break this operation up into one beginProgram call, one or more continueProgram calls, and one finishProgram call. - */ -uint32_t w25n01g_pageProgram(uint32_t address, const uint8_t *data, int length) -{ - w25n01g_pageProgramBegin(address); - w25n01g_pageProgramContinue((uint8_t *)data, length); - w25n01g_pageProgramFinish(); - - return address + length; -} - -void w25n01g_flush(void) -{ - if (bufferDirty) { - currentPage = W25N01G_LINEAR_TO_PAGE(programStartAddress); // reset page to the page being written - w25n01g_programExecute(W25N01G_LINEAR_TO_PAGE(programStartAddress)); - bufferDirty = false; - isProgramming = true; - } else { - isProgramming = false; - } -} - -void w25n01g_addError(uint32_t address, uint8_t code) -{ - UNUSED(address); - UNUSED(code); -} - -/* - * Read `length` bytes into the provided `buffer` from the flash starting from the given `address` (which need not lie - * on a page boundary). - * - * Waits up to W25N01G_TIMEOUT_PAGE_READ_MS milliseconds for the flash to become ready before reading. - * - * The number of bytes actually read is returned, which can be zero if an error or timeout occurred. - */ -// Continuous read mode (BUF = 0): -// (1) "Page Data Read" command is executed for the page pointed by address -// (2) "Read Data" command is executed for bytes not requested and data are discarded -// (3) "Read Data" command is executed and data are stored directly into caller's buffer -// -// Buffered read mode (BUF = 1), non-read ahead -// (1) If currentBufferPage != requested page, then issue PAGE_DATA_READ on requested page. -// (2) Compute transferLength as smaller of remaining length and requested length. -// (3) Issue READ_DATA on column address. -// (4) Return transferLength. -int w25n01g_readBytes(uint32_t address, uint8_t *buffer, int length) -{ - uint32_t targetPage = W25N01G_LINEAR_TO_PAGE(address); - - if (currentPage != targetPage) { - if (!w25n01g_waitForReadyInternal()) { - return 0; - } - currentPage = UINT32_MAX; - w25n01g_performCommandWithPageAddress(W25N01G_INSTRUCTION_PAGE_DATA_READ, targetPage); - if (!w25n01g_waitForReady(W25N01G_TIMEOUT_PAGE_READ_MS)) { - return 0; - } - currentPage = targetPage; - } - - uint16_t column = W25N01G_LINEAR_TO_COLUMN(address); - uint16_t transferLength; - - if (length > W25N01G_PAGE_SIZE - column) { - transferLength = W25N01G_PAGE_SIZE - column; - } else { - transferLength = length; - } - - const uint8_t cmd[4] = {W25N01G_INSTRUCTION_READ_DATA, (column >> 8) & 0xff, (column >> 0) & 0xff, 0}; - - busTransferDescriptor_t readDescr[] = {{.length = sizeof(cmd), .rxBuf = NULL, .txBuf = cmd}, {.length = transferLength, .rxBuf = buffer, .txBuf = NULL}}; - busTransferMultiple(busDev, readDescr, ARRAYLEN(readDescr)); - - if (!w25n01g_waitForReady(W25N01G_TIMEOUT_PAGE_READ_MS)) { - return 0; - } - - // Check ECC - uint8_t statReg = w25n01g_readRegister(W25N01G_STAT_REG); - uint8_t eccCode = W25N01G_STATUS_FLAG_ECC(statReg); - switch (eccCode) { - case 0: // Successful read, no ECC correction - break; - - case 1: // Successful read with ECC correction - case 2: // Uncorrectable ECC in a single page - case 3: // Uncorrectable ECC in multiple pages - w25n01g_addError(address, eccCode); - w25n01g_deviceReset(); - break; - } - - return transferLength; -} - -int w25n01g_readExtensionBytes(uint32_t address, uint8_t *buffer, int length) -{ - if (!w25n01g_waitForReadyInternal()) { - return 0; - } - - w25n01g_performCommandWithPageAddress(W25N01G_INSTRUCTION_PAGE_DATA_READ, W25N01G_LINEAR_TO_PAGE(address)); - - uint32_t column = 2048; - - uint8_t cmd[4]; - cmd[0] = W25N01G_INSTRUCTION_READ_DATA; - cmd[1] = (column >> 8) & 0xff; - cmd[2] = (column >> 0) & 0xff; - cmd[3] = 0; - - busTransferDescriptor_t readDescr[] = {{.length = sizeof(cmd), .rxBuf = NULL, .txBuf = cmd}, {.length = length, .rxBuf = buffer, .txBuf = NULL}}; - busTransferMultiple(busDev, readDescr, ARRAYLEN(readDescr)); - - w25n01g_setTimeout(W25N01G_TIMEOUT_PAGE_READ_MS); - - return length; -} - -/** - * Fetch information about the detected flash chip layout. - * - * Can be called before calling w25n01g_init() (the result would have totalSize = 0). - */ -const flashGeometry_t* w25n01g_getGeometry(void) -{ - return &geometry; -} - -bool w25n01g_init(int flashNumToUse) -{ - busDev = busDeviceInit(BUSTYPE_SPI, DEVHW_W25N01G, flashNumToUse, OWNER_FLASH); - if (busDev == NULL) { - return false; - } - - uint8_t in[4] = { 0 }; - uint32_t chipID; - - delay(50); // short delay required after initialisation of SPI device instance. - - busReadBuf(busDev, W25N01G_INSTRUCTION_RDID, in, sizeof(in)); - - chipID = (in[1] << 16) | (in[2] << 8) | (in[3]); - - return w25n01g_detect(chipID); -} - -#endif \ No newline at end of file diff --git a/src/main/drivers/max7456.c b/src/main/drivers/max7456.c index 22f8d9cdf4d..4d541d32027 100644 --- a/src/main/drivers/max7456.c +++ b/src/main/drivers/max7456.c @@ -240,8 +240,8 @@ static void max7456WaitUntilNoBusy(void) } } -// Sets wether the OSD drawing is enabled. Returns true iff -// changes were succesfully performed. +// Sets whether the OSD drawing is enabled. Returns true iff +// changes were successfully performed. static bool max7456OSDSetEnabled(bool enabled) { if (enabled && !(state.registers.vm0 | OSD_ENABLE)) { diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index 35b9c61a424..0b0311ca416 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -176,14 +176,13 @@ #define SYM_FLIGHT_HOURS_REMAINING 0xDB // 219 Flight time (hours) remaining #define SYM_GROUND_COURSE 0xDC // 220 Ground course #define SYM_ALERT 0xDD // 221 General alert symbol +#define SYM_MIN_GROUND_SPEED 0xDE // 222 Minimum Ground Speed #define SYM_TERRAIN_FOLLOWING 0xFB // 251 Terrain following (also Alt adjust) #define SYM_CROSS_TRACK_ERROR 0xFC // 252 Cross track error -#define SYM_ADSB 0xFD // 253 ADBS +#define SYM_ADSB 0xFD // 253 ADSB #define SYM_BLACKBOX 0xFE // 254 Blackbox -#define SYM_ADSB 0xFD // 253 ADSB - #define SYM_LOGO_START 0x101 // 257 to 297, INAV logo #define SYM_LOGO_WIDTH 10 #define SYM_LOGO_HEIGHT 4 @@ -237,6 +236,10 @@ #define SYM_RX_BAND 0x169 // 361 RX Band #define SYM_RX_MODE 0x16A // 362 RX Mode +#define SYM_THR_GAUGE_EMPTY 0x16B // 363 Throttle gauge empty +#define SYM_THR_GAUGE_HALF 0x16C // 364 Throttle gauge 1 step +#define SYM_THR_GAUGE_FULL 0x16D // 365 Throttle gauge 2 steps + #define SYM_AH_CH_TYPE3 0x190 // 400 to 402, crosshair 3 #define SYM_AH_CH_TYPE4 0x193 // 403 to 405, crosshair 4 #define SYM_AH_CH_TYPE5 0x196 // 406 to 408, crosshair 5 diff --git a/src/main/drivers/rangefinder/rangefinder_vl53l1x.c b/src/main/drivers/rangefinder/rangefinder_vl53l1x.c index 839d06e9dc4..f4f95297ae7 100644 --- a/src/main/drivers/rangefinder/rangefinder_vl53l1x.c +++ b/src/main/drivers/rangefinder/rangefinder_vl53l1x.c @@ -216,7 +216,7 @@ typedef int8_t VL53L1X_ERROR; /*!< Zone dynamic config GPH ID check failed - API out of sync */ #define VL53L1_ERROR_XTALK_EXTRACTION_NO_SAMPLE_FAIL ((VL53L1X_ERROR) - 22) - /*!< Thrown when run_xtalk_extraction fn has 0 succesful samples + /*!< Thrown when run_xtalk_extraction fn has 0 successful samples * when using the full array to sample the xtalk. In this case there is * not enough information to generate new Xtalk parm info. The function * will exit and leave the current xtalk parameters unaltered */ diff --git a/src/main/drivers/sdcard/sdcard_impl.h b/src/main/drivers/sdcard/sdcard_impl.h index 9fcb9a46902..3eba6095904 100644 --- a/src/main/drivers/sdcard/sdcard_impl.h +++ b/src/main/drivers/sdcard/sdcard_impl.h @@ -32,6 +32,7 @@ #define SDCARD_TIMEOUT_INIT_MILLIS 200 #define SDCARD_MAX_CONSECUTIVE_FAILURES 8 +#define SDCARD_MAX_OPERATION_RETRIES 3 typedef enum { // In these states we run at the initialization 400kHz clockspeed: @@ -62,6 +63,7 @@ typedef struct sdcard_t { uint32_t operationStartTime; uint8_t failureCount; + uint8_t operationRetries; // Retry count for current operation before reset uint8_t version; bool highCapacity; diff --git a/src/main/drivers/sdcard/sdcard_sdio.c b/src/main/drivers/sdcard/sdcard_sdio.c index 9914719fb75..d7cd8ff3c54 100644 --- a/src/main/drivers/sdcard/sdcard_sdio.c +++ b/src/main/drivers/sdcard/sdcard_sdio.c @@ -318,7 +318,7 @@ static bool sdcardSdio_poll(void) #endif sdcard.state = SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; } else if (sdcard.multiWriteBlocksRemain == 1) { - // This function changes the sd card state for us whether immediately succesful or delayed: + // This function changes the sd card state for us whether immediately successful or delayed: sdcard_endWriteBlocks(); } else { sdcard.state = SDCARD_STATE_READY; @@ -460,18 +460,29 @@ static sdcardOperationStatus_e sdcardSdio_writeBlock(uint32_t blockIndex, uint8_ sdcard.state = SDCARD_STATE_SENDING_WRITE; if (SD_WriteBlocks_DMA(blockIndex, (uint32_t*) buffer, 512, block_count) != SD_OK) { - /* Our write was rejected! This could be due to a bad address but we hope not to attempt that, so assume - * the card is broken and needs reset. + /* Our write was rejected! Try a few times before giving up. + * This handles transient DMA/bus issues without full card reset. */ + if (sdcard.operationRetries < SDCARD_MAX_OPERATION_RETRIES) { + sdcard.operationRetries++; + // Brief delay before retry + delay(1); + return SDCARD_OPERATION_BUSY; + } + + // Max retries exceeded, reset card + sdcard.operationRetries = 0; sdcardSdio_reset(); // Announce write failure: if (sdcard.pendingOperation.callback) { sdcard.pendingOperation.callback(SDCARD_BLOCK_OPERATION_WRITE, sdcard.pendingOperation.blockIndex, NULL, sdcard.pendingOperation.callbackData); } - return SDCARD_OPERATION_FAILURE; + return SDCARD_OPERATION_FAILURE; } + // Success - reset retry counter + sdcard.operationRetries = 0; return SDCARD_OPERATION_IN_PROGRESS; } @@ -486,7 +497,7 @@ static sdcardOperationStatus_e sdcardSdio_writeBlock(uint32_t blockIndex, uint8_ * Returns: * SDCARD_OPERATION_SUCCESS - Multi-block write has been queued * SDCARD_OPERATION_BUSY - The card is already busy and cannot accept your write - * SDCARD_OPERATION_FAILURE - A fatal error occured, card will be reset + * SDCARD_OPERATION_FAILURE - A fatal error occurred, card will be reset */ static sdcardOperationStatus_e sdcardSdio_beginWriteBlocks(uint32_t blockIndex, uint32_t blockCount) { @@ -545,8 +556,22 @@ static bool sdcardSdio_readBlock(uint32_t blockIndex, uint8_t *buffer, sdcard_op sdcard.state = SDCARD_STATE_READING; sdcard.operationStartTime = millis(); + // Success - reset retry counter + sdcard.operationRetries = 0; return true; } else { + /* Read was rejected! Try a few times before giving up. + * This handles transient DMA/bus issues without full card reset. + */ + if (sdcard.operationRetries < SDCARD_MAX_OPERATION_RETRIES) { + sdcard.operationRetries++; + // Brief delay before retry + delay(1); + return false; + } + + // Max retries exceeded, reset card + sdcard.operationRetries = 0; sdcardSdio_reset(); if (sdcard.pendingOperation.callback) { sdcard.pendingOperation.callback( @@ -601,6 +626,7 @@ void sdcardSdio_init(void) sdcard.operationStartTime = millis(); sdcard.state = SDCARD_STATE_RESET; sdcard.failureCount = 0; + sdcard.operationRetries = 0; } /** diff --git a/src/main/drivers/sdcard/sdcard_spi.c b/src/main/drivers/sdcard/sdcard_spi.c index 1b374e70e6b..214f1fd8105 100644 --- a/src/main/drivers/sdcard/sdcard_spi.c +++ b/src/main/drivers/sdcard/sdcard_spi.c @@ -580,7 +580,7 @@ static bool sdcardSpi_poll(void) sdcard.multiWriteNextBlock++; sdcard.state = SDCARD_STATE_WRITING_MULTIPLE_BLOCKS; } else if (sdcard.multiWriteBlocksRemain == 1) { - // This function changes the sd card state for us whether immediately succesful or delayed: + // This function changes the sd card state for us whether immediately successful or delayed: if (sdcardSpi_endWriteBlocks() == SDCARD_OPERATION_SUCCESS) { sdcardSpi_deselect(); } @@ -739,7 +739,7 @@ static sdcardOperationStatus_e sdcardSpi_writeBlock(uint32_t blockIndex, uint8_t * Returns: * SDCARD_OPERATION_SUCCESS - Multi-block write has been queued * SDCARD_OPERATION_BUSY - The card is already busy and cannot accept your write - * SDCARD_OPERATION_FAILURE - A fatal error occured, card will be reset + * SDCARD_OPERATION_FAILURE - A fatal error occurred, card will be reset */ static sdcardOperationStatus_e sdcardSpi_beginWriteBlocks(uint32_t blockIndex, uint32_t blockCount) { diff --git a/src/main/drivers/serial_tcp.c b/src/main/drivers/serial_tcp.c index 915f2a53605..7b1c462572e 100644 --- a/src/main/drivers/serial_tcp.c +++ b/src/main/drivers/serial_tcp.c @@ -47,6 +47,7 @@ static const struct serialPortVTable tcpVTable[]; static tcpPort_t tcpPorts[SERIAL_PORT_COUNT]; +uint16_t tcpBasePort = TCP_BASE_PORT_DEFAULT; static void *tcpReceiveThread(void* arg) { @@ -66,7 +67,7 @@ static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id) return NULL; } - uint16_t tcpPort = BASE_IP_ADDRESS + id - 1; + uint16_t tcpPort = tcpBasePort + id - 1; if (lookupAddress(NULL, tcpPort, SOCK_STREAM, (struct sockaddr*)&port->sockAddress, &sockaddrlen) != 0) { return NULL; } diff --git a/src/main/drivers/serial_tcp.h b/src/main/drivers/serial_tcp.h index 7f394b8ccd3..c20e197a63a 100644 --- a/src/main/drivers/serial_tcp.h +++ b/src/main/drivers/serial_tcp.h @@ -21,6 +21,8 @@ #pragma once #include +#include +#include #include #include #include @@ -28,7 +30,7 @@ #include "drivers/serial.h" -#define BASE_IP_ADDRESS 5760 +#define TCP_BASE_PORT_DEFAULT 5760 #define TCP_BUFFER_SIZE 2048 #define TCP_MAX_PACKET_SIZE 65535 @@ -55,4 +57,6 @@ serialPort_t *tcpOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, extern void tcpSend(tcpPort_t *port); extern int tcpReceive(tcpPort_t *port); extern void tcpReceiveBytesEx( int portIndex, const uint8_t* buffer, ssize_t recvSize ); -extern uint32_t tcpRXBytesFree(int portIndex); \ No newline at end of file +extern uint32_t tcpRXBytesFree(int portIndex); + +extern uint16_t tcpBasePort; diff --git a/src/main/drivers/timer_def_stm32h7xx.h b/src/main/drivers/timer_def_stm32h7xx.h index 649a037eb87..3def9c5d2ad 100644 --- a/src/main/drivers/timer_def_stm32h7xx.h +++ b/src/main/drivers/timer_def_stm32h7xx.h @@ -64,7 +64,13 @@ #define DEF_TIM_DMA__BTCH_TIM4_CH1 DEF_TIM_DMA_FULL(DMA_REQUEST_TIM4_CH1) #define DEF_TIM_DMA__BTCH_TIM4_CH2 DEF_TIM_DMA_FULL(DMA_REQUEST_TIM4_CH2) #define DEF_TIM_DMA__BTCH_TIM4_CH3 DEF_TIM_DMA_FULL(DMA_REQUEST_TIM4_CH3) + +#ifdef USE_DSHOT_DMAR +// Workaround because DMA_REQUEST_TIM4_CH4 does not exist +#define DEF_TIM_DMA__BTCH_TIM4_CH4 DEF_TIM_DMA_FULL(DMA_REQUEST_TIM4_UP) +#else #define DEF_TIM_DMA__BTCH_TIM4_CH4 NONE +#endif #define DEF_TIM_DMA__BTCH_TIM5_CH1 DEF_TIM_DMA_FULL(DMA_REQUEST_TIM5_CH1) #define DEF_TIM_DMA__BTCH_TIM5_CH2 DEF_TIM_DMA_FULL(DMA_REQUEST_TIM5_CH2) diff --git a/src/main/drivers/usb_msc_h7xx.c b/src/main/drivers/usb_msc_h7xx.c index 9bd6bdc8b37..396a5951ba8 100644 --- a/src/main/drivers/usb_msc_h7xx.c +++ b/src/main/drivers/usb_msc_h7xx.c @@ -84,7 +84,9 @@ uint8_t mscStart(void) IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, 0, 0); IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, 0, 0); - USBD_Init(&USBD_Device, &VCP_Desc, 0); + // Use MSC descriptor for standalone MSC mode (not composite) + // This fixes Windows enumeration issue with new USB library v2.11.3 + USBD_Init(&USBD_Device, &MSC_Desc, 0); /** Regsiter class */ USBD_RegisterClass(&USBD_Device, USBD_MSC_CLASS); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index c60dccfb25a..6c60f08c6ed 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -73,7 +73,7 @@ bool cliMode = false; #include "fc/fc_core.h" #include "fc/cli.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" @@ -3731,7 +3731,7 @@ static void cliDumpControlProfile(uint8_t profileIndex, uint8_t dumpMask) cliPrintHashLine("control_profile"); cliPrintLinef("control_profile %d\r\n", getConfigProfile() + 1); dumpAllValues(PROFILE_VALUE, dumpMask); - dumpAllValues(CONTROL_RATE_VALUE, dumpMask); + dumpAllValues(CONTROL_VALUE, dumpMask); dumpAllValues(EZ_TUNE_VALUE, dumpMask); } diff --git a/src/main/fc/config.c b/src/main/fc/config.c index fb2a83061d8..d3021317ae5 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -67,7 +67,7 @@ #include "flight/ez_tune.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "fc/rc_curves.h" @@ -321,7 +321,7 @@ void resetConfigs(void) static void activateConfig(void) { - activateControlRateConfig(); + activateControlConfig(); activateBatteryProfile(); activateMixerConfig(); @@ -443,7 +443,7 @@ bool setConfigProfile(uint8_t profileIndex) pgActivateProfile(profileIndex); systemConfigMutable()->current_profile_index = profileIndex; // set the control rate profile to match - setControlRateProfile(profileIndex); + setControlProfile(profileIndex); #ifdef USE_EZ_TUNE ezTuneUpdate(); #endif diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 17c8ded8c1e..e3bde5f3eb7 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -63,7 +63,7 @@ typedef enum { FEATURE_PWM_OUTPUT_ENABLE = 1 << 28, FEATURE_OSD = 1 << 29, FEATURE_FW_LAUNCH = 1 << 30, - FEATURE_FW_AUTOTRIM = 1 << 31, + FEATURE_FW_AUTOTRIM = 1U << 31, } features_e; typedef struct systemConfig_s { diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/control_profile.c similarity index 76% rename from src/main/fc/controlrate_profile.c rename to src/main/fc/control_profile.c index c8b144e136c..316643b343f 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/control_profile.c @@ -27,26 +27,27 @@ #include "config/parameter_group_ids.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/rc_curves.h" #include "fc/settings.h" -const controlRateConfig_t *currentControlRateProfile; +const controlConfig_t *currentControlProfile; +PG_REGISTER_ARRAY_WITH_RESET_FN(controlConfig_t, MAX_CONTROL_PROFILE_COUNT, controlProfiles, PG_CONTROL_PROFILES, 0); -PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 4); - -void pgResetFn_controlRateProfiles(controlRateConfig_t *instance) +void pgResetFn_controlProfiles(controlConfig_t *instance) { - for (int i = 0; i < MAX_CONTROL_RATE_PROFILE_COUNT; i++) { - RESET_CONFIG(controlRateConfig_t, &instance[i], + for (int i = 0; i < MAX_CONTROL_PROFILE_COUNT; i++) { + RESET_CONFIG(controlConfig_t, &instance[i], .throttle = { .rcMid8 = SETTING_THR_MID_DEFAULT, .rcExpo8 = SETTING_THR_EXPO_DEFAULT, .dynPID = SETTING_TPA_RATE_DEFAULT, .dynPID_on_YAW = SETTING_TPA_ON_YAW_DEFAULT, .pa_breakpoint = SETTING_TPA_BREAKPOINT_DEFAULT, - .fixedWingTauMs = SETTING_FW_TPA_TIME_CONSTANT_DEFAULT + .fixedWingTauMs = SETTING_FW_TPA_TIME_CONSTANT_DEFAULT, + .apa_pow = SETTING_APA_POW_DEFAULT, + .tpa_pitch_compensation = SETTING_TPA_PITCH_COMPENSATION_DEFAULT }, .stabilized = { @@ -82,21 +83,21 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *instance) } } -void setControlRateProfile(uint8_t profileIndex) +void setControlProfile(uint8_t profileIndex) { - if (profileIndex >= MAX_CONTROL_RATE_PROFILE_COUNT) { + if (profileIndex >= MAX_CONTROL_PROFILE_COUNT) { profileIndex = 0; } - currentControlRateProfile = controlRateProfiles(profileIndex); + currentControlProfile = controlProfiles(profileIndex); } -void activateControlRateConfig(void) +void activateControlConfig(void) { - generateThrottleCurve(currentControlRateProfile); + generateThrottleCurve(currentControlProfile); } -void changeControlRateProfile(uint8_t profileIndex) +void changeControlProfile(uint8_t profileIndex) { - setControlRateProfile(profileIndex); - activateControlRateConfig(); + setControlProfile(profileIndex); + activateControlConfig(); } diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/control_profile.h similarity index 62% rename from src/main/fc/controlrate_profile.h rename to src/main/fc/control_profile.h index 424e80f1ad8..1d07601f79d 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/control_profile.h @@ -21,16 +21,16 @@ #include "config/parameter_group.h" -#include "fc/controlrate_profile_config_struct.h" +#include "fc/control_profile_config_struct.h" #include "fc/settings.h" -#define MAX_CONTROL_RATE_PROFILE_COUNT SETTING_CONSTANT_MAX_CONTROL_RATE_PROFILE_COUNT +#define MAX_CONTROL_PROFILE_COUNT SETTING_CONSTANT_MAX_CONTROL_PROFILE_COUNT -PG_DECLARE_ARRAY(controlRateConfig_t, MAX_CONTROL_RATE_PROFILE_COUNT, controlRateProfiles); +PG_DECLARE_ARRAY(controlConfig_t, MAX_CONTROL_PROFILE_COUNT, controlProfiles); -extern const controlRateConfig_t *currentControlRateProfile; +extern const controlConfig_t *currentControlProfile; -void setControlRateProfile(uint8_t profileIndex); -void changeControlRateProfile(uint8_t profileIndex); -void activateControlRateConfig(void); -uint8_t getCurrentControlRateProfile(void); +void setControlProfile(uint8_t profileIndex); +void changeControlProfile(uint8_t profileIndex); +void activateControlConfig(void); +uint8_t getCurrentControlProfile(void); diff --git a/src/main/fc/controlrate_profile_config_struct.h b/src/main/fc/control_profile_config_struct.h similarity index 86% rename from src/main/fc/controlrate_profile_config_struct.h rename to src/main/fc/control_profile_config_struct.h index e4038537603..9300858fe36 100644 --- a/src/main/fc/controlrate_profile_config_struct.h +++ b/src/main/fc/control_profile_config_struct.h @@ -23,7 +23,7 @@ #include #include -typedef struct controlRateConfig_s { +typedef struct controlConfig_s { struct { uint8_t rcMid8; @@ -32,6 +32,8 @@ typedef struct controlRateConfig_s { bool dynPID_on_YAW; uint16_t pa_breakpoint; // Breakpoint where TPA is activated uint16_t fixedWingTauMs; // Time constant of airplane TPA PT1-filter + uint16_t apa_pow; // Use airspeed instead of throttle position for TPA calculation,0 to disable + uint8_t tpa_pitch_compensation; // Pitch angle based throttle compensation for fixed wing } throttle; struct { @@ -61,4 +63,4 @@ typedef struct controlRateConfig_s { } rateDynamics; #endif -} controlRateConfig_t; +} controlConfig_t; diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index b2a6c1c8026..ce874c8aedc 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -22,6 +22,7 @@ #include "platform.h" #include "blackbox/blackbox.h" +#include "blackbox/blackbox_io.h" #include "build/debug.h" @@ -53,7 +54,7 @@ #include "fc/fc_core.h" #include "fc/cli.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/multifunction.h" #include "fc/rc_adjustments.h" #include "fc/rc_smoothing.h" @@ -279,20 +280,16 @@ static void updateArmingStatus(void) #endif #ifdef USE_GEOZONE - if (feature(FEATURE_GEOZONE) && geozoneIsBlockingArming()) { - ENABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE); - } else { - DISABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE); - } + if (feature(FEATURE_GEOZONE) && geozoneIsBlockingArming()) { + ENABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE); + } else { + DISABLE_ARMING_FLAG(ARMING_DISABLED_GEOZONE); + } #endif /* CHECK: */ - if ( - sensors(SENSOR_ACC) && - !STATE(ACCELEROMETER_CALIBRATED) && - // Require ACC calibration only if any of the setting might require it - isAccRequired() - ) { + // Require ACC calibration only if any of the setting might require it + if (sensors(SENSOR_ACC) && !STATE(ACCELEROMETER_CALIBRATED) && isAccRequired()) { ENABLE_ARMING_FLAG(ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED); } else { @@ -394,15 +391,15 @@ static void processPilotAndFailSafeActions(float dT) } else { // Compute ROLL PITCH and YAW command - rcCommand[ROLL] = getAxisRcCommand(rxGetChannelValue(ROLL), FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband); - rcCommand[PITCH] = getAxisRcCommand(rxGetChannelValue(PITCH), FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband); - rcCommand[YAW] = -getAxisRcCommand(rxGetChannelValue(YAW), FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcYawExpo8 : currentControlRateProfile->stabilized.rcYawExpo8, rcControlsConfig()->yaw_deadband); + rcCommand[ROLL] = getAxisRcCommand(rxGetChannelValue(ROLL), FLIGHT_MODE(MANUAL_MODE) ? currentControlProfile->manual.rcExpo8 : currentControlProfile->stabilized.rcExpo8, rcControlsConfig()->deadband); + rcCommand[PITCH] = getAxisRcCommand(rxGetChannelValue(PITCH), FLIGHT_MODE(MANUAL_MODE) ? currentControlProfile->manual.rcExpo8 : currentControlProfile->stabilized.rcExpo8, rcControlsConfig()->deadband); + rcCommand[YAW] = -getAxisRcCommand(rxGetChannelValue(YAW), FLIGHT_MODE(MANUAL_MODE) ? currentControlProfile->manual.rcYawExpo8 : currentControlProfile->stabilized.rcYawExpo8, rcControlsConfig()->yaw_deadband); // Apply manual control rates if (FLIGHT_MODE(MANUAL_MODE)) { - rcCommand[ROLL] = rcCommand[ROLL] * currentControlRateProfile->manual.rates[FD_ROLL] / 100L; - rcCommand[PITCH] = rcCommand[PITCH] * currentControlRateProfile->manual.rates[FD_PITCH] / 100L; - rcCommand[YAW] = rcCommand[YAW] * currentControlRateProfile->manual.rates[FD_YAW] / 100L; + rcCommand[ROLL] = rcCommand[ROLL] * currentControlProfile->manual.rates[FD_ROLL] / 100L; + rcCommand[PITCH] = rcCommand[PITCH] * currentControlProfile->manual.rates[FD_PITCH] / 100L; + rcCommand[YAW] = rcCommand[YAW] * currentControlProfile->manual.rates[FD_YAW] / 100L; } else { DEBUG_SET(DEBUG_RATE_DYNAMICS, 0, rcCommand[ROLL]); rcCommand[ROLL] = applyRateDynamics(rcCommand[ROLL], ROLL, dT); @@ -442,12 +439,6 @@ void disarm(disarmReason_t disarmReason) lastDisarmTimeUs = micros(); DISABLE_ARMING_FLAG(ARMED); DISABLE_STATE(IN_FLIGHT_EMERG_REARM); - -#ifdef USE_BLACKBOX - if (feature(FEATURE_BLACKBOX)) { - blackboxFinish(); - } -#endif #ifdef USE_DSHOT if (FLIGHT_MODE(TURTLE_MODE)) { sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_NORMAL); @@ -591,16 +582,6 @@ void tryArm(void) resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); -#ifdef USE_BLACKBOX - if (feature(FEATURE_BLACKBOX)) { - serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP); - if (sharedBlackboxAndMspPort) { - mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort); - } - blackboxStart(); - } -#endif - //beep to indicate arming if (navigationPositionEstimateIsHealthy()) { beeper(BEEPER_ARMING_GPS_FIX); @@ -675,7 +656,7 @@ void processRx(timeUs_t currentTimeUs) if (!cliMode) { bool canUseRxData = rxIsReceivingSignal() && !FLIGHT_MODE(FAILSAFE_MODE); updateAdjustmentStates(canUseRxData); - processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData); + processRcAdjustments(CONST_CAST(controlConfig_t*, currentControlProfile), canUseRxData); } // Angle mode forced on briefly after emergency inflight rearm to help stabilise attitude (currently limited to MR) @@ -882,6 +863,44 @@ static void applyThrottleTiltCompensation(void) } } +bool isMspConfigActive(bool isActive) +{ + static timeMs_t lastActive = 0; + + if (isActive) { + lastActive = millis(); + } + + return millis() - lastActive < 1000; +} +#ifdef USE_BLACKBOX +static void processBlackbox(void) +{ + if (getBlackboxState() == BLACKBOX_STATE_DISABLED || isBlackboxDeviceFull()) { + return; + } + + /* Logging with arm_control set to -1 inhibited when connected to Configurator to avoid Blackbox setting issues */ + if (getBlackboxState() == BLACKBOX_STATE_STOPPED) { + if ((blackboxConfig()->arm_control == -1 && !areSensorsCalibrating() && !isMspConfigActive(NULL)) || ARMING_FLAG(ARMED)) { + serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP); + if (sharedBlackboxAndMspPort) { + mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort); + } + + blackboxStart(); + } + } else if (!ARMING_FLAG(ARMED)) { + if ((blackboxConfig()->arm_control == -1 && isMspConfigActive(NULL)) || + (blackboxConfig()->arm_control >= 0 && micros() - lastDisarmTimeUs > (timeUs_t)(USECS_PER_SEC * blackboxConfig()->arm_control))) { + + blackboxFinish(); + } + } + + blackboxUpdate(micros()); +} +#endif void taskMainPidLoop(timeUs_t currentTimeUs) { @@ -983,7 +1002,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) #ifdef USE_BLACKBOX if (!cliMode && feature(FEATURE_BLACKBOX)) { - blackboxUpdate(micros()); + processBlackbox(); } #endif } diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index ee5764696d8..02a5c889a65 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -47,5 +47,5 @@ bool areSensorsCalibrating(void); float getFlightTime(void); void resetFlightTime(void); float getArmTime(void); - -void fcReboot(bool bootLoader); \ No newline at end of file +void fcReboot(bool bootLoader); +bool isMspConfigActive(bool isActive); \ No newline at end of file diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 7983ade44cd..a8ca6c0c199 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -89,6 +89,7 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" #include "fc/firmware_update.h" +#include "fc/stats.h" #include "flight/failsafe.h" #include "flight/imu.h" @@ -746,5 +747,7 @@ void init(void) persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE); #endif + statsInit(); + systemState |= SYSTEM_STATE_READY; } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 23b400aaf37..3e81b028de2 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -64,7 +64,7 @@ #include "fc/fc_core.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/fc_msp.h" #include "fc/fc_msp_box.h" #include "fc/firmware_update.h" @@ -422,6 +422,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, getHwRangefinderStatus()); sbufWriteU8(dst, getHwPitotmeterStatus()); sbufWriteU8(dst, getHwOpticalFlowStatus()); + + isMspConfigActive(true); // used to indicate configurator connection active break; case MSP_ACTIVEBOXES: @@ -556,22 +558,35 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; #ifdef USE_PROGRAMMING_FRAMEWORK case MSP2_INAV_LOGIC_CONDITIONS: - for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) { - sbufWriteU8(dst, logicConditions(i)->enabled); - sbufWriteU8(dst, logicConditions(i)->activatorId); - sbufWriteU8(dst, logicConditions(i)->operation); - sbufWriteU8(dst, logicConditions(i)->operandA.type); - sbufWriteU32(dst, logicConditions(i)->operandA.value); - sbufWriteU8(dst, logicConditions(i)->operandB.type); - sbufWriteU32(dst, logicConditions(i)->operandB.value); - sbufWriteU8(dst, logicConditions(i)->flags); - } - break; + return false; // Deprecated, causes buffer overflow for 14*64 bytes. case MSP2_INAV_LOGIC_CONDITIONS_STATUS: for (int i = 0; i < MAX_LOGIC_CONDITIONS; i++) { sbufWriteU32(dst, logicConditionGetValue(i)); } break; + case MSP2_INAV_LOGIC_CONDITIONS_CONFIGURED: + { + // Returns 8-byte bitmask where bit N = 1 if logic condition N is configured (non-default) + uint64_t mask = 0; + for (int i = 0; i < MIN(MAX_LOGIC_CONDITIONS, 64); i++) { + const logicCondition_t *lc = logicConditions(i); + // Check if any field differs from default reset values + bool isConfigured = (lc->enabled != 0) || + (lc->activatorId != -1) || + (lc->operation != 0) || + (lc->operandA.type != LOGIC_CONDITION_OPERAND_TYPE_VALUE) || + (lc->operandA.value != 0) || + (lc->operandB.type != LOGIC_CONDITION_OPERAND_TYPE_VALUE) || + (lc->operandB.value != 0) || + (lc->flags != 0); + if (isConfigured) { + mask |= ((uint64_t)1 << i); + } + } + sbufWriteU32(dst, (uint32_t)(mask & 0xFFFFFFFF)); // Lower 32 bits + sbufWriteU32(dst, (uint32_t)((mask >> 32) & 0xFFFFFFFF)); // Upper 32 bits + } + break; case MSP2_INAV_GVAR_STATUS: for (int i = 0; i < MAX_GLOBAL_VARIABLES; i++) { sbufWriteU32(dst, gvGet(i)); @@ -640,6 +655,17 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #endif break; + case MSP2_INAV_FULL_LOCAL_POSE: + sbufWriteU16(dst, attitude.values.roll); + sbufWriteU16(dst, attitude.values.pitch); + sbufWriteU16(dst, attitude.values.yaw); + const navEstimatedPosVel_t *absoluteActualState = &posControl.actualState.abs; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + sbufWriteU32(dst, (int32_t)lrintf(absoluteActualState->pos.v[axis])); + sbufWriteU16(dst, (int16_t)lrintf(absoluteActualState->vel.v[axis])); + } + break; + case MSP_SONAR_ALTITUDE: #ifdef USE_RANGEFINDER sbufWriteU32(dst, rangefinderGetLatestAltitude()); @@ -690,36 +716,36 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP_RC_TUNING: sbufWriteU8(dst, 100); //rcRate8 kept for compatibity reasons, this setting is no longer used - sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8); + sbufWriteU8(dst, currentControlProfile->stabilized.rcExpo8); for (int i = 0 ; i < 3; i++) { - sbufWriteU8(dst, currentControlRateProfile->stabilized.rates[i]); // R,P,Y see flight_dynamics_index_t + sbufWriteU8(dst, currentControlProfile->stabilized.rates[i]); // R,P,Y see flight_dynamics_index_t } - sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID); - sbufWriteU8(dst, currentControlRateProfile->throttle.rcMid8); - sbufWriteU8(dst, currentControlRateProfile->throttle.rcExpo8); - sbufWriteU16(dst, currentControlRateProfile->throttle.pa_breakpoint); - sbufWriteU8(dst, currentControlRateProfile->stabilized.rcYawExpo8); + sbufWriteU8(dst, currentControlProfile->throttle.dynPID); + sbufWriteU8(dst, currentControlProfile->throttle.rcMid8); + sbufWriteU8(dst, currentControlProfile->throttle.rcExpo8); + sbufWriteU16(dst, currentControlProfile->throttle.pa_breakpoint); + sbufWriteU8(dst, currentControlProfile->stabilized.rcYawExpo8); break; case MSP2_INAV_RATE_PROFILE: // throttle - sbufWriteU8(dst, currentControlRateProfile->throttle.rcMid8); - sbufWriteU8(dst, currentControlRateProfile->throttle.rcExpo8); - sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID); - sbufWriteU16(dst, currentControlRateProfile->throttle.pa_breakpoint); + sbufWriteU8(dst, currentControlProfile->throttle.rcMid8); + sbufWriteU8(dst, currentControlProfile->throttle.rcExpo8); + sbufWriteU8(dst, currentControlProfile->throttle.dynPID); + sbufWriteU16(dst, currentControlProfile->throttle.pa_breakpoint); // stabilized - sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8); - sbufWriteU8(dst, currentControlRateProfile->stabilized.rcYawExpo8); + sbufWriteU8(dst, currentControlProfile->stabilized.rcExpo8); + sbufWriteU8(dst, currentControlProfile->stabilized.rcYawExpo8); for (uint8_t i = 0 ; i < 3; ++i) { - sbufWriteU8(dst, currentControlRateProfile->stabilized.rates[i]); // R,P,Y see flight_dynamics_index_t + sbufWriteU8(dst, currentControlProfile->stabilized.rates[i]); // R,P,Y see flight_dynamics_index_t } // manual - sbufWriteU8(dst, currentControlRateProfile->manual.rcExpo8); - sbufWriteU8(dst, currentControlRateProfile->manual.rcYawExpo8); + sbufWriteU8(dst, currentControlProfile->manual.rcExpo8); + sbufWriteU8(dst, currentControlProfile->manual.rcYawExpo8); for (uint8_t i = 0 ; i < 3; ++i) { - sbufWriteU8(dst, currentControlRateProfile->manual.rates[i]); // R,P,Y see flight_dynamics_index_t + sbufWriteU8(dst, currentControlProfile->manual.rates[i]); // R,P,Y see flight_dynamics_index_t } break; @@ -969,8 +995,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } sbufWriteU32(dst, adsbVehicle->vehicleValues.icao); - sbufWriteU32(dst, adsbVehicle->vehicleValues.lat); - sbufWriteU32(dst, adsbVehicle->vehicleValues.lon); + sbufWriteU32(dst, adsbVehicle->vehicleValues.gps.lat); + sbufWriteU32(dst, adsbVehicle->vehicleValues.gps.lon); sbufWriteU32(dst, adsbVehicle->vehicleValues.alt); sbufWriteU16(dst, (uint16_t)CENTIDEGREES_TO_DEGREES(adsbVehicle->vehicleValues.heading)); sbufWriteU8(dst, adsbVehicle->vehicleValues.tslc); @@ -1582,6 +1608,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, osdConfig()->sidebar_scroll_arrows); sbufWriteU8(dst, osdConfig()->units); sbufWriteU8(dst, osdConfig()->stats_energy_unit); +#ifdef USE_ADSB + sbufWriteU8(dst, osdConfig()->adsb_warning_style); +#else + sbufWriteU8(dst, 0); +#endif break; #endif @@ -1718,12 +1749,12 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP2_INAV_RATE_DYNAMICS: { - sbufWriteU8(dst, currentControlRateProfile->rateDynamics.sensitivityCenter); - sbufWriteU8(dst, currentControlRateProfile->rateDynamics.sensitivityEnd); - sbufWriteU8(dst, currentControlRateProfile->rateDynamics.correctionCenter); - sbufWriteU8(dst, currentControlRateProfile->rateDynamics.correctionEnd); - sbufWriteU8(dst, currentControlRateProfile->rateDynamics.weightCenter); - sbufWriteU8(dst, currentControlRateProfile->rateDynamics.weightEnd); + sbufWriteU8(dst, currentControlProfile->rateDynamics.sensitivityCenter); + sbufWriteU8(dst, currentControlProfile->rateDynamics.sensitivityEnd); + sbufWriteU8(dst, currentControlProfile->rateDynamics.correctionCenter); + sbufWriteU8(dst, currentControlProfile->rateDynamics.correctionEnd); + sbufWriteU8(dst, currentControlProfile->rateDynamics.weightCenter); + sbufWriteU8(dst, currentControlProfile->rateDynamics.weightEnd); } break; @@ -1993,24 +2024,24 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_SET_RC_TUNING: if ((dataSize == 10) || (dataSize == 11)) { sbufReadU8(src); //Read rcRate8, kept for protocol compatibility reasons - // need to cast away const to set controlRateProfile - ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcExpo8 = sbufReadU8(src); + // need to cast away const to set controlProfile + ((controlConfig_t*)currentControlProfile)->stabilized.rcExpo8 = sbufReadU8(src); for (int i = 0; i < 3; i++) { tmp_u8 = sbufReadU8(src); if (i == FD_YAW) { - ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); + ((controlConfig_t*)currentControlProfile)->stabilized.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); } else { - ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); + ((controlConfig_t*)currentControlProfile)->stabilized.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); } } tmp_u8 = sbufReadU8(src); - ((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = MIN(tmp_u8, SETTING_TPA_RATE_MAX); - ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcMid8 = sbufReadU8(src); - ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcExpo8 = sbufReadU8(src); - ((controlRateConfig_t*)currentControlRateProfile)->throttle.pa_breakpoint = sbufReadU16(src); + ((controlConfig_t*)currentControlProfile)->throttle.dynPID = MIN(tmp_u8, SETTING_TPA_RATE_MAX); + ((controlConfig_t*)currentControlProfile)->throttle.rcMid8 = sbufReadU8(src); + ((controlConfig_t*)currentControlProfile)->throttle.rcExpo8 = sbufReadU8(src); + ((controlConfig_t*)currentControlProfile)->throttle.pa_breakpoint = sbufReadU16(src); if (dataSize > 10) { - ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcYawExpo8 = sbufReadU8(src); + ((controlConfig_t*)currentControlProfile)->stabilized.rcYawExpo8 = sbufReadU8(src); } schedulePidGainsUpdate(); @@ -2021,35 +2052,35 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP2_INAV_SET_RATE_PROFILE: if (dataSize == 15) { - controlRateConfig_t *currentControlRateProfile_p = (controlRateConfig_t*)currentControlRateProfile; // need to cast away const to set controlRateProfile + controlConfig_t *currentControlProfile_p = (controlConfig_t*)currentControlProfile; // need to cast away const to set controlProfile // throttle - currentControlRateProfile_p->throttle.rcMid8 = sbufReadU8(src); - currentControlRateProfile_p->throttle.rcExpo8 = sbufReadU8(src); - currentControlRateProfile_p->throttle.dynPID = sbufReadU8(src); - currentControlRateProfile_p->throttle.pa_breakpoint = sbufReadU16(src); + currentControlProfile_p->throttle.rcMid8 = sbufReadU8(src); + currentControlProfile_p->throttle.rcExpo8 = sbufReadU8(src); + currentControlProfile_p->throttle.dynPID = sbufReadU8(src); + currentControlProfile_p->throttle.pa_breakpoint = sbufReadU16(src); // stabilized - currentControlRateProfile_p->stabilized.rcExpo8 = sbufReadU8(src); - currentControlRateProfile_p->stabilized.rcYawExpo8 = sbufReadU8(src); + currentControlProfile_p->stabilized.rcExpo8 = sbufReadU8(src); + currentControlProfile_p->stabilized.rcYawExpo8 = sbufReadU8(src); for (uint8_t i = 0; i < 3; ++i) { tmp_u8 = sbufReadU8(src); if (i == FD_YAW) { - currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); + currentControlProfile_p->stabilized.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); } else { - currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); + currentControlProfile_p->stabilized.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); } } // manual - currentControlRateProfile_p->manual.rcExpo8 = sbufReadU8(src); - currentControlRateProfile_p->manual.rcYawExpo8 = sbufReadU8(src); + currentControlProfile_p->manual.rcExpo8 = sbufReadU8(src); + currentControlProfile_p->manual.rcYawExpo8 = sbufReadU8(src); for (uint8_t i = 0; i < 3; ++i) { tmp_u8 = sbufReadU8(src); if (i == FD_YAW) { - currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); + currentControlProfile_p->manual.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); } else { - currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); + currentControlProfile_p->manual.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); } } @@ -2332,6 +2363,23 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } else return MSP_RESULT_ERROR; break; + + case MSP2_INAV_SET_GVAR: + if (dataSize != 5) { + return MSP_RESULT_ERROR; + } + { + uint8_t gvarIndex; + if (!sbufReadU8Safe(&gvarIndex, src)) { + return MSP_RESULT_ERROR; + } + const int32_t gvarValue = (int32_t)sbufReadU32(src); + if (gvarIndex >= MAX_GLOBAL_VARIABLES) { + return MSP_RESULT_ERROR; + } + gvSet(gvarIndex, gvarValue); + } + break; #endif case MSP2_COMMON_SET_MOTOR_MIXER: sbufReadU8Safe(&tmp_u8, src); @@ -2708,6 +2756,30 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) osdStartFullRedraw(); break; + case MSP2_INAV_OSD_UPDATE_POSITION: { + if (dataSize == 3) { + uint8_t item; + sbufReadU8Safe(&item, src); + if (item >= OSD_ITEM_COUNT) { + return MSP_RESULT_ERROR; + } + + uint16_t pos = sbufReadU16(src); + + osdEraseCustomItem(item); + osdLayoutsConfigMutable()->item_pos[getCurrentLayout()][item] = pos | (1 << 13); + osdDrawCustomItem(item); + + return MSP_RESULT_ACK; + + } else{ + return MSP_RESULT_ERROR; + } + + } + + + case MSP_OSD_CHAR_WRITE: if (dataSize >= 55) { osdCharacter_t chr; @@ -2821,7 +2893,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) #ifdef USE_FLASHFS case MSP_DATAFLASH_ERASE: - flashfsEraseCompletely(); + if (blackboxMayEditConfig()) { + flashfsEraseCompletely(); + } else { + return MSP_RESULT_ERROR; + } break; #endif @@ -3285,7 +3361,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP2_INAV_OSD_SET_PREFERENCES: { - if (dataSize == 9) { + if ( + dataSize == 9 +#ifdef USE_ADSB + || dataSize == 10 +#endif + ) { osdConfigMutable()->video_system = sbufReadU8(src); osdConfigMutable()->main_voltage_decimals = sbufReadU8(src); osdConfigMutable()->ahi_reverse_roll = sbufReadU8(src); @@ -3295,6 +3376,11 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) osdConfigMutable()->sidebar_scroll_arrows = sbufReadU8(src); osdConfigMutable()->units = sbufReadU8(src); osdConfigMutable()->stats_energy_unit = sbufReadU8(src); +#ifdef USE_ADSB + if(dataSize == 10) { + osdConfigMutable()->adsb_warning_style = sbufReadU8(src); + } +#endif osdStartFullRedraw(); } else return MSP_RESULT_ERROR; @@ -3508,12 +3594,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP2_INAV_SET_RATE_DYNAMICS: if (dataSize == 6) { - ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.sensitivityCenter = sbufReadU8(src); - ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.sensitivityEnd = sbufReadU8(src); - ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.correctionCenter = sbufReadU8(src); - ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.correctionEnd = sbufReadU8(src); - ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightCenter = sbufReadU8(src); - ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightEnd = sbufReadU8(src); + ((controlConfig_t*)currentControlProfile)->rateDynamics.sensitivityCenter = sbufReadU8(src); + ((controlConfig_t*)currentControlProfile)->rateDynamics.sensitivityEnd = sbufReadU8(src); + ((controlConfig_t*)currentControlProfile)->rateDynamics.correctionCenter = sbufReadU8(src); + ((controlConfig_t*)currentControlProfile)->rateDynamics.correctionEnd = sbufReadU8(src); + ((controlConfig_t*)currentControlProfile)->rateDynamics.weightCenter = sbufReadU8(src); + ((controlConfig_t*)currentControlProfile)->rateDynamics.weightEnd = sbufReadU8(src); } else { return MSP_RESULT_ERROR; @@ -3754,7 +3840,7 @@ static bool mspSettingInfoCommand(sbuf_t *dst, sbuf_t *src) FALLTHROUGH; case PROFILE_VALUE: FALLTHROUGH; - case CONTROL_RATE_VALUE: + case CONTROL_VALUE: sbufWriteU8(dst, getConfigProfile()); sbufWriteU8(dst, MAX_PROFILE_COUNT); break; diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 47c359b00c2..99d884cbce3 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -36,7 +36,7 @@ #include "drivers/vtx_common.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/rc_adjustments.h" #include "fc/rc_curves.h" #include "fc/settings.h" @@ -373,7 +373,7 @@ static void applyAdjustmentPID(adjustmentFunction_e adjustmentFunction, uint16_t applyAdjustmentU16(adjustmentFunction, val, delta, SETTING_CONSTANT_RPYL_PID_MIN, SETTING_CONSTANT_RPYL_PID_MAX); } -static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) +static void applyStepAdjustment(controlConfig_t *controlConfig, uint8_t adjustmentFunction, int delta) { if (delta > 0) { beeperConfirmationBeeps(2); @@ -382,23 +382,23 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t } switch (adjustmentFunction) { case ADJUSTMENT_RC_EXPO: - applyAdjustmentExpo(ADJUSTMENT_RC_EXPO, &controlRateConfig->stabilized.rcExpo8, delta); + applyAdjustmentExpo(ADJUSTMENT_RC_EXPO, &controlConfig->stabilized.rcExpo8, delta); break; case ADJUSTMENT_RC_YAW_EXPO: - applyAdjustmentExpo(ADJUSTMENT_RC_YAW_EXPO, &controlRateConfig->stabilized.rcYawExpo8, delta); + applyAdjustmentExpo(ADJUSTMENT_RC_YAW_EXPO, &controlConfig->stabilized.rcYawExpo8, delta); break; case ADJUSTMENT_MANUAL_RC_EXPO: - applyAdjustmentExpo(ADJUSTMENT_MANUAL_RC_EXPO, &controlRateConfig->manual.rcExpo8, delta); + applyAdjustmentExpo(ADJUSTMENT_MANUAL_RC_EXPO, &controlConfig->manual.rcExpo8, delta); break; case ADJUSTMENT_MANUAL_RC_YAW_EXPO: - applyAdjustmentExpo(ADJUSTMENT_MANUAL_RC_YAW_EXPO, &controlRateConfig->manual.rcYawExpo8, delta); + applyAdjustmentExpo(ADJUSTMENT_MANUAL_RC_YAW_EXPO, &controlConfig->manual.rcYawExpo8, delta); break; case ADJUSTMENT_THROTTLE_EXPO: - applyAdjustmentExpo(ADJUSTMENT_THROTTLE_EXPO, &controlRateConfig->throttle.rcExpo8, delta); + applyAdjustmentExpo(ADJUSTMENT_THROTTLE_EXPO, &controlConfig->throttle.rcExpo8, delta); break; case ADJUSTMENT_PITCH_ROLL_RATE: case ADJUSTMENT_PITCH_RATE: - applyAdjustmentU8(ADJUSTMENT_PITCH_RATE, &controlRateConfig->stabilized.rates[FD_PITCH], delta, SETTING_PITCH_RATE_MIN, SETTING_PITCH_RATE_MAX); + applyAdjustmentU8(ADJUSTMENT_PITCH_RATE, &controlConfig->stabilized.rates[FD_PITCH], delta, SETTING_PITCH_RATE_MIN, SETTING_PITCH_RATE_MAX); if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { schedulePidGainsUpdate(); break; @@ -407,25 +407,25 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t FALLTHROUGH; case ADJUSTMENT_ROLL_RATE: - applyAdjustmentU8(ADJUSTMENT_ROLL_RATE, &controlRateConfig->stabilized.rates[FD_ROLL], delta, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); + applyAdjustmentU8(ADJUSTMENT_ROLL_RATE, &controlConfig->stabilized.rates[FD_ROLL], delta, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); schedulePidGainsUpdate(); break; case ADJUSTMENT_MANUAL_PITCH_ROLL_RATE: case ADJUSTMENT_MANUAL_ROLL_RATE: - applyAdjustmentManualRate(ADJUSTMENT_MANUAL_ROLL_RATE, &controlRateConfig->manual.rates[FD_ROLL], delta); + applyAdjustmentManualRate(ADJUSTMENT_MANUAL_ROLL_RATE, &controlConfig->manual.rates[FD_ROLL], delta); if (adjustmentFunction == ADJUSTMENT_MANUAL_ROLL_RATE) break; // follow though for combined ADJUSTMENT_MANUAL_PITCH_ROLL_RATE FALLTHROUGH; case ADJUSTMENT_MANUAL_PITCH_RATE: - applyAdjustmentManualRate(ADJUSTMENT_MANUAL_PITCH_RATE, &controlRateConfig->manual.rates[FD_PITCH], delta); + applyAdjustmentManualRate(ADJUSTMENT_MANUAL_PITCH_RATE, &controlConfig->manual.rates[FD_PITCH], delta); break; case ADJUSTMENT_YAW_RATE: - applyAdjustmentU8(ADJUSTMENT_YAW_RATE, &controlRateConfig->stabilized.rates[FD_YAW], delta, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); + applyAdjustmentU8(ADJUSTMENT_YAW_RATE, &controlConfig->stabilized.rates[FD_YAW], delta, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); schedulePidGainsUpdate(); break; case ADJUSTMENT_MANUAL_YAW_RATE: - applyAdjustmentManualRate(ADJUSTMENT_MANUAL_YAW_RATE, &controlRateConfig->manual.rates[FD_YAW], delta); + applyAdjustmentManualRate(ADJUSTMENT_MANUAL_YAW_RATE, &controlConfig->manual.rates[FD_YAW], delta); break; case ADJUSTMENT_PITCH_ROLL_P: case ADJUSTMENT_PITCH_P: @@ -594,13 +594,13 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t break; #endif case ADJUSTMENT_TPA: - applyAdjustmentU8(ADJUSTMENT_TPA, &controlRateConfig->throttle.dynPID, delta, 0, SETTING_TPA_RATE_MAX); + applyAdjustmentU8(ADJUSTMENT_TPA, &controlConfig->throttle.dynPID, delta, 0, SETTING_TPA_RATE_MAX); break; case ADJUSTMENT_TPA_BREAKPOINT: - applyAdjustmentU16(ADJUSTMENT_TPA_BREAKPOINT, &controlRateConfig->throttle.pa_breakpoint, delta, PWM_RANGE_MIN, PWM_RANGE_MAX); + applyAdjustmentU16(ADJUSTMENT_TPA_BREAKPOINT, &controlConfig->throttle.pa_breakpoint, delta, PWM_RANGE_MIN, PWM_RANGE_MAX); break; case ADJUSTMENT_FW_TPA_TIME_CONSTANT: - applyAdjustmentU16(ADJUSTMENT_FW_TPA_TIME_CONSTANT, &controlRateConfig->throttle.fixedWingTauMs, delta, SETTING_FW_TPA_TIME_CONSTANT_MIN, SETTING_FW_TPA_TIME_CONSTANT_MAX); + applyAdjustmentU16(ADJUSTMENT_FW_TPA_TIME_CONSTANT, &controlConfig->throttle.fixedWingTauMs, delta, SETTING_FW_TPA_TIME_CONSTANT_MIN, SETTING_FW_TPA_TIME_CONSTANT_MAX); break; case ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS: applyAdjustmentU8(ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS, &navConfigMutable()->fw.control_smoothness, delta, SETTING_NAV_FW_CONTROL_SMOOTHNESS_MIN, SETTING_NAV_FW_CONTROL_SMOOTHNESS_MAX); @@ -633,8 +633,8 @@ static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) switch (adjustmentFunction) { case ADJUSTMENT_RATE_PROFILE: - if (getCurrentControlRateProfile() != position) { - changeControlRateProfile(position); + if (getCurrentControlProfile() != position) { + changeControlProfile(position); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RATE_PROFILE, position); applied = true; } @@ -649,7 +649,7 @@ static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) #define RESET_FREQUENCY_2HZ (1000 / 2) -void processRcAdjustments(controlRateConfig_t *controlRateConfig, bool canUseRxData) +void processRcAdjustments(controlConfig_t *controlConfig, bool canUseRxData) { const uint32_t now = millis(); @@ -695,7 +695,7 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, bool canUseRxD } // it is legitimate to adjust an otherwise const item here - applyStepAdjustment(controlRateConfig, adjustmentFunction, delta); + applyStepAdjustment(controlConfig, adjustmentFunction, delta); #ifdef USE_INFLIGHT_PROFILE_ADJUSTMENT } else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) { const uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.selectConfig.switchPositions); diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index d900dc82475..684cc8bf148 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -145,7 +145,7 @@ PG_DECLARE_ARRAY(adjustmentRange_t, MAX_ADJUSTMENT_RANGE_COUNT, adjustmentRanges void resetAdjustmentStates(void); void updateAdjustmentStates(bool canUseRxData); -struct controlRateConfig_s; -void processRcAdjustments(struct controlRateConfig_s *controlRateConfig, bool canUseRxData); +struct controlConfig_s; +void processRcAdjustments(struct controlConfig_s *controlConfig, bool canUseRxData); bool isAdjustmentFunctionSelected(uint8_t adjustmentFunction); uint8_t getActiveAdjustmentFunctions(uint8_t *adjustmentFunctions); diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index d03bd466a49..ce9bbab4b23 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -41,7 +41,7 @@ #include "fc/cli.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/fc_core.h" #include "fc/rc_controls.h" #include "fc/rc_curves.h" @@ -134,9 +134,11 @@ bool throttleStickIsLow(void) int16_t throttleStickMixedValue(void) { int16_t throttleValue; + uint16_t lowLimit = feature(FEATURE_REVERSIBLE_MOTORS) ? PWM_RANGE_MIN : rxConfig()->mincheck; + + throttleValue = constrain(rxGetChannelValue(THROTTLE), lowLimit, PWM_RANGE_MAX); + throttleValue = (uint16_t)(throttleValue - lowLimit) * PWM_RANGE_MIN / (PWM_RANGE_MAX - lowLimit); // [LOWLIMIT;2000] -> [0;1000] - throttleValue = constrain(rxGetChannelValue(THROTTLE), rxConfig()->mincheck, PWM_RANGE_MAX); - throttleValue = (uint16_t)(throttleValue - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); // [MINCHECK;2000] -> [0;1000] return rcLookupThrottle(throttleValue); } diff --git a/src/main/fc/rc_curves.c b/src/main/fc/rc_curves.c index 5453083d279..52b9a685b57 100644 --- a/src/main/fc/rc_curves.c +++ b/src/main/fc/rc_curves.c @@ -23,7 +23,7 @@ #include "common/maths.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/rc_controls.h" #include "fc/rc_curves.h" @@ -36,19 +36,19 @@ static EXTENDED_FASTRAM int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE int16_t lookupThrottleRCMid; // THROTTLE curve mid point -void generateThrottleCurve(const controlRateConfig_t *controlRateConfig) +void generateThrottleCurve(const controlConfig_t *controlConfig) { const int minThrottle = getThrottleIdleValue(); - lookupThrottleRCMid = minThrottle + (int32_t)(getMaxThrottle() - minThrottle) * controlRateConfig->throttle.rcMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE] + lookupThrottleRCMid = minThrottle + (int32_t)(getMaxThrottle() - minThrottle) * controlConfig->throttle.rcMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE] for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { - const int16_t tmp = 10 * i - controlRateConfig->throttle.rcMid8; + const int16_t tmp = 10 * i - controlConfig->throttle.rcMid8; uint8_t y = 1; if (tmp > 0) - y = 100 - controlRateConfig->throttle.rcMid8; + y = 100 - controlConfig->throttle.rcMid8; if (tmp < 0) - y = controlRateConfig->throttle.rcMid8; - lookupThrottleRC[i] = 10 * controlRateConfig->throttle.rcMid8 + tmp * (100 - controlRateConfig->throttle.rcExpo8 + (int32_t) controlRateConfig->throttle.rcExpo8 * (tmp * tmp) / (y * y)) / 10; + y = controlConfig->throttle.rcMid8; + lookupThrottleRC[i] = 10 * controlConfig->throttle.rcMid8 + tmp * (100 - controlConfig->throttle.rcExpo8 + (int32_t) controlConfig->throttle.rcExpo8 * (tmp * tmp) / (y * y)) / 10; lookupThrottleRC[i] = minThrottle + (int32_t) (getMaxThrottle() - minThrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] } } diff --git a/src/main/fc/rc_curves.h b/src/main/fc/rc_curves.h index af7cd618e74..531b44fbf17 100644 --- a/src/main/fc/rc_curves.h +++ b/src/main/fc/rc_curves.h @@ -17,8 +17,8 @@ #pragma once -struct controlRateConfig_s; -void generateThrottleCurve(const struct controlRateConfig_s *controlRateConfig); +struct controlConfig_s; +void generateThrottleCurve(const struct controlConfig_s *controlConfig); int16_t rcLookup(int32_t stickDeflection, uint8_t expo); uint16_t rcLookupThrottle(uint16_t tmp); diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 109f430f843..912c38362bf 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -235,8 +235,8 @@ static uint16_t getValueOffset(const setting_t *value) return value->offset; case PROFILE_VALUE: return value->offset + sizeof(pidProfile_t) * getConfigProfile(); - case CONTROL_RATE_VALUE: - return value->offset + sizeof(controlRateConfig_t) * getConfigProfile(); + case CONTROL_VALUE: + return value->offset + sizeof(controlConfig_t) * getConfigProfile(); case EZ_TUNE_VALUE: return value->offset + sizeof(ezTuneSettings_t) * getConfigProfile(); case BATTERY_CONFIG_VALUE: diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index 75e8bdbef32..31c4ee3de77 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -32,7 +32,7 @@ typedef enum { // value section, bits 3-5 MASTER_VALUE = (0 << SETTING_SECTION_OFFSET), PROFILE_VALUE = (1 << SETTING_SECTION_OFFSET), - CONTROL_RATE_VALUE = (2 << SETTING_SECTION_OFFSET), + CONTROL_VALUE = (2 << SETTING_SECTION_OFFSET), BATTERY_CONFIG_VALUE = (3 << SETTING_SECTION_OFFSET), MIXER_CONFIG_VALUE = (4 << SETTING_SECTION_OFFSET), EZ_TUNE_VALUE = (5 << SETTING_SECTION_OFFSET) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 14512c5005e..8675fa0efa8 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -8,7 +8,7 @@ tables: values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO", "USD1_V0", "NRA"] enum: rangefinderType_e - name: mag_hardware - values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"] + values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "QMC5883P", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"] enum: magSensor_e - name: opflow_hardware values: ["NONE", "CXOF", "MSP", "FAKE"] @@ -33,10 +33,10 @@ tables: - name: failsafe_procedure values: ["LAND", "DROP", "RTH", "NONE"] - name: current_sensor - values: ["NONE", "ADC", "VIRTUAL", "FAKE", "ESC"] + values: ["NONE", "ADC", "VIRTUAL", "FAKE", "ESC", "SMARTPORT"] enum: currentSensor_e - name: voltage_sensor - values: ["NONE", "ADC", "ESC", "FAKE"] + values: ["NONE", "ADC", "ESC", "FAKE", "SMARTPORT"] enum: voltageSensor_e - name: imu_inertia_comp_method values: ["VELNED", "TURNRATE","ADAPTIVE"] @@ -201,6 +201,9 @@ tables: - name: mavlink_radio_type values: ["GENERIC", "ELRS", "SIK"] enum: mavlinkRadio_e + - name: mavlink_autopilot_type + values: ["GENERIC", "ARDUPILOT"] + enum: mavlinkAutopilotType_e - name: default_altitude_source values: ["GPS", "BARO", "GPS_ONLY", "BARO_ONLY"] enum: navDefaultAltitudeSensor_e @@ -209,6 +212,9 @@ tables: enum: fenceAction_e - name: geozone_rth_no_way_home values: [RTH, EMRG_LAND] + - name: osd_adsb_warning_style + values: [ "COMPACT", "EXTENDED" ] + enum: osd_adsb_warning_style_e constants: RPYL_PID_MIN: 0 @@ -220,7 +226,7 @@ constants: ROLL_PITCH_RATE_MIN: 4 ROLL_PITCH_RATE_MAX: 180 - MAX_CONTROL_RATE_PROFILE_COUNT: 3 + MAX_CONTROL_PROFILE_COUNT: 3 MAX_BATTERY_PROFILE_COUNT: 3 @@ -665,10 +671,10 @@ groups: default_value: "NONE" table: pitot_hardware - name: pitot_lpf_milli_hz - description: "Pitot tube lowpass filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay" + description: "Pitot tube lowpass filter cutoff frequency in milli Hz(0.001hz). Set as 0 to disable LPF Lower cutoff frequencies result in smoother response at expense of command control delay" min: 0 - max: 10000 - default_value: 350 + max: 50000 + default_value: 3000 - name: pitot_scale description: "Pitot tube scale factor" min: 0 @@ -782,12 +788,12 @@ groups: field: halfDuplex table: tristate - name: msp_override_channels - description: "Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires custom firmware with `USE_RX_MSP` and `USE_MSP_RC_OVERRIDE` compile options and the `MSP RC Override` flight mode." + description: "Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires the `MSP RC Override` flight mode." default_value: 0 field: mspOverrideChannels condition: USE_MSP_RC_OVERRIDE min: 0 - max: 65535 + max: UINT32_MAX - name: PG_BLACKBOX_CONFIG type: blackboxConfig_t @@ -817,6 +823,12 @@ groups: field: invertedCardDetection condition: USE_SDCARD type: bool + - name: blackbox_arm_control + description: "Determines behaviour of logging in relation to Arm state. For settings from 0 to 60 logging will start on Arm with the setting determining how long logging will continue after disarm in seconds, i.e. set to 0 to stop logging at disarm or 10 to stop logging 10s after disarm. Set to -1 to start logging from boot up until power off (Use with caution - mainly for debugging and best used with BLACKBOX mode)." + default_value: 0 + field: arm_control + min: -1 + max: 60 - name: PG_MOTOR_CONFIG type: motorConfig_t @@ -967,7 +979,7 @@ groups: headers: ["sensors/battery_config_structs.h"] members: - name: vbat_meter_type - description: "Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running" + description: "Vbat voltage source. Possible values: `NONE`, `ADC`, `SMARTPORT`, `ESC`. `ESC` requires ESC telemetry enabled and running. `SMARTPORT` requires SmartPort Master enabled and running." condition: USE_ADC default_value: ADC field: voltage.type @@ -999,7 +1011,7 @@ groups: min: -32768 max: 32767 - name: current_meter_type - description: "ADC , VIRTUAL, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position." + description: "ADC, VIRTUAL, FAKE, ESC, SMARTPORT, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position." default_value: "ADC" field: current.type table: current_sensor @@ -1092,12 +1104,12 @@ groups: field: capacity.critical min: 0 max: 4294967295 - - name: controlrate_profile - description: "Control rate profile to switch to when the battery profile is selected, 0 to disable and keep the currently selected control rate profile" + - name: use_control_profile + description: "Control profile to switch to when the battery profile is selected, 0 to disable and keep the currently selected control profile" default_value: 0 - field: controlRateProfile + field: controlProfile min: 0 - max: MAX_CONTROL_RATE_PROFILE_COUNT + max: MAX_CONTROL_PROFILE_COUNT - name: throttle_scale description: "Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50%" @@ -1246,10 +1258,10 @@ groups: default_value: ON field: mixer_config.motorstopOnLow type: bool - - name: mixer_pid_profile_linking - description: "If enabled, pid profile_index will follow mixer_profile index. Set to OFF(default) if you want to handle PID profile by your self. Recommend to set to ON on all mixer_profiles to let the mixer_profile handle the PID profile switching on a VTOL or mixed platform type setup." + - name: mixer_control_profile_linking + description: "If enabled, control_profile_index will follow mixer_profile index. Set to OFF(default) if you want to handle control_profile by your self. Recommend to set to ON on all mixer_profiles to let the mixer_profile handle the control_profile switching on a VTOL or mixed platform type setup." default_value: OFF - field: mixer_config.PIDProfileLinking + field: mixer_config.controlProfileLinking type: bool - name: mixer_automated_switch description: "If set to on, This mixer_profile will try to switch to another mixer_profile when 1.RTH heading home is requested and distance to home is lager than 3*nav_fw_loiter_radius on mixer_profile is a MULTIROTOR or TRICOPTER platform_type. 2. RTH landing is requested on this mixer_profile is a AIRPLANE platform_type" @@ -1267,6 +1279,24 @@ groups: default_value: OFF field: mixer_config.tailsitterOrientationOffset type: bool + - name: transition_pid_mmix_multiplier_roll + description: "roll axis pid multiplier applied to motor mixer only on mixer trasition mode, 1000(default) is 1.0x, -1000 is 1.0x on opposite" + default_value: 1000 + field: mixer_config.transition_PID_mmix_multiplier_roll + min: -5000 + max: 5000 + - name: transition_pid_mmix_multiplier_pitch + description: "pitch axis pid multiplier applied to motor mixer only on mixer trasition mode, 1000(default) is 1.0x, -1000 is 1.0x on opposite" + default_value: 1000 + field: mixer_config.transition_PID_mmix_multiplier_pitch + min: -5000 + max: 5000 + - name: transition_pid_mmix_multiplier_yaw + description: "yaw axis pid multiplier applied to motor mixer only on mixer trasition mode, 1000(default) is 1.0x, -1000 is 1.0x on opposite" + default_value: 1000 + field: mixer_config.transition_PID_mmix_multiplier_yaw + min: -5000 + max: 5000 - name: PG_REVERSIBLE_MOTORS_CONFIG type: reversibleMotorsConfig_t @@ -1332,10 +1362,10 @@ groups: min: 1 max: 60 - - name: PG_CONTROL_RATE_PROFILES - type: controlRateConfig_t - headers: ["fc/controlrate_profile_config_struct.h"] - value_type: CONTROL_RATE_VALUE + - name: PG_CONTROL_PROFILES + type: controlConfig_t + headers: ["fc/control_profile_config_struct.h"] + value_type: CONTROL_VALUE members: - name: thr_mid description: "Throttle value when the stick is set to mid-position. Used in the throttle curve calculation." @@ -1344,23 +1374,36 @@ groups: min: 0 max: 100 - name: thr_expo - description: "Throttle exposition value" + description: "Throttle exponential value" default_value: 0 field: throttle.rcExpo8 min: 0 max: 100 + - name: apa_pow + description: "Use airspeed instead of throttle position for PID attenuation if airspeed is available on fixedwing. pid_multiplier = (referenceAirspeed/airspeed)**(apa_pow/100). Set to 0 will disable this feature and use throttle based PID attenuation;" + type: uint16_t + field: throttle.apa_pow + default_value: 120 + min: 0 + max: 200 - name: tpa_rate - description: "Throttle PID attenuation reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate." + description: "Throttle based PID attenuation(TPA) reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. On multirotor, For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. for fixedwing modifies PIDFF. See **PID Attenuation and scaling** Wiki for full details." default_value: 0 field: throttle.dynPID min: 0 - max: 100 + max: 200 - name: tpa_breakpoint description: "See tpa_rate." default_value: 1500 field: throttle.pa_breakpoint min: PWM_RANGE_MIN max: PWM_RANGE_MAX + - name: tpa_pitch_compensation + description: "Pitch angle based throttle compensation for fixed wing. Positive values will increase throttle when pitching up, and decrease throttle when pitching down." + default_value: 8 + field: throttle.tpa_pitch_compensation + min: 0 + max: 20 - name: tpa_on_yaw description: "Throttle PID attenuation also reduces influence on YAW for multi-rotor, Should be set to ON for tilting rotors." type: bool @@ -1373,13 +1416,13 @@ groups: min: 0 max: 5000 - name: rc_expo - description: "Exposition value used for the PITCH/ROLL axes by all the stabilized flights modes (all but `MANUAL`)" + description: "Exponential value used for the PITCH/ROLL axes by all the stabilized flights modes (all but `MANUAL`)" default_value: 70 field: stabilized.rcExpo8 min: 0 max: 100 - name: rc_yaw_expo - description: "Exposition value used for the YAW axis by all the stabilized flights modes (all but `MANUAL`)" + description: "Exponential value used for the YAW axis by all the stabilized flights modes (all but `MANUAL`)" default_value: 20 field: stabilized.rcYawExpo8 min: 0 @@ -1405,13 +1448,13 @@ groups: min: 1 max: 180 - name: manual_rc_expo - description: "Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100]" + description: "Exponential value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100]" default_value: 35 field: manual.rcExpo8 min: 0 max: 100 - name: manual_rc_yaw_expo - description: "Exposition value used for the YAW axis by the `MANUAL` flight mode [0-100]" + description: "Exponential value used for the YAW axis by the `MANUAL` flight mode [0-100]" default_value: 20 field: manual.rcYawExpo8 min: 0 @@ -1483,16 +1526,6 @@ groups: description: "The end stick weight for Rate Dynamics" condition: USE_RATE_DYNAMICS - - name: PG_SERIAL_CONFIG - type: serialConfig_t - headers: ["io/serial.h"] - members: - - name: reboot_character - description: "Special character used to trigger reboot" - default_value: 82 - min: 48 - max: 126 - - name: PG_IMU_CONFIG type: imuConfig_t headers: ["flight/imu.h"] @@ -2160,8 +2193,8 @@ groups: min: 0 max: 255 - name: nav_fw_pos_z_ff - description: "FF gain of altitude PID controller (Fixedwing)" - default_value: 10 + description: "FF gain of altitude PID controller. Not used if nav_fw_alt_use_position is set ON (Fixedwing)" + default_value: 30 field: bank_fw.pid[PID_POS_Z].FF min: 0 max: 255 @@ -2171,6 +2204,11 @@ groups: field: fwAltControlResponseFactor min: 5 max: 100 + - name: nav_fw_alt_use_position + description: "Use position for fixed wing altitude control rather than velocity (default method)." + default_value: OFF + field: fwAltControlUsePos + type: bool - name: nav_fw_pos_xy_p description: "P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH" default_value: 75 @@ -2441,19 +2479,19 @@ groups: field: w_z_baro_v min: 0 max: 10 - default_value: 0.1 + default_value: 0.35 - name: inav_w_z_gps_p description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors." field: w_z_gps_p min: 0 max: 10 - default_value: 0.2 + default_value: 0.35 - name: inav_w_z_gps_v description: "Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors." field: w_z_gps_v min: 0 max: 10 - default_value: 0.1 + default_value: 0.35 - name: inav_w_xy_gps_p description: "Weight of GPS coordinates in estimated UAV position and speed." default_value: 1.0 @@ -3193,6 +3231,12 @@ groups: min: 0 max: 255 default_value: 2 + - name: mavlink_autopilot_type + field: mavlink.autopilot_type + description: "Autopilot type to advertise for MAVLink telemetry" + table: mavlink_autopilot_type + default_value: "GENERIC" + type: uint8_t - name: mavlink_rc_chan_rate description: "Rate of the RC channels message for MAVLink telemetry" field: mavlink.rc_channels_rate @@ -3247,6 +3291,13 @@ groups: table: mavlink_radio_type default_value: "GENERIC" type: uint8_t + - name: mavlink_sysid + field: mavlink.sysid + description: "MAVLink System ID" + type: uint8_t + min: 1 + max: 255 + default_value: 1 - name: PG_OSD_CONFIG type: osdConfig_t @@ -3260,7 +3311,7 @@ groups: type: uint8_t default_value: "OFF" - name: osd_video_system - description: "Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, 'DJIWTF', 'AVATAR' and `BF43COMPAT`" + description: "Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, 'DJIWTF', 'AVATAR', `BF43COMPAT`, `BFHDCOMPAT` and `DJI_NATIVE`" default_value: "AUTO" table: osd_video_system field: video_system @@ -3278,6 +3329,13 @@ groups: max: 600 type: int16_t field: msp_displayport_fullframe_interval + - name: osd_framerate_hz + description: "OSD element refresh rate in Hz. Controls how often OSD elements are updated (except artificial horizon and telemetry which are always updated). Higher values provide smoother updates but increase CPU load. Set to -1 for legacy mode (one element per frame). Default: -1" + default_value: -1 + min: -1 + max: 60 + type: int8_t + field: osd_framerate_hz - name: osd_units description: "IMPERIAL, METRIC, UK" default_value: "METRIC" @@ -3399,7 +3457,7 @@ groups: default_value: 4 field: snr_alarm min: -20 - max: 99 + max: 30 - name: osd_link_quality_alarm condition: USE_SERIALRX_CRSF || USE_RX_MSP description: "LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50%" @@ -3460,7 +3518,6 @@ groups: field: ahi_max_pitch min: 10 max: 90 - default_value: 20 - name: osd_crosshairs_style description: "To set the visual type for the crosshair" default_value: "DEFAULT" @@ -3633,6 +3690,13 @@ groups: min: 0 max: 64000 type: uint16_t + - name: osd_adsb_warning_style + description: "ADSB warning element style, how rich information on scree will be, Possible values are `COMPACT` and `EXTENDED`" + default_value: "COMPACT" + table: osd_adsb_warning_style + field: adsb_warning_style + type: uint8_t + condition: USE_ADSB - name: osd_estimations_wind_compensation description: "Use wind estimation for remaining flight time/distance estimation" default_value: ON @@ -3730,9 +3794,9 @@ groups: min: 0 max: 16 default_value: 0 - - name: osd_pan_servo_pwm2centideg - description: Centidegrees of pan servo rotation us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. - field: pan_servo_pwm2centideg + - name: osd_pan_servo_range_decadegrees + description: Decadegrees of pan servo rotation. A servo with 180 degrees of rotation typically needs `18` for this setting. Using a negative value will reverse the direction. + field: osd_pan_servo_range_decadegrees default_value: 0 min: -36 max: 36 @@ -3931,6 +3995,10 @@ groups: max: INT32_MAX condition: USE_ADC default_value: 0 + - name: stats_flight_count + description: "Total number of flights. The value is updated on every disarm when \"stats\" are enabled." + default_value: 0 + max: UINT16_MAX - name: PG_TIME_CONFIG type: timeConfig_t diff --git a/src/main/fc/stats.c b/src/main/fc/stats.c index f719ccb0d61..2fc4c63633d 100644 --- a/src/main/fc/stats.c +++ b/src/main/fc/stats.c @@ -5,8 +5,10 @@ #include "fc/settings.h" #include "fc/stats.h" +#include "fc/runtime_config.h" #include "sensors/battery.h" +#include "sensors/sensors.h" #include "drivers/time.h" #include "navigation/navigation.h" @@ -16,14 +18,16 @@ #include "config/parameter_group_ids.h" #define MIN_FLIGHT_TIME_TO_RECORD_STATS_S 10 //prevent recording stats for that short "flights" [s] +#define MIN_FLIGHT_DISTANCE_M 30 // minimum distance flown for a flight to be registered [m] -PG_REGISTER_WITH_RESET_TEMPLATE(statsConfig_t, statsConfig, PG_STATS_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(statsConfig_t, statsConfig, PG_STATS_CONFIG, 2); PG_RESET_TEMPLATE(statsConfig_t, statsConfig, .stats_enabled = SETTING_STATS_DEFAULT, .stats_total_time = SETTING_STATS_TOTAL_TIME_DEFAULT, .stats_total_dist = SETTING_STATS_TOTAL_DIST_DEFAULT, + .stats_flight_count = SETTING_STATS_FLIGHT_COUNT_DEFAULT, #ifdef USE_ADC .stats_total_energy = SETTING_STATS_TOTAL_ENERGY_DEFAULT #endif @@ -31,6 +35,7 @@ PG_RESET_TEMPLATE(statsConfig_t, statsConfig, static uint32_t arm_millis; static uint32_t arm_distance_cm; +static uint16_t prev_flight_count; #ifdef USE_ADC static uint32_t arm_mWhDrawn; @@ -41,6 +46,11 @@ uint32_t getFlyingEnergy(void) { } #endif +void statsInit(void) +{ + prev_flight_count = statsConfig()->stats_flight_count; +} + void statsOnArm(void) { arm_millis = millis(); @@ -57,6 +67,18 @@ void statsOnDisarm(void) if (dt >= MIN_FLIGHT_TIME_TO_RECORD_STATS_S) { statsConfigMutable()->stats_total_time += dt; //[s] statsConfigMutable()->stats_total_dist += (getTotalTravelDistance() - arm_distance_cm) / 100; //[m] +#ifdef USE_GPS + // flight counter is incremented at most once per power on + if (sensors(SENSOR_GPS)) { + if ((getTotalTravelDistance() - arm_distance_cm) / 100 >= MIN_FLIGHT_DISTANCE_M) { + statsConfigMutable()->stats_flight_count = prev_flight_count + 1; + } + } else { + statsConfigMutable()->stats_flight_count = prev_flight_count + 1; + } +#else + statsConfigMutable()->stats_flight_count = prev_flight_count + 1; +#endif // USE_GPS #ifdef USE_ADC if (feature(FEATURE_VBAT) && isAmperageConfigured()) { const uint32_t energy = getMWhDrawn() - arm_mWhDrawn; diff --git a/src/main/fc/stats.h b/src/main/fc/stats.h index 54b676f6577..d0b4b162b60 100644 --- a/src/main/fc/stats.h +++ b/src/main/fc/stats.h @@ -5,6 +5,7 @@ typedef struct statsConfig_s { uint32_t stats_total_time; // [Seconds] uint32_t stats_total_dist; // [Metres] + uint16_t stats_flight_count; #ifdef USE_ADC uint32_t stats_total_energy; // deciWatt hour (x0.1Wh) #endif @@ -12,11 +13,13 @@ typedef struct statsConfig_s { } statsConfig_t; uint32_t getFlyingEnergy(void); +void statsInit(void); void statsOnArm(void); void statsOnDisarm(void); #else +#define statsInit() do {} while (0) #define statsOnArm() do {} while (0) #define statsOnDisarm() do {} while (0) diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index e67e3fe9b47..68707849071 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -32,7 +32,7 @@ #include "fc/settings.h" #include "flight/pid.h" #include "sensors/gyro.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "rx/rx.h" @@ -136,12 +136,12 @@ void ezTuneUpdate(void) { pidProfileMutable()->bank_mc.pid[PID_YAW].FF = pidDefaultsYaw[3] * getYawPidScale(ezTune()->aggressiveness); //Setup rates - ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_ROLL] = scaleRange(ezTune()->rate, 0, 200, 30, 90); - ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_PITCH] = scaleRange(ezTune()->rate, 0, 200, 30, 90); - ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_YAW] = scaleRange(ezTune()->rate, 0, 200, 30, 90) - 10; + ((controlConfig_t*)currentControlProfile)->stabilized.rates[FD_ROLL] = scaleRange(ezTune()->rate, 0, 200, 30, 90); + ((controlConfig_t*)currentControlProfile)->stabilized.rates[FD_PITCH] = scaleRange(ezTune()->rate, 0, 200, 30, 90); + ((controlConfig_t*)currentControlProfile)->stabilized.rates[FD_YAW] = scaleRange(ezTune()->rate, 0, 200, 30, 90) - 10; - ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcExpo8 = scaleRange(ezTune()->rate, 0, 200, 40, 100); - ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcYawExpo8 = scaleRange(ezTune()->rate, 0, 200, 40, 100); + ((controlConfig_t*)currentControlProfile)->stabilized.rcExpo8 = scaleRange(ezTune()->rate, 0, 200, 40, 100); + ((controlConfig_t*)currentControlProfile)->stabilized.rcYawExpo8 = scaleRange(ezTune()->rate, 0, 200, 40, 100); //D-Boost snappiness pidProfileMutable()->dBoostMin = scaleRangef(ezTune()->snappiness, 0, 100, 1.0f, 0.0f); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 109882b5f66..fd55f432b52 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -39,7 +39,7 @@ #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/settings.h" #include "flight/failsafe.h" diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index d3f9d90d7db..b012c040d27 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -103,6 +103,11 @@ STATIC_FASTRAM pt1Filter_t rotRateFilterY; STATIC_FASTRAM pt1Filter_t rotRateFilterZ; FASTRAM fpVector3_t imuMeasuredRotationBFFiltered = {.v = {0.0f, 0.0f, 0.0f}}; +STATIC_FASTRAM pt1Filter_t accelFilterX; +STATIC_FASTRAM pt1Filter_t accelFilterY; +STATIC_FASTRAM pt1Filter_t accelFilterZ; +FASTRAM fpVector3_t imuMeasuredAccelBFFiltered = {.v = {0.0f, 0.0f, 0.0f}}; + STATIC_FASTRAM pt1Filter_t HeadVecEFFilterX; STATIC_FASTRAM pt1Filter_t HeadVecEFFilterY; STATIC_FASTRAM pt1Filter_t HeadVecEFFilterZ; @@ -116,6 +121,7 @@ FASTRAM bool gpsHeadingInitialized; FASTRAM bool imuUpdated = false; static float imuCalculateAccelerometerWeightNearness(fpVector3_t* accBF); +static float imuCalculateAccelerometerWeightRateIgnore(const float acc_ignore_slope_multipiler); PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2); @@ -196,6 +202,10 @@ void imuInit(void) pt1FilterReset(&rotRateFilterX, 0); pt1FilterReset(&rotRateFilterY, 0); pt1FilterReset(&rotRateFilterZ, 0); + // Initialize accel filter + pt1FilterReset(&accelFilterX, 0); + pt1FilterReset(&accelFilterY, 0); + pt1FilterReset(&accelFilterZ, 0); // Initialize Heading vector filter pt1FilterReset(&HeadVecEFFilterX, 0); pt1FilterReset(&HeadVecEFFilterY, 0); @@ -339,17 +349,25 @@ bool isGPSTrustworthy(void) static float imuCalculateMcCogWeight(void) { - float wCoG = imuCalculateAccelerometerWeightNearness(&imuMeasuredAccelBF); + //used when flying stright. 1G acceleration + float wCoG = imuCalculateAccelerometerWeightNearness(&imuMeasuredAccelBFFiltered); float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBFFiltered)); const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate)) * 4.0f; wCoG *= scaleRangef(constrainf(rotRateMagnitude, 0.0f, rateSlopeMax), 0.0f, rateSlopeMax, 1.0f, 0.0f); return wCoG; } +static float imuCalculateMcCogAccWeight(void) +{ + fpVector3_t accBFNorm; + vectorScale(&accBFNorm, &imuMeasuredAccelBFFiltered, 1.0f / GRAVITY_CMSS); + float wCoGAcc = constrainf((accBFNorm.z - 1.0f)* 2, 0.0f, 1.0f); //z direction is verified via SITL + wCoGAcc = wCoGAcc * imuCalculateAccelerometerWeightRateIgnore(4.0f); //reduce weight when fast angular rate, gps acc is considered lagging + return wCoGAcc; +} -static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler) +static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, const fpVector3_t * vCOG, const fpVector3_t * vCOGAcc, float accWScaler, float magWScaler) { STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 }; - fpQuaternion_t prevOrientation = orientation; fpVector3_t vRotation = *gyroBF; @@ -358,10 +376,10 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe /* Step 1: Yaw correction */ // Use measured magnetic field vector - if (magBF || useCOG) { + if (magBF || vCOG || vCOGAcc) { float wMag = 1.0f; float wCoG = 1.0f; - if(magBF){wCoG *= imuConfig()->gps_yaw_weight / 100.0f;} + if (magBF) { wCoG *= imuConfig()->gps_yaw_weight / 100.0f; } fpVector3_t vMagErr = { .v = { 0.0f, 0.0f, 0.0f } }; fpVector3_t vCoGErr = { .v = { 0.0f, 0.0f, 0.0f } }; @@ -399,7 +417,8 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe quaternionRotateVector(&vMagErr, &vMagErr, &orientation); } } - if (useCOG) { + if (vCOG || vCOGAcc) { + fpVector3_t vCoGlocal = { .v = { 0.0f, 0.0f, 0.0f } }; fpVector3_t vForward = { .v = { 0.0f, 0.0f, 0.0f } }; //vForward as trust vector if (STATE(MULTIROTOR) && (!isMixerTransitionMixing)){ @@ -408,49 +427,51 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe vForward.x = 1.0f; } fpVector3_t vHeadingEF; - - // Use raw heading error (from GPS or whatever else) - while (courseOverGround > M_PIf) courseOverGround -= (2.0f * M_PIf); - while (courseOverGround < -M_PIf) courseOverGround += (2.0f * M_PIf); - - // William Premerlani and Paul Bizard, Direction Cosine Matrix IMU - Eqn. 22-23 - // (Rxx; Ryx) - measured (estimated) heading vector (EF) - // (-cos(COG), sin(COG)) - reference heading vector (EF) - - float airSpeed = gpsSol.groundSpeed; - // Compute heading vector in EF from scalar CoG,x axis of accelerometer is pointing backwards. - fpVector3_t vCoG = { .v = { -cos_approx(courseOverGround), sin_approx(courseOverGround), 0.0f } }; -#if defined(USE_WIND_ESTIMATOR) - // remove wind elements in vCoG for better heading estimation - if (isEstimatedWindSpeedValid() && imuConfig()->gps_yaw_windcomp) - { - vectorScale(&vCoG, &vCoG, gpsSol.groundSpeed); - vCoG.x += getEstimatedWindSpeed(X); - vCoG.y -= getEstimatedWindSpeed(Y); - airSpeed = fast_fsqrtf(vectorNormSquared(&vCoG)); - vectorNormalize(&vCoG, &vCoG); - } -#endif - wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1000), 400, 1000, 0.0f, 1.0f); // Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation); - + if (vCOG) { + vCoGlocal = *vCOG; + float airSpeed = gpsSol.groundSpeed; + #if defined(USE_WIND_ESTIMATOR) + // remove wind elements in vCoGlocal for better heading estimation + if (isEstimatedWindSpeedValid() && imuConfig()->gps_yaw_windcomp) + { + vectorScale(&vCoGlocal, &vCoGlocal, gpsSol.groundSpeed); + vCoGlocal.x += getEstimatedWindSpeed(X); + vCoGlocal.y -= getEstimatedWindSpeed(Y); + airSpeed = fast_fsqrtf(vectorNormSquared(&vCoGlocal)); + } + #endif + wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1000), 400, 1000, 0.0f, 1.0f); + } else { //vCOG is not avaliable and vCOGAcc is avaliable, set the weight of vCOG to zero + wCoG = 0.0f; + } if (STATE(MULTIROTOR)){ //when multicopter`s orientation or speed is changing rapidly. less weight on gps heading wCoG *= imuCalculateMcCogWeight(); - //scale accroading to multirotor`s tilt angle + //handle acc based vector + if(vCOGAcc){ + float wCoGAcc = imuCalculateMcCogAccWeight();//stronger weight on acc if body frame z axis greate than 1G + if (wCoGAcc > wCoG){ + //when copter is accelerating use gps acc vector instead of gps speed vector + wCoG = wCoGAcc; + vCoGlocal = *vCOGAcc; + } + } + //scale according to multirotor`s tilt angle wCoG *= scaleRangef(constrainf(vHeadingEF.z, COS20DEG, COS10DEG), COS20DEG, COS10DEG, 1.0f, 0.0f); - //for inverted flying, wCoG is lowered by imuCalculateMcCogWeight no additional processing needed + // Inverted flight relies on the existing tilt scaling(scaleRangef); there is no extra handling here } vHeadingEF.z = 0.0f; // We zeroed out vHeadingEF.z - make sure the whole vector didn't go to zero - if (vectorNormSquared(&vHeadingEF) > 0.01f) { + if (vectorNormSquared(&vHeadingEF) > 0.01f && vectorNormSquared(&vCoGlocal) > 0.01f) { // Normalize to unit vector vectorNormalize(&vHeadingEF, &vHeadingEF); + vectorNormalize(&vCoGlocal, &vCoGlocal); // error is cross product between reference heading and estimated heading (calculated in EF) - vectorCrossProduct(&vCoGErr, &vCoG, &vHeadingEF); + vectorCrossProduct(&vCoGErr, &vCoGlocal, &vHeadingEF); // Rotate error back into body frame quaternionRotateVector(&vCoGErr, &vCoGErr, &orientation); @@ -637,6 +658,11 @@ static void imuCalculateFilters(float dT) imuMeasuredRotationBFFiltered.x = pt1FilterApply4(&rotRateFilterX, imuMeasuredRotationBF.x, IMU_ROTATION_LPF, dT); imuMeasuredRotationBFFiltered.y = pt1FilterApply4(&rotRateFilterY, imuMeasuredRotationBF.y, IMU_ROTATION_LPF, dT); imuMeasuredRotationBFFiltered.z = pt1FilterApply4(&rotRateFilterZ, imuMeasuredRotationBF.z, IMU_ROTATION_LPF, dT); + + imuMeasuredAccelBFFiltered.x = pt1FilterApply4(&accelFilterX, imuMeasuredAccelBF.x, IMU_ROTATION_LPF, dT); + imuMeasuredAccelBFFiltered.y = pt1FilterApply4(&accelFilterY, imuMeasuredAccelBF.y, IMU_ROTATION_LPF, dT); + imuMeasuredAccelBFFiltered.z = pt1FilterApply4(&accelFilterZ, imuMeasuredAccelBF.z, IMU_ROTATION_LPF, dT); + HeadVecEFFiltered.x = pt1FilterApply4(&HeadVecEFFilterX, rMat[0][0], IMU_ROTATION_LPF, dT); HeadVecEFFiltered.y = pt1FilterApply4(&HeadVecEFFilterY, rMat[1][0], IMU_ROTATION_LPF, dT); HeadVecEFFiltered.z = pt1FilterApply4(&HeadVecEFFilterZ, rMat[2][0], IMU_ROTATION_LPF, dT); @@ -646,7 +672,7 @@ static void imuCalculateFilters(float dT) GPS3DspeedFiltered = pt1FilterApply4(&GPS3DspeedFilter, GPS3Dspeed, IMU_ROTATION_LPF, dT); } -static void imuCalculateGPSacceleration(fpVector3_t *vEstcentrifugalAccelBF, float *acc_ignore_slope_multipiler) +static void imuCalculateGPSacceleration(fpVector3_t *vEstAccelEF,fpVector3_t *vEstcentrifugalAccelBF, float *acc_ignore_slope_multipiler) { static rtcTime_t lastGPSNewDataTime = 0; static bool lastGPSHeartbeat; @@ -660,17 +686,16 @@ static void imuCalculateGPSacceleration(fpVector3_t *vEstcentrifugalAccelBF, flo if (lastGPSHeartbeat != gpsSol.flags.gpsHeartbeat && time_delta_ms > 0) { // on new gps frame, update accEF and estimate centrifugal accleration - fpVector3_t vGPSacc = {.v = {0.0f, 0.0f, 0.0f}}; - vGPSacc.x = -(currentGPSvel.x - lastGPSvel.x) / (MS2S(time_delta_ms)); // the x axis of accerometer is pointing backward - vGPSacc.y = (currentGPSvel.y - lastGPSvel.y) / (MS2S(time_delta_ms)); - vGPSacc.z = (currentGPSvel.z - lastGPSvel.z) / (MS2S(time_delta_ms)); + vEstAccelEF->x = -(currentGPSvel.x - lastGPSvel.x) / (MS2S(time_delta_ms)); // the x axis of accerometer is pointing backward + vEstAccelEF->y = (currentGPSvel.y - lastGPSvel.y) / (MS2S(time_delta_ms)); + vEstAccelEF->z = (currentGPSvel.z - lastGPSvel.z) / (MS2S(time_delta_ms)); // Calculate estimated centrifugal accleration vector in body frame - quaternionRotateVector(vEstcentrifugalAccelBF, &vGPSacc, &orientation); // EF -> BF + quaternionRotateVector(vEstcentrifugalAccelBF, vEstAccelEF, &orientation); // EF -> BF lastGPSNewDataTime = currenttime; lastGPSvel = currentGPSvel; } lastGPSHeartbeat = gpsSol.flags.gpsHeartbeat; - *acc_ignore_slope_multipiler = 4; + *acc_ignore_slope_multipiler = 4.0f; } static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF, float dT, float *acc_ignore_slope_multipiler) @@ -684,14 +709,14 @@ static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF *acc_ignore_slope_multipiler = 4.0f; } #ifdef USE_PITOT - if (sensors(SENSOR_PITOT) && pitotIsHealthy() && currentspeed < 0) + else if (sensors(SENSOR_PITOT) && pitotIsHealthy()) { // second choice is pitot currentspeed = getAirspeedEstimate(); *acc_ignore_slope_multipiler = 2.0f; } #endif - if (currentspeed < 0) + else { //third choice is fixedWingReferenceAirspeed currentspeed = pidProfile()->fixedWingReferenceAirspeed; @@ -738,10 +763,11 @@ static void imuCalculateEstimatedAttitude(float dT) #else const bool canUseMAG = false; #endif - - float courseOverGround = 0; + static fpVector3_t vCOG; + static fpVector3_t vCOGAcc; bool useMag = false; bool useCOG = false; + bool useCOGAcc = false; #if defined(USE_GPS) bool canUseCOG = isGPSHeadingValid(); @@ -753,7 +779,15 @@ static void imuCalculateEstimatedAttitude(float dT) // Use GPS (if available) if (canUseCOG) { if (gpsHeadingInitialized) { - courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse); + float courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse); + while (courseOverGround > M_PIf) courseOverGround -= (2.0f * M_PIf); + while (courseOverGround < -M_PIf) courseOverGround += (2.0f * M_PIf); + // William Premerlani and Paul Bizard, Direction Cosine Matrix IMU - Eqn. 22-23 + // (Rxx; Ryx) - measured (estimated) heading vector (EF) + // (-cos(COG), sin(COG)) - reference heading vector (EF) + vCOG.x = -cos_approx(courseOverGround); // the x axis of accerometer is pointing tail + vCOG.y = sin_approx(courseOverGround); + vCOG.z = 0; useCOG = true; } else if (!canUseMAG) { @@ -775,7 +809,8 @@ static void imuCalculateEstimatedAttitude(float dT) float acc_ignore_slope_multipiler = 1.0f; // when using gps centrifugal_force_compensation, AccelerometerWeightRateIgnore slope will be multiplied by this value if (isGPSTrustworthy()) { - imuCalculateGPSacceleration(&vEstcentrifugalAccelBF_velned, &acc_ignore_slope_multipiler); + imuCalculateGPSacceleration(&vCOGAcc, &vEstcentrifugalAccelBF_velned, &acc_ignore_slope_multipiler); + useCOGAcc = true; //currently only for multicopter } if (STATE(AIRPLANE)) { @@ -824,7 +859,8 @@ static void imuCalculateEstimatedAttitude(float dT) imuMahonyAHRSupdate(dT, &imuMeasuredRotationBF, useAcc ? &compansatedGravityBF : NULL, useMag ? &measuredMagBF : NULL, - useCOG, courseOverGround, + useCOG ? &vCOG : NULL, + useCOGAcc ? &vCOGAcc : NULL, accWeight, magWeight); imuUpdateTailSitter(); diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 8afcc50e579..6d285d0f794 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -25,6 +25,7 @@ #include "config/parameter_group.h" extern fpVector3_t imuMeasuredAccelBF; // cm/s/s +extern fpVector3_t imuMeasuredAccelBFFiltered; // cm/s/s extern fpVector3_t imuMeasuredRotationBF; // rad/s extern fpVector3_t imuMeasuredRotationBFFiltered; // rad/s extern fpVector3_t compansatedGravityBF; // cm/s/s diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 716b48d7aa4..a80992b772d 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -42,7 +42,7 @@ #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/settings.h" #include "flight/failsafe.h" @@ -264,8 +264,7 @@ void mixerResetDisarmedMotors(void) motor_disarmed[i] = motorZeroCommand; } } - -#ifdef USE_DSHOT +#if !defined(SITL_BUILD) static uint16_t handleOutputScaling( int16_t input, // Input value from the mixer int16_t stopThreshold, // Threshold value to check if motor should be rotating or not @@ -277,12 +276,8 @@ static uint16_t handleOutputScaling( bool moveForward // If motor should be rotating FORWARD or BACKWARD ) { - int value; - if (moveForward && input < stopThreshold) { - //Send motor stop command - value = onStopValue; - } - else if (!moveForward && input > stopThreshold) { + int16_t value; + if ((moveForward && input < stopThreshold) || (!moveForward && input > stopThreshold)) { //Send motor stop command value = onStopValue; } @@ -291,8 +286,11 @@ static uint16_t handleOutputScaling( value = scaleRangef(input, inputScaleMin, inputScaleMax, outputScaleMin, outputScaleMax); value = constrain(value, outputScaleMin, outputScaleMax); } + return value; } +#endif +#ifdef USE_DSHOT static void applyTurtleModeToMotors(void) { if (ARMING_FLAG(ARMED)) { @@ -373,11 +371,9 @@ void FAST_CODE writeMotors(void) #if !defined(SITL_BUILD) for (int i = 0; i < motorCount; i++) { uint16_t motorValue; - #ifdef USE_DSHOT - // If we use DSHOT we need to convert motorValue to DSHOT ranges if (isMotorProtocolDigital()) { - + // If we use DSHOT we need to convert motorValue to DSHOT ranges if (feature(FEATURE_REVERSIBLE_MOTORS)) { if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) { motorValue = handleOutputScaling( @@ -416,7 +412,9 @@ void FAST_CODE writeMotors(void) ); } } - else { + else +#endif + { if (feature(FEATURE_REVERSIBLE_MOTORS)) { if (reversibleMotorsThrottleState == MOTOR_DIRECTION_FORWARD) { motorValue = handleOutputScaling( @@ -444,12 +442,7 @@ void FAST_CODE writeMotors(void) } else { motorValue = motor[i]; } - } -#else - // We don't define USE_DSHOT - motorValue = motor[i]; -#endif pwmWriteMotor(i, motorValue); } @@ -527,6 +520,11 @@ void FAST_CODE mixTable(void) input[ROLL] = axisPID[ROLL]; input[PITCH] = axisPID[PITCH]; input[YAW] = axisPID[YAW]; + if(isMixerTransitionMixing){ + input[ROLL] = input[ROLL] * (currentMixerConfig.transition_PID_mmix_multiplier_roll / 1000.0f); + input[PITCH] = input[PITCH] * (currentMixerConfig.transition_PID_mmix_multiplier_pitch / 1000.0f); + input[YAW] = input[YAW] * (currentMixerConfig.transition_PID_mmix_multiplier_yaw / 1000.0f); + } } // Initial mixer concept by bdoiron74 reused and optimized for Air Mode @@ -558,7 +556,6 @@ void FAST_CODE mixTable(void) } else #endif if (feature(FEATURE_REVERSIBLE_MOTORS)) { - if (rcCommand[THROTTLE] >= (throttleDeadbandHigh) || STATE(SET_REVERSIBLE_MOTORS_FORWARD)) { /* * Throttle is above deadband, FORWARD direction @@ -576,12 +573,11 @@ void FAST_CODE mixTable(void) throttleRangeMin = motorConfig()->mincommand; } - motorValueWhenStopped = getReversibleMotorsThrottleDeadband(); mixerThrottleCommand = constrain(rcCommand[THROTTLE], throttleRangeMin, throttleRangeMax); #ifdef USE_DSHOT - if(isMotorProtocolDigital() && feature(FEATURE_REVERSIBLE_MOTORS) && reversibleMotorsThrottleState == MOTOR_DIRECTION_BACKWARD) { + if(isMotorProtocolDigital() && reversibleMotorsThrottleState == MOTOR_DIRECTION_BACKWARD) { /* * We need to start the throttle output from stick input to start in the middle of the stick at the low and. * Without this, it's starting at the high side. @@ -636,13 +632,13 @@ void FAST_CODE mixTable(void) } else { motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax); } - + //stop motors if (currentMixer[i].throttle <= 0.0f) { motor[i] = motorZeroCommand; } //spin stopped motors only in mixer transition mode - if (isMixerTransitionMixing && currentMixer[i].throttle <= -1.05f && currentMixer[i].throttle >= -2.0f && (!feature(FEATURE_REVERSIBLE_MOTORS))) { + if (isMixerTransitionMixing && currentMixer[i].throttle <= -1.05f && currentMixer[i].throttle >= -2.0f && !feature(FEATURE_REVERSIBLE_MOTORS)) { motor[i] = -currentMixer[i].throttle * 1000; motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax); } @@ -732,7 +728,7 @@ uint16_t getMaxThrottle(void) { static uint16_t throttle = 0; - if (throttle == 0) { + if (throttle == 0) { if (STATE(ROVER) || STATE(BOAT)) { throttle = MAX_THROTTLE_ROVER; } else { diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 5769b56fbdb..a39fbfeedd9 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -50,10 +50,13 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) .hasFlaps = SETTING_HAS_FLAPS_DEFAULT, .appliedMixerPreset = SETTING_MODEL_PREVIEW_TYPE_DEFAULT, // This flag is not available in CLI and used by Configurator only .motorstopOnLow = SETTING_MOTORSTOP_ON_LOW_DEFAULT, - .PIDProfileLinking = SETTING_MIXER_PID_PROFILE_LINKING_DEFAULT, + .controlProfileLinking = SETTING_MIXER_CONTROL_PROFILE_LINKING_DEFAULT, .automated_switch = SETTING_MIXER_AUTOMATED_SWITCH_DEFAULT, .switchTransitionTimer = SETTING_MIXER_SWITCH_TRANS_TIMER_DEFAULT, .tailsitterOrientationOffset = SETTING_TAILSITTER_ORIENTATION_OFFSET_DEFAULT, + .transition_PID_mmix_multiplier_roll = SETTING_TRANSITION_PID_MMIX_MULTIPLIER_ROLL_DEFAULT, + .transition_PID_mmix_multiplier_pitch = SETTING_TRANSITION_PID_MMIX_MULTIPLIER_PITCH_DEFAULT, + .transition_PID_mmix_multiplier_yaw = SETTING_TRANSITION_PID_MMIX_MULTIPLIER_YAW_DEFAULT }); for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) { @@ -91,7 +94,7 @@ void mixerConfigInit(void) servosInit(); mixerUpdateStateFlags(); mixerInit(); - if (currentMixerConfig.PIDProfileLinking) + if (currentMixerConfig.controlProfileLinking) { // LOG_INFO(PWM, "mixer switch pidInit"); setConfigProfile(getConfigMixerProfile()); diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 947ca701553..715732d0685 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -15,10 +15,13 @@ typedef struct mixerConfig_s { bool hasFlaps; int16_t appliedMixerPreset; bool motorstopOnLow; - bool PIDProfileLinking; + bool controlProfileLinking; bool automated_switch; int16_t switchTransitionTimer; bool tailsitterOrientationOffset; + int16_t transition_PID_mmix_multiplier_roll; + int16_t transition_PID_mmix_multiplier_pitch; + int16_t transition_PID_mmix_multiplier_yaw; } mixerConfig_t; typedef struct mixerProfile_s { mixerConfig_t mixer_config; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 4ca0e3c9c0d..7a112ff8f4f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -34,7 +34,7 @@ #include "config/parameter_group_ids.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" @@ -126,7 +126,7 @@ static EXTENDED_FASTRAM pt1Filter_t headingHoldRateFilter; static EXTENDED_FASTRAM pt1Filter_t fixedWingTpaFilter; // Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied -STATIC_FASTRAM bool pidGainsUpdateRequired; +STATIC_FASTRAM bool pidGainsUpdateRequired= true; FASTRAM int16_t axisPID[FLIGHT_DYNAMICS_INDEX_COUNT]; #ifdef USE_BLACKBOX @@ -179,7 +179,7 @@ static EXTENDED_FASTRAM bool angleHoldIsLevel = false; static EXTENDED_FASTRAM float fixedWingLevelTrim; static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 10); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 11); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -308,6 +308,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT, .fwAltControlResponseFactor = SETTING_NAV_FW_ALT_CONTROL_RESPONSE_DEFAULT, + .fwAltControlUsePos = SETTING_NAV_FW_ALT_USE_POSITION_DEFAULT, #ifdef USE_SMITH_PREDICTOR .smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT, .smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT, @@ -381,8 +382,8 @@ bool pidInitFilters(void) void pidResetTPAFilter(void) { - if (usedPidControllerType == PID_TYPE_PIFF && currentControlRateProfile->throttle.fixedWingTauMs > 0) { - pt1FilterInitRC(&fixedWingTpaFilter, MS2S(currentControlRateProfile->throttle.fixedWingTauMs), US2S(TASK_PERIOD_HZ(TASK_AUX_RATE_HZ))); + if (usedPidControllerType == PID_TYPE_PIFF && currentControlProfile->throttle.fixedWingTauMs > 0) { + pt1FilterInitRC(&fixedWingTpaFilter, MS2S(currentControlProfile->throttle.fixedWingTauMs), US2S(TASK_PERIOD_HZ(TASK_AUX_RATE_HZ))); pt1FilterReset(&fixedWingTpaFilter, getThrottleIdleValue()); } } @@ -441,26 +442,33 @@ float pidRcCommandToRate(int16_t stick, uint8_t rate) return scaleRangef((float) stick, -500.0f, 500.0f, -maxRateDPS, maxRateDPS); } +static float calculateFixedWingAirspeedTPAFactor(void){ + const float airspeed = getAirspeedEstimate(); // in cm/s + const float referenceAirspeed = pidProfile()->fixedWingReferenceAirspeed; // in cm/s + float tpaFactor= powf(referenceAirspeed/(airspeed+0.01f), currentControlProfile->throttle.apa_pow/100.0f); + tpaFactor= constrainf(tpaFactor, 0.3f, 2.0f); + return tpaFactor; +} + static float calculateFixedWingTPAFactor(uint16_t throttle) { float tpaFactor; // tpa_rate is amount of curve TPA applied to PIDs // tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned) - if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > getThrottleIdleValue() && !FLIGHT_MODE(AUTO_TUNE) && ARMING_FLAG(ARMED)) { + if (currentControlProfile->throttle.dynPID != 0 && currentControlProfile->throttle.pa_breakpoint > getThrottleIdleValue() && !FLIGHT_MODE(AUTO_TUNE) && ARMING_FLAG(ARMED)) { if (throttle > getThrottleIdleValue()) { // Calculate TPA according to throttle - tpaFactor = 0.5f + ((float)(currentControlRateProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f); - - // Limit to [0.5; 2] range - tpaFactor = constrainf(tpaFactor, 0.5f, 2.0f); + tpaFactor = 0.5f + ((float)(currentControlProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f); } else { tpaFactor = 2.0f; } // Attenuate TPA curve according to configured amount - tpaFactor = 1.0f + (tpaFactor - 1.0f) * (currentControlRateProfile->throttle.dynPID / 100.0f); + tpaFactor = 1.0f + (tpaFactor - 1.0f) * (currentControlProfile->throttle.dynPID / 100.0f); + // Limit to [0.5; 2] range + tpaFactor = constrainf(tpaFactor, 0.3f, 2.0f); } else { tpaFactor = 1.0f; @@ -469,22 +477,40 @@ static float calculateFixedWingTPAFactor(uint16_t throttle) return tpaFactor; } -static float calculateMultirotorTPAFactor(void) +static float calculateMultirotorTPAFactor(uint16_t throttle) { float tpaFactor; // TPA should be updated only when TPA is actually set - if (currentControlRateProfile->throttle.dynPID == 0 || rcCommand[THROTTLE] < currentControlRateProfile->throttle.pa_breakpoint) { + if (currentControlProfile->throttle.dynPID == 0 || throttle < currentControlProfile->throttle.pa_breakpoint) { tpaFactor = 1.0f; - } else if (rcCommand[THROTTLE] < getMaxThrottle()) { - tpaFactor = (100 - (uint16_t)currentControlRateProfile->throttle.dynPID * (rcCommand[THROTTLE] - currentControlRateProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - currentControlRateProfile->throttle.pa_breakpoint)) / 100.0f; + } else if (throttle < getMaxThrottle()) { + tpaFactor = (100 - (uint16_t)currentControlProfile->throttle.dynPID * (throttle - currentControlProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - currentControlProfile->throttle.pa_breakpoint)) / 100.0f; } else { - tpaFactor = (100 - currentControlRateProfile->throttle.dynPID) / 100.0f; + tpaFactor = (100 - constrain(currentControlProfile->throttle.dynPID, 0, 100)) / 100.0f; } return tpaFactor; } +static float calculateTPAThtrottle(void) +{ + uint16_t tpaThrottle = 0; + static const fpVector3_t vDown = { .v = { 0.0f, 0.0f, 1.0f } }; + + if (usedPidControllerType == PID_TYPE_PIFF && (currentControlProfile->throttle.fixedWingTauMs > 0)) { //fixed wing TPA with filtering + fpVector3_t vForward = { .v = { HeadVecEFFiltered.x, -HeadVecEFFiltered.y, -HeadVecEFFiltered.z } }; + float groundCos = vectorDotProduct(&vForward, &vDown); + int16_t throttleAdjustment = currentControlProfile->throttle.tpa_pitch_compensation * groundCos * 90.0f / 1.57079632679f; //when 1deg pitch up, increase throttle by pitch(deg)_to_throttle. cos(89 deg)*90/(pi/2)=0.99995,cos(80 deg)*90/(pi/2)=9.9493, + uint16_t throttleAdjusted = rcCommand[THROTTLE] + constrain(throttleAdjustment, -1000, 1000); + tpaThrottle = pt1FilterApply(&fixedWingTpaFilter, constrain(throttleAdjusted, 1000, 2000)); + } + else { + tpaThrottle = rcCommand[THROTTLE]; //multirotor TPA without filtering + } + return tpaThrottle; +} + void schedulePidGainsUpdate(void) { pidGainsUpdateRequired = true; @@ -492,22 +518,7 @@ void schedulePidGainsUpdate(void) void updatePIDCoefficients(void) { - STATIC_FASTRAM uint16_t prevThrottle = 0; - - // Check if throttle changed. Different logic for fixed wing vs multirotor - if (usedPidControllerType == PID_TYPE_PIFF && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) { - uint16_t filteredThrottle = pt1FilterApply(&fixedWingTpaFilter, rcCommand[THROTTLE]); - if (filteredThrottle != prevThrottle) { - prevThrottle = filteredThrottle; - pidGainsUpdateRequired = true; - } - } - else { - if (rcCommand[THROTTLE] != prevThrottle) { - prevThrottle = rcCommand[THROTTLE]; - pidGainsUpdateRequired = true; - } - } + STATIC_FASTRAM float tpaFactorprev=-1.0f; #ifdef USE_ANTIGRAVITY if (usedPidControllerType == PID_TYPE_PID) { @@ -522,14 +533,29 @@ void updatePIDCoefficients(void) for (int axis = 0; axis < 3; axis++) { pidState[axis].stickPosition = constrain(rxGetChannelValue(axis) - PWM_RANGE_MIDDLE, -500, 500) / 500.0f; } + + float tpaFactor=1.0f; + if(usedPidControllerType == PID_TYPE_PIFF){ // Fixed wing TPA calculation + if(currentControlProfile->throttle.apa_pow>0 && pitotValidForAirspeed()){ + tpaFactor = calculateFixedWingAirspeedTPAFactor(); + }else{ + tpaFactor = calculateFixedWingTPAFactor(calculateTPAThtrottle()); + } + } else { + tpaFactor = calculateMultirotorTPAFactor(calculateTPAThtrottle()); + } + if (tpaFactor != tpaFactorprev) { + pidGainsUpdateRequired = true; + } + tpaFactorprev = tpaFactor; + // If nothing changed - don't waste time recalculating coefficients if (!pidGainsUpdateRequired) { return; } - const float tpaFactor = usedPidControllerType == PID_TYPE_PIFF ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor(); - + // PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments //TODO: Next step would be to update those only at THROTTLE or inflight adjustments change for (int axis = 0; axis < 3; axis++) { @@ -543,7 +569,7 @@ void updatePIDCoefficients(void) pidState[axis].kT = 0.0f; } else { - const float axisTPA = (axis == FD_YAW && (!currentControlRateProfile->throttle.dynPID_on_YAW)) ? 1.0f : tpaFactor; + const float axisTPA = (axis == FD_YAW && (!currentControlProfile->throttle.dynPID_on_YAW)) ? 1.0f : tpaFactor; pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * axisTPA; pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER; pidState[axis].kD = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * axisTPA; @@ -653,7 +679,7 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam } } - float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f); + float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER), -currentControlProfile->stabilized.rates[axis] * 10.0f, currentControlProfile->stabilized.rates[axis] * 10.0f); // Apply simple LPF to angleRateTarget to make response less jerky // Ideas behind this: @@ -765,7 +791,7 @@ static void nullRateController(pidState_t *pidState, float dT, float dT_inv) { * On top of that, after stick is relased, it also locks Iterm to prevent it from accumulating error and unloading it after */ static void iTermLockApply(pidState_t *pidState, const float rateTarget, const float rateError) { - const float maxRate = currentControlRateProfile->stabilized.rates[pidState->axis] * 10.0f; + const float maxRate = currentControlProfile->stabilized.rates[pidState->axis] * 10.0f; //Compute damping factor based on rate target and max rate scaled by fw_iterm_lock_rate_threshold const float dampingFactor = attenuation(rateTarget, maxRate * pidProfile()->fwItermLockRateLimit / 100.0f); @@ -1060,15 +1086,15 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarge imuTransformVectorEarthToBody(&targetRates); // Add in roll and pitch - pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.x, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f); - pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y * pidProfile()->fixedWingCoordinatedPitchGain, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f); + pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.x, -currentControlProfile->stabilized.rates[ROLL] * 10.0f, currentControlProfile->stabilized.rates[ROLL] * 10.0f); + pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y * pidProfile()->fixedWingCoordinatedPitchGain, -currentControlProfile->stabilized.rates[PITCH] * 10.0f, currentControlProfile->stabilized.rates[PITCH] * 10.0f); // Replace YAW on quads - add it in on airplanes if (STATE(AIRPLANE)) { - pidState[YAW].rateTarget = constrainf(pidState[YAW].rateTarget + targetRates.z * pidProfile()->fixedWingCoordinatedYawGain, -currentControlRateProfile->stabilized.rates[YAW] * 10.0f, currentControlRateProfile->stabilized.rates[YAW] * 10.0f); + pidState[YAW].rateTarget = constrainf(pidState[YAW].rateTarget + targetRates.z * pidProfile()->fixedWingCoordinatedYawGain, -currentControlProfile->stabilized.rates[YAW] * 10.0f, currentControlProfile->stabilized.rates[YAW] * 10.0f); } else { - pidState[YAW].rateTarget = constrainf(targetRates.z, -currentControlRateProfile->stabilized.rates[YAW] * 10.0f, currentControlRateProfile->stabilized.rates[YAW] * 10.0f); + pidState[YAW].rateTarget = constrainf(targetRates.z, -currentControlProfile->stabilized.rates[YAW] * 10.0f, currentControlProfile->stabilized.rates[YAW] * 10.0f); } } @@ -1205,9 +1231,9 @@ void FAST_CODE pidController(float dT) rateTarget = pidHeadingHold(dT); } else { #ifdef USE_PROGRAMMING_FRAMEWORK - rateTarget = pidRcCommandToRate(getRcCommandOverride(rcCommand, axis), currentControlRateProfile->stabilized.rates[axis]); + rateTarget = pidRcCommandToRate(getRcCommandOverride(rcCommand, axis), currentControlProfile->stabilized.rates[axis]); #else - rateTarget = pidRcCommandToRate(rcCommand[axis], currentControlRateProfile->stabilized.rates[axis]); + rateTarget = pidRcCommandToRate(rcCommand[axis], currentControlProfile->stabilized.rates[axis]); #endif } @@ -1215,7 +1241,7 @@ void FAST_CODE pidController(float dT) pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); #ifdef USE_ADAPTIVE_FILTER - adaptiveFilterPushRate(axis, pidState[axis].rateTarget, currentControlRateProfile->stabilized.rates[axis]); + adaptiveFilterPushRate(axis, pidState[axis].rateTarget, currentControlProfile->stabilized.rates[axis]); #endif #ifdef USE_GYRO_KALMAN @@ -1262,8 +1288,8 @@ void FAST_CODE pidController(float dT) } // Apply FPV camera mix - if (canUseFpvCameraMix && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && currentControlRateProfile->misc.fpvCamAngleDegrees && STATE(MULTIROTOR)) { - pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees); + if (canUseFpvCameraMix && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && currentControlProfile->misc.fpvCamAngleDegrees && STATE(MULTIROTOR)) { + pidApplyFpvCameraAngleMix(pidState, currentControlProfile->misc.fpvCamAngleDegrees); } // Prevent strong Iterm accumulation during stick inputs @@ -1327,7 +1353,7 @@ void pidInit(void) #ifdef USE_D_BOOST // Rate * 10 * 10. First 10 is to convert stick to DPS. Second 10 is to convert target to acceleration. // We assume, max acceleration is when pilot deflects the stick fully in 100ms - pidState[axis].dBoostTargetAcceleration = currentControlRateProfile->stabilized.rates[axis] * 10 * 10; + pidState[axis].dBoostTargetAcceleration = currentControlProfile->stabilized.rates[axis] * 10 * 10; #endif pidState[axis].axis = axis; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 26aeb86990d..ff2e85031b7 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -150,6 +150,7 @@ typedef struct pidProfile_s { float fixedWingLevelTrimGain; uint8_t fwAltControlResponseFactor; + bool fwAltControlUsePos; #ifdef USE_SMITH_PREDICTOR float smithPredictorStrength; float smithPredictorDelay; @@ -196,7 +197,7 @@ float getAxisIterm(uint8_t axis); float getTotalRateTarget(void); void pidResetTPAFilter(void); -struct controlRateConfig_s; +struct controlConfig_s; struct motorConfig_s; struct rxConfig_s; diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index c4be8101bc4..1c488292bd9 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -38,7 +38,7 @@ #include "config/parameter_group_ids.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/rc_adjustments.h" @@ -94,7 +94,7 @@ void autotuneUpdateGains(pidAutotuneData_t * data) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { pidBankMutable()->pid[axis].FF = lrintf(data[axis].gainFF); - ((controlRateConfig_t *)currentControlRateProfile)->stabilized.rates[axis] = lrintf(data[axis].rate/10.0f); + ((controlConfig_t *)currentControlProfile)->stabilized.rates[axis] = lrintf(data[axis].rate/10.0f); } schedulePidGainsUpdate(); } @@ -117,8 +117,8 @@ void autotuneStart(void) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { tuneCurrent[axis].gainFF = pidBank()->pid[axis].FF; - tuneCurrent[axis].rate = currentControlRateProfile->stabilized.rates[axis] * 10.0f; - tuneCurrent[axis].initialRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f; + tuneCurrent[axis].rate = currentControlProfile->stabilized.rates[axis] * 10.0f; + tuneCurrent[axis].initialRate = currentControlProfile->stabilized.rates[axis] * 10.0f; tuneCurrent[axis].absDesiredRateAccum = 0; tuneCurrent[axis].absReachedRateAccum = 0; tuneCurrent[axis].absPidOutputAccum = 0; diff --git a/src/main/flight/power_limits.c b/src/main/flight/power_limits.c index 1fe13240fe7..1770fd71a7f 100644 --- a/src/main/flight/power_limits.c +++ b/src/main/flight/power_limits.c @@ -73,7 +73,10 @@ static bool wasLimitingPower = false; #endif void powerLimiterInit(void) { - if (currentBatteryProfile->powerLimits.burstCurrent < currentBatteryProfile->powerLimits.continuousCurrent) { + // Only enforce burst >= continuous if burst is enabled (non-zero) + // A value of 0 means "disabled/unlimited", not "zero amps allowed" + if (currentBatteryProfile->powerLimits.burstCurrent > 0 && + currentBatteryProfile->powerLimits.burstCurrent < currentBatteryProfile->powerLimits.continuousCurrent) { currentBatteryProfileMutable->powerLimits.burstCurrent = currentBatteryProfile->powerLimits.continuousCurrent; } @@ -87,7 +90,9 @@ void powerLimiterInit(void) { pt1FilterInitRC(¤tThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0); #ifdef USE_ADC - if (currentBatteryProfile->powerLimits.burstPower < currentBatteryProfile->powerLimits.continuousPower) { + // Only enforce burst >= continuous if burst is enabled (non-zero) + if (currentBatteryProfile->powerLimits.burstPower > 0 && + currentBatteryProfile->powerLimits.burstPower < currentBatteryProfile->powerLimits.continuousPower) { currentBatteryProfileMutable->powerLimits.burstPower = currentBatteryProfile->powerLimits.continuousPower; } diff --git a/src/main/flight/rate_dynamics.c b/src/main/flight/rate_dynamics.c index 8f5c7a62346..79a80466abb 100644 --- a/src/main/flight/rate_dynamics.c +++ b/src/main/flight/rate_dynamics.c @@ -22,7 +22,7 @@ #include #include "rate_dynamics.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include static FASTRAM float lastRcCommandData[3]; @@ -48,10 +48,10 @@ FAST_CODE static float calculateK(const float k, const float dT) { FAST_CODE int applyRateDynamics(int rcCommand, const int axis, const float dT) { if ( - currentControlRateProfile->rateDynamics.sensitivityCenter != 100 || - currentControlRateProfile->rateDynamics.sensitivityEnd != 100 || - currentControlRateProfile->rateDynamics.weightCenter > 0 || - currentControlRateProfile->rateDynamics.weightEnd > 0 + currentControlProfile->rateDynamics.sensitivityCenter != 100 || + currentControlProfile->rateDynamics.sensitivityEnd != 100 || + currentControlProfile->rateDynamics.weightCenter > 0 || + currentControlProfile->rateDynamics.weightEnd > 0 ) { float pterm_centerStick, pterm_endStick, pterm, iterm_centerStick, iterm_endStick, dterm_centerStick, dterm_endStick, dterm; @@ -62,19 +62,19 @@ FAST_CODE int applyRateDynamics(int rcCommand, const int axis, const float dT) { rcCommandPercent = abs(rcCommand) / 500.0f; // make rcCommandPercent go from 0 to 1 inverseRcCommandPercent = 1.0f - rcCommandPercent; - pterm_centerStick = inverseRcCommandPercent * rcCommand * (currentControlRateProfile->rateDynamics.sensitivityCenter / 100.0f); // valid pterm values are between 50-150 - pterm_endStick = rcCommandPercent * rcCommand * (currentControlRateProfile->rateDynamics.sensitivityEnd / 100.0f); + pterm_centerStick = inverseRcCommandPercent * rcCommand * (currentControlProfile->rateDynamics.sensitivityCenter / 100.0f); // valid pterm values are between 50-150 + pterm_endStick = rcCommandPercent * rcCommand * (currentControlProfile->rateDynamics.sensitivityEnd / 100.0f); pterm = pterm_centerStick + pterm_endStick; rcCommandError = rcCommand - (pterm + iterm[axis]); rcCommand = pterm; // add this fake pterm to the rcCommand - iterm_centerStick = inverseRcCommandPercent * rcCommandError * calculateK(currentControlRateProfile->rateDynamics.correctionCenter / 100.0f, dT); // valid iterm values are between 0-95 - iterm_endStick = rcCommandPercent * rcCommandError * calculateK(currentControlRateProfile->rateDynamics.correctionEnd / 100.0f, dT); + iterm_centerStick = inverseRcCommandPercent * rcCommandError * calculateK(currentControlProfile->rateDynamics.correctionCenter / 100.0f, dT); // valid iterm values are between 0-95 + iterm_endStick = rcCommandPercent * rcCommandError * calculateK(currentControlProfile->rateDynamics.correctionEnd / 100.0f, dT); iterm[axis] += iterm_centerStick + iterm_endStick; rcCommand = rcCommand + iterm[axis]; // add the iterm to the rcCommand - dterm_centerStick = inverseRcCommandPercent * (lastRcCommandData[axis] - rcCommand) * calculateK(currentControlRateProfile->rateDynamics.weightCenter / 100.0f, dT); // valid dterm values are between 0-95 - dterm_endStick = rcCommandPercent * (lastRcCommandData[axis] - rcCommand) * calculateK(currentControlRateProfile->rateDynamics.weightEnd / 100.0f, dT); + dterm_centerStick = inverseRcCommandPercent * (lastRcCommandData[axis] - rcCommand) * calculateK(currentControlProfile->rateDynamics.weightCenter / 100.0f, dT); // valid dterm values are between 0-95 + dterm_endStick = rcCommandPercent * (lastRcCommandData[axis] - rcCommand) * calculateK(currentControlProfile->rateDynamics.weightEnd / 100.0f, dT); dterm = dterm_centerStick + dterm_endStick; rcCommand = rcCommand + dterm; // add dterm to the rcCommand (this is real dterm) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 51f362e44ca..9d2720bec09 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -46,7 +46,7 @@ #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/settings.h" #include "flight/imu.h" @@ -715,3 +715,8 @@ bool isMixerUsingServos(void) { return mixerUsesServos; } + +uint8_t getMinServoIndex(void) +{ + return minServoIndex; +} diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 3e3b087c3db..bc001455a91 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -192,3 +192,4 @@ void servoMixer(float dT); void servoComputeScalingFactors(uint8_t servoIndex); void servosInit(void); int getServoCount(void); +uint8_t getMinServoIndex(void); diff --git a/src/main/io/adsb.c b/src/main/io/adsb.c index 260f89fa6af..34ed1183e00 100644 --- a/src/main/io/adsb.c +++ b/src/main/io/adsb.c @@ -1,18 +1,25 @@ /* - * This file is part of Cleanflight. + * This file is part of INAV Project. * - * Cleanflight 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 Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. * - * Cleanflight 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. + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 Cleanflight. If not, see . + * along with this program. If not, see http://www.gnu.org/licenses/. */ @@ -86,6 +93,46 @@ adsbVehicle_t *findVehicleClosest(void) { return adsbLocal; } +/** + * find the closest vehicle, apply filter max verticalDistance + * @return + */ +adsbVehicle_t *findVehicleClosestLimit(int32_t maxVerticalDistance) { + + //////////////////////////////////////////////////////////// + //debug vehicle + /*adsbVehicleValues_t* vehicle = getVehicleForFill(); + if(vehicle != NULL){ + char name[9] = "DUMMY "; + vehicle->icao = 666; + vehicle->gps.lat = 492383514; + vehicle->gps.lon = 165148681; + vehicle->alt = 100000; + vehicle->heading = 66; + vehicle->flags = ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS; + vehicle->altitudeType = 0; + memcpy(&(vehicle->callsign), name, sizeof(vehicle->callsign)); + vehicle->emitterType = 6; + vehicle->tslc = 0; + adsbNewVehicle(vehicle); + }*/ + //////////////////////////////////////////////////////////// + + adsbVehicle_t *adsbLocal = NULL; + for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { + if(adsbVehiclesList[i].ttl > 0 && adsbVehiclesList[i].calculatedVehicleValues.valid){ + if(adsbVehiclesList[i].calculatedVehicleValues.verticalDistance > 0 && maxVerticalDistance > 0 && adsbVehiclesList[i].calculatedVehicleValues.verticalDistance > maxVerticalDistance){ + continue; + } + + if (adsbLocal == NULL || adsbLocal->calculatedVehicleValues.dist > adsbVehiclesList[i].calculatedVehicleValues.dist) { + adsbLocal = &adsbVehiclesList[i]; + } + } + } + return adsbLocal; +} + adsbVehicle_t *findFreeSpaceInList(void) { //find expired first for (uint8_t i = 0; i < MAX_ADSB_VEHICLES; i++) { @@ -121,16 +168,6 @@ adsbVehicleStatus_t* getAdsbStatus(void){ return &adsbVehiclesStatus; } -void gpsDistanceCmBearing(int32_t currentLat1, int32_t currentLon1, int32_t destinationLat2, int32_t destinationLon2, uint32_t *dist, int32_t *bearing) { - float GPS_scaleLonDown = cos_approx((fabsf((float) gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f); - const float dLat = destinationLat2 - currentLat1; // difference of latitude in 1/10 000 000 degrees - const float dLon = (float) (destinationLon2 - currentLon1) * GPS_scaleLonDown; - - *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR; - *bearing = 9000.0f + RADIANS_TO_CENTIDEGREES(atan2_approx(-dLat, dLon)); // Convert the output radians to 100xdeg - *bearing = wrap_36000(*bearing); -}; - bool adsbHeartbeat(void){ adsbVehiclesStatus.heartbeatMessagesTotal++; return true; @@ -138,6 +175,11 @@ bool adsbHeartbeat(void){ void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal) { + if(vehicleValuesLocal->icao == 0) + { + return; + } + // no valid lat lon or altitude if((vehicleValuesLocal->flags & (ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS)) != (ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS)){ return; @@ -148,28 +190,27 @@ void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal) { adsbVehicle_t *vehicle = NULL; vehicle = findVehicleByIcao(vehicleValuesLocal->icao); - if(vehicle != NULL && vehicleValuesLocal->tslc > ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST){ - vehicle->ttl = 0; + if(vehicleValuesLocal->tslc > ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST){ + if(vehicle != NULL){ + vehicle->ttl = 0; + } return; } // non GPS mode, GPS is not fix, just find free space in list or by icao and save vehicle without calculated values - if (!enviromentOkForCalculatingDistaceBearing()) { - + if (!isEnvironmentOkForCalculatingADSBDistanceBearing()) { if(vehicle == NULL){ vehicle = findFreeSpaceInList(); } if (vehicle != NULL) { memcpy(&(vehicle->vehicleValues), vehicleValuesLocal, sizeof(vehicle->vehicleValues)); - vehicle->ttl = ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST; + vehicle->ttl = MAX(0, ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST - vehicleValuesLocal->tslc); vehicle->calculatedVehicleValues.valid = false; return; } } else { // GPS mode, GPS is fixed and has enough sats - - if(vehicle == NULL){ vehicle = findFreeSpaceInList(); } @@ -180,21 +221,39 @@ void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal) { if(vehicle == NULL){ vehicle = findVehicleFarthest(); + if(vehicle != NULL) + { + //calculate distance to new vehicle, we need to compare if new vehicle is closer than the farthest plane + fpVector3_t vehicleVector; + geoConvertGeodeticToLocal(&vehicleVector, &posControl.gpsOrigin, &vehicleValuesLocal->gps, GEO_ALT_RELATIVE); + if(calculateDistanceToDestination(&vehicleVector) > vehicle->calculatedVehicleValues.dist){ + //saved vehicle in list is closer, no need to update vehicle in list + vehicle = NULL; + } + } } if (vehicle != NULL) { memcpy(&(vehicle->vehicleValues), vehicleValuesLocal, sizeof(vehicle->vehicleValues)); recalculateVehicle(vehicle); - vehicle->ttl = ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST; + vehicle->ttl = MAX(0, ADSB_MAX_SECONDS_KEEP_INACTIVE_PLANE_IN_LIST - vehicleValuesLocal->tslc); return; } } }; void recalculateVehicle(adsbVehicle_t* vehicle){ - gpsDistanceCmBearing(gpsSol.llh.lat, gpsSol.llh.lon, vehicle->vehicleValues.lat, vehicle->vehicleValues.lon, &(vehicle->calculatedVehicleValues.dist), &(vehicle->calculatedVehicleValues.dir)); + if(vehicle->ttl == 0){ + return; + } + + fpVector3_t vehicleVector; + geoConvertGeodeticToLocal(&vehicleVector, &posControl.gpsOrigin, &vehicle->vehicleValues.gps, GEO_ALT_RELATIVE); - if (vehicle != NULL && vehicle->calculatedVehicleValues.dist > ADSB_LIMIT_CM) { + vehicle->calculatedVehicleValues.dist = calculateDistanceToDestination(&vehicleVector); + vehicle->calculatedVehicleValues.dir = calculateBearingToDestination(&vehicleVector); + + if (vehicle->calculatedVehicleValues.dist > ADSB_LIMIT_CM) { vehicle->ttl = 0; return; } @@ -215,14 +274,26 @@ void adsbTtlClean(timeUs_t currentTimeUs) { if (adsbVehiclesList[i].ttl > 0) { adsbVehiclesList[i].ttl--; } + + if (adsbVehiclesList[i].ttl > 0) { + recalculateVehicle(&adsbVehiclesList[i]); + } } adsbTtlLastCleanServiced = currentTimeUs; } }; -bool enviromentOkForCalculatingDistaceBearing(void){ - return (STATE(GPS_FIX) && gpsSol.numSat > 4); +bool isEnvironmentOkForCalculatingADSBDistanceBearing(void){ + return + (gpsSol.numSat > 4 && + ( + STATE(GPS_FIX) + #ifdef USE_GPS_FIX_ESTIMATION + || STATE(GPS_ESTIMATED_FIX) + #endif + ) + ); } #endif diff --git a/src/main/io/adsb.h b/src/main/io/adsb.h index 77f2e31fa47..bf530e14a5e 100644 --- a/src/main/io/adsb.h +++ b/src/main/io/adsb.h @@ -19,6 +19,7 @@ #include #include "common/time.h" +#include "io/gps.h" #include "fc/runtime_config.h" #define ADSB_CALL_SIGN_MAX_LENGTH 9 @@ -27,16 +28,16 @@ typedef struct { bool valid; int32_t dir; // centidegrees direction to plane, pivot is inav FC - uint32_t dist; // CM distance to plane, pivot is inav FC - int32_t verticalDistance; // CM, vertical distance to plane, pivot is inav FC + uint32_t dist; // horisontal distance to plane, cm, pivot is inav FC + int32_t verticalDistance; // vertical distance to plane, cm, pivot is inav FC } adsbVehicleCalculatedValues_t; typedef struct { uint32_t icao; // ICAO address - int32_t lat; // Latitude, expressed as degrees * 1E7 - int32_t lon; // Longitude, expressed as degrees * 1E7 - int32_t alt; // Barometric/Geometric Altitude (ASL), in cm - uint16_t heading; // Course over ground in centidegrees + uint16_t horVelocity; // [cm/s] + gpsLocation_t gps; + int32_t alt; // [cm] Barometric/Geometric Altitude (MSL) + uint16_t heading; // [centidegrees] Course over ground uint16_t flags; // Flags to indicate various statuses including valid data fields uint8_t altitudeType; // Type from ADSB_ALTITUDE_TYPE enum char callsign[ADSB_CALL_SIGN_MAX_LENGTH]; // The callsign, 8 chars + NULL @@ -50,8 +51,6 @@ typedef struct { uint8_t ttl; } adsbVehicle_t; - - typedef struct { uint32_t vehiclesMessagesTotal; uint32_t heartbeatMessagesTotal; @@ -59,11 +58,11 @@ typedef struct { void adsbNewVehicle(adsbVehicleValues_t* vehicleValuesLocal); bool adsbHeartbeat(void); -adsbVehicle_t * findVehicleClosest(void); +adsbVehicle_t *findVehicleClosestLimit(int32_t maxVerticalDistance); adsbVehicle_t * findVehicle(uint8_t index); uint8_t getActiveVehiclesCount(void); void adsbTtlClean(timeUs_t currentTimeUs); adsbVehicleStatus_t* getAdsbStatus(void); adsbVehicleValues_t* getVehicleForFill(void); -bool enviromentOkForCalculatingDistaceBearing(void); +bool isEnvironmentOkForCalculatingADSBDistanceBearing(void); void recalculateVehicle(adsbVehicle_t* vehicle); \ No newline at end of file diff --git a/src/main/io/asyncfatfs/asyncfatfs.c b/src/main/io/asyncfatfs/asyncfatfs.c index 82ed7d65830..d06fc2b8a76 100644 --- a/src/main/io/asyncfatfs/asyncfatfs.c +++ b/src/main/io/asyncfatfs/asyncfatfs.c @@ -3382,7 +3382,7 @@ static void afatfs_findLargestContiguousFreeBlockBegin(void) * Returns: * AFATFS_OPERATION_IN_PROGRESS - SD card is busy, call again later to resume * AFATFS_OPERATION_SUCCESS - When the search has finished and afatfs.initState.freeSpaceSearch has been updated with the details of the best gap. - * AFATFS_OPERATION_FAILURE - When a read error occured + * AFATFS_OPERATION_FAILURE - When a read error occurred */ static afatfsOperationStatus_e afatfs_findLargestContiguousFreeBlockContinue(void) { @@ -3402,7 +3402,7 @@ static afatfsOperationStatus_e afatfs_findLargestContiguousFreeBlockContinue(voi break; case AFATFS_FIND_CLUSTER_FATAL: - // Some sort of read error occured + // Some sort of read error occurred return AFATFS_OPERATION_FAILURE; case AFATFS_FIND_CLUSTER_NOT_FOUND: @@ -3443,7 +3443,7 @@ static afatfsOperationStatus_e afatfs_findLargestContiguousFreeBlockContinue(voi break; case AFATFS_FIND_CLUSTER_FATAL: - // Some sort of read error occured + // Some sort of read error occurred return AFATFS_OPERATION_FAILURE; case AFATFS_FIND_CLUSTER_IN_PROGRESS: diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 444d91378ab..18be89d85d1 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -38,7 +38,7 @@ #include "common/typeconversion.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/runtime_config.h" #include "fc/rc_controls.h" @@ -68,7 +68,7 @@ #include "config/feature.h" -controlRateConfig_t *getControlRateConfig(uint8_t profileIndex); +controlConfig_t *getControlConfig(uint8_t profileIndex); #define MICROSECONDS_IN_A_SECOND (1000 * 1000) @@ -172,8 +172,8 @@ static const char* const gpsFixTypeText[] = { "3D" }; -static const char* tickerCharacters = "|/-\\"; // use 2/4/8 characters so that the divide is optimal. -#define TICKER_CHARACTER_COUNT (sizeof(tickerCharacters) / sizeof(char)) +static const char tickerCharacters[] = "|/-\\"; // use 2/4/8 characters so that the divide is optimal. +#define TICKER_CHARACTER_COUNT (sizeof(tickerCharacters) - 1) static timeUs_t nextPageAt; static bool forcePageChange; diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c index cb7a3a49762..9dc99889bc1 100644 --- a/src/main/io/displayport_msp_osd.c +++ b/src/main/io/displayport_msp_osd.c @@ -568,7 +568,7 @@ static mspResult_e fixDjiBrokenO4ProcessMspCommand(mspPacket_t *cmd, mspPacket_t sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); if (cmd->cmd == MSP_STATUS_EX) { sbufWriteU8(dst, 3); // PID_PROFILE_COUNT - sbufWriteU8(dst, 1); // getCurrentControlRateProfileIndex() + sbufWriteU8(dst, 1); // getCurrentControlProfileIndex() } else { sbufWriteU16(dst, cycleTime); // gyro cycle time } diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 89549768edb..f0899aa69d5 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -746,8 +746,8 @@ static bool gpsParseFrameUBLOX(void) ubloxNavSat2NavSig(&_buffer.svinfo.channel[i], &satelites[i]); } for(int i =_buffer.svinfo.numSvs; i < UBLOX_MAX_SIGNALS; ++i) { - satelites->gnssId = 0xFF; - satelites->svId = 0xFF; + satelites[i].gnssId = 0xFF; + satelites[i].svId = 0xFF; } } break; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index d44ea0a8988..536da49b3f2 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -78,7 +78,7 @@ #include "io/osd/custom_elements.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/fc_core.h" #include "fc/fc_tasks.h" #include "fc/multifunction.h" @@ -316,6 +316,14 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals, } } +/** + * return flight direction on degrees + */ +static int16_t osdGetFlightDirection(void) +{ + return STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading()); +} + /** * Converts distance into a string based on the current unit system. * @param dist Distance in centimeters @@ -390,36 +398,116 @@ static int32_t osdConvertVelocityToUnit(int32_t vel) * @param vel Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds) * @param _3D is a 3D velocity * @param _max is a maximum velocity + * @return length of the formatted string */ -void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D, bool _max) +int osdFormatVelocityStr(char* buff, int32_t vel, osd_SpeedTypes_e speedType, bool _max) { + int strLen = 0; + switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - if (_max) { - tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); - } else { - tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_MPH : SYM_MPH)); - } - break; - case OSD_UNIT_METRIC: - if (_max) { - tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH)); - } else { - tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KMH : SYM_KMH)); - } - break; - case OSD_UNIT_GA: - if (_max) { - tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KT : SYM_KT)); - } else { - tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), (_3D ? SYM_3D_KT : SYM_KT)); - } - break; + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + if (_max) { + switch (speedType) { + case OSD_SPEED_TYPE_GROUND: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), SYM_MPH); + break; + case OSD_SPEED_TYPE_AIR: + strLen = tfp_sprintf(buff, "%c%c%3d%c", SYM_MAX, SYM_AIR, (int)osdConvertVelocityToUnit(vel), SYM_MPH); + break; + case OSD_SPEED_TYPE_3D: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), SYM_3D_MPH); + break; + case OSD_SPEED_TYPE_MIN_GROUND: + break; + } + } else { + switch (speedType){ + case OSD_SPEED_TYPE_GROUND: + strLen = tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), SYM_MPH); + break; + case OSD_SPEED_TYPE_AIR: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_AIR, (int)osdConvertVelocityToUnit(vel), SYM_MPH); + break; + case OSD_SPEED_TYPE_3D: + strLen = tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), SYM_3D_MPH); + break; + case OSD_SPEED_TYPE_MIN_GROUND: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MIN_GROUND_SPEED, (int)osdConvertVelocityToUnit(vel), SYM_MPH); + break; + } + } + break; + case OSD_UNIT_METRIC: + if (_max) { + switch (speedType) { + case OSD_SPEED_TYPE_GROUND: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), SYM_KMH); + break; + case OSD_SPEED_TYPE_AIR: + strLen = tfp_sprintf(buff, "%c%c%3d%c", SYM_MAX, SYM_AIR, (int)osdConvertVelocityToUnit(vel), SYM_KMH); + break; + case OSD_SPEED_TYPE_3D: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), SYM_3D_KMH); + break; + case OSD_SPEED_TYPE_MIN_GROUND: + break; + } + } else { + switch (speedType) { + case OSD_SPEED_TYPE_GROUND: + strLen = tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), SYM_KMH); + break; + case OSD_SPEED_TYPE_AIR: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_AIR, (int)osdConvertVelocityToUnit(vel), SYM_KMH); + break; + case OSD_SPEED_TYPE_3D: + strLen = tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), SYM_3D_KMH); + break; + case OSD_SPEED_TYPE_MIN_GROUND: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MIN_GROUND_SPEED, (int)osdConvertVelocityToUnit(vel), SYM_KMH); + break; + } + } + break; + case OSD_UNIT_GA: + if (_max) { + switch (speedType) { + case OSD_SPEED_TYPE_GROUND: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), SYM_KT); + break; + case OSD_SPEED_TYPE_AIR: + strLen = tfp_sprintf(buff, "%c%c%3d%c", SYM_MAX, SYM_AIR, (int)osdConvertVelocityToUnit(vel), SYM_KT); + break; + case OSD_SPEED_TYPE_3D: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MAX, (int)osdConvertVelocityToUnit(vel), SYM_3D_KT); + break; + case OSD_SPEED_TYPE_MIN_GROUND: + break; + } + } else { + switch (speedType) { + case OSD_SPEED_TYPE_GROUND: + strLen = tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), SYM_KT); + break; + case OSD_SPEED_TYPE_AIR: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_AIR, (int)osdConvertVelocityToUnit(vel), SYM_KT); + break; + case OSD_SPEED_TYPE_3D: + strLen = tfp_sprintf(buff, "%3d%c", (int)osdConvertVelocityToUnit(vel), SYM_3D_KT); + break; + case OSD_SPEED_TYPE_MIN_GROUND: + strLen = tfp_sprintf(buff, "%c%3d%c", SYM_MIN_GROUND_SPEED, (int)osdConvertVelocityToUnit(vel), SYM_KT); + break; + } + } + break; } + + return strLen; } /** @@ -427,7 +515,7 @@ void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D, bool _max) */ static void osdGenerateAverageVelocityStr(char* buff) { uint32_t cmPerSec = getTotalTravelDistance() / getFlightTime(); - osdFormatVelocityStr(buff, cmPerSec, false, false); + osdFormatVelocityStr(buff, cmPerSec, OSD_SPEED_TYPE_GROUND, false); } /** @@ -1222,7 +1310,9 @@ int16_t osdGetPanServoOffset(void) servoMiddle = PWM_RANGE_MIDDLE + gimbalConfig()->panTrim; } - return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_pwm2centideg); + float servoDegreesScaleFactor = 1000.0f / (servoParams(servoIndex)->max - servoParams(servoIndex)->min); + + return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * (int16_t)(osdConfig()->osd_pan_servo_range_decadegrees) * servoDegreesScaleFactor); } // Returns a heading angle in degrees normalized to [0, 360). @@ -1407,6 +1497,41 @@ static void osdDrawRadar(uint16_t *drawn, uint32_t *usedScale) osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale); } +/** + * + * @param display + * @param gx + * @param gy + * @param degrees (in degrees 0 - 360) + * @param elemAttr + */ +static void osdDrawDirCardinal(displayPort_t *display, unsigned gx, unsigned gy, int degrees, textAttributes_t elemAttr) +{ + uint8_t iconIndexOffset = 0; + if(osdConfig()->video_system == VIDEO_SYSTEM_DJI_NATIVE) { + iconIndexOffset = constrain((osdGetHeadingAngle((int)degrees) / 30), 0, 12); + }else{ + int dir = wrap_180((int16_t)osdGetHeadingAngle((int)degrees)); + iconIndexOffset = constrain(((dir + 180) / 30), 0, 12); + } + + if (iconIndexOffset == 12) { + iconIndexOffset = 0; // Directly behind + } + displayWriteCharWithAttr(display, gx, gy, SYM_HUD_CARDINAL + iconIndexOffset, elemAttr); +} + +#ifdef USE_ADSB +const char* getAdsbEmitterTypeString(uint8_t typeStr) +{ + if (typeStr <= sizeof(ADSB_EMITTER_TYPE_STRINGS) / sizeof(ADSB_EMITTER_TYPE_STRINGS[0])){ + return ADSB_EMITTER_TYPE_STRINGS[typeStr]; + } + + return "UNK "; +} +#endif + static uint16_t crc_accumulate(uint8_t data, uint16_t crcAccum) { uint8_t tmp; @@ -1416,7 +1541,6 @@ static uint16_t crc_accumulate(uint8_t data, uint16_t crcAccum) return crcAccum; } - static void osdDisplayTelemetry(void) { uint32_t trk_data; @@ -1883,19 +2007,23 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_GPS_SPEED: - osdFormatVelocityStr(buff, gpsSol.groundSpeed, false, false); + osdFormatVelocityStr(buff, gpsSol.groundSpeed, OSD_SPEED_TYPE_GROUND, false); + break; + + case OSD_NAV_MIN_GROUND_SPEED: + osdFormatVelocityStr(buff, getMinGroundSpeed(navConfig()->general.min_ground_speed) * 100, OSD_SPEED_TYPE_MIN_GROUND, false); break; case OSD_GPS_MAX_SPEED: - osdFormatVelocityStr(buff, stats.max_speed, false, true); + osdFormatVelocityStr(buff, stats.max_speed, OSD_SPEED_TYPE_GROUND, true); break; case OSD_3D_SPEED: - osdFormatVelocityStr(buff, osdGet3DSpeed(), true, false); + osdFormatVelocityStr(buff, osdGet3DSpeed(), OSD_SPEED_TYPE_3D, false); break; case OSD_3D_MAX_SPEED: - osdFormatVelocityStr(buff, stats.max_3D_speed, true, true); + osdFormatVelocityStr(buff, stats.max_3D_speed, OSD_SPEED_TYPE_3D, true); break; case OSD_GLIDESLOPE: @@ -1940,11 +2068,10 @@ static bool osdDrawSingleElement(uint8_t item) else { int16_t panHomeDirOffset = 0; - if (!(osdConfig()->pan_servo_pwm2centideg == 0)){ + if (!(osdConfig()->osd_pan_servo_range_decadegrees == 0)){ panHomeDirOffset = osdGetPanServoOffset(); } - int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading()); - int homeDirection = GPS_directionToHome - flightDirection + panHomeDirOffset; + int homeDirection = GPS_directionToHome - osdGetFlightDirection() + panHomeDirOffset; osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), homeDirection); } } else { @@ -2134,66 +2261,118 @@ static bool osdDrawSingleElement(uint8_t item) #ifdef USE_ADSB case OSD_ADSB_WARNING: { - static uint8_t adsblen = 1; - uint8_t arrowPositionX = 0; - - for (int i = 0; i <= adsblen; i++) { - buff[i] = SYM_BLANK; - } - - buff[adsblen]='\0'; - displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); // clear any previous chars because variable element size - adsblen=1; - adsbVehicle_t *vehicle = findVehicleClosest(); + static uint8_t adsbLengthForClearFirstLine = 0; + static uint8_t adsbLengthForClearSecondLine = 0; - if(vehicle != NULL){ + uint8_t buffIndexFirstLine = 0; + uint8_t arrowIndexIndex = 0; + adsbVehicle_t *vehicle = findVehicleClosestLimit(METERS_TO_CENTIMETERS(osdConfig()->adsb_ignore_plane_above_me_limit)); + if (vehicle != NULL) { recalculateVehicle(vehicle); } if ( - vehicle != NULL && - (vehicle->calculatedVehicleValues.dist > 0) && - vehicle->calculatedVehicleValues.dist < METERS_TO_CENTIMETERS(osdConfig()->adsb_distance_warning) && - (osdConfig()->adsb_ignore_plane_above_me_limit == 0 || METERS_TO_CENTIMETERS(osdConfig()->adsb_ignore_plane_above_me_limit) > vehicle->calculatedVehicleValues.verticalDistance) - ){ - buff[0] = SYM_ADSB; - osdFormatDistanceStr(&buff[1], (int32_t)vehicle->calculatedVehicleValues.dist); - adsblen = strlen(buff); - - buff[adsblen-1] = SYM_BLANK; - - arrowPositionX = adsblen-1; - osdFormatDistanceStr(&buff[adsblen], vehicle->calculatedVehicleValues.verticalDistance); - adsblen = strlen(buff)-1; + vehicle != NULL + && (vehicle->calculatedVehicleValues.dist > 0 + && vehicle->calculatedVehicleValues.dist < METERS_TO_CENTIMETERS(osdConfig()->adsb_distance_warning)) + && isEnvironmentOkForCalculatingADSBDistanceBearing() + + ) { + adsbLengthForClearFirstLine = 11; + + buff[buffIndexFirstLine++] = SYM_ADSB; + + ////////////////////////////////////////////////////// + // distance to ADSB vehicle + osdFormatDistanceSymbol(&buff[buffIndexFirstLine], (int32_t)vehicle->calculatedVehicleValues.dist, 0, 3); + buffIndexFirstLine += 4; + ////////////////////////////////////////////////////// + + ////////////////////////////////////////////////////// + // direction to ADSB vehicle + arrowIndexIndex = buffIndexFirstLine; + buff[arrowIndexIndex] = SYM_BLANK; // space for direction arrow + buffIndexFirstLine++; + ////////////////////////////////////////////////////// + + ////////////////////////////////////////////////////// + // ALT diff to ADSB vehicle just space + osdFormatAltitudeSymbol(&buff[buffIndexFirstLine], ((int32_t)vehicle->calculatedVehicleValues.verticalDistance)); + buffIndexFirstLine += osdConfig()->decimals_altitude + 2; + ////////////////////////////////////////////////////// if (vehicle->calculatedVehicleValues.dist < METERS_TO_CENTIMETERS(osdConfig()->adsb_distance_alert)) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } - } - buff[adsblen]='\0'; - displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); + buff[buffIndexFirstLine]='\0'; + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY, buff, elemAttr); - if (arrowPositionX > 0){ - int16_t panHomeDirOffset = 0; - if (osdConfig()->pan_servo_pwm2centideg != 0){ - panHomeDirOffset = osdGetPanServoOffset(); + ////////////////////////////////////////////////////// + // ALT diff to ADSB vehicle draw + int16_t panServoDirOffset = 0; + if (osdConfig()->osd_pan_servo_range_decadegrees != 0){ + panServoDirOffset = osdGetPanServoOffset(); } - int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading()); - osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX + arrowPositionX, elemPosY), CENTIDEGREES_TO_DEGREES(vehicle->calculatedVehicleValues.dir) - flightDirection + panHomeDirOffset); - } + if (arrowIndexIndex > 0 && isImuHeadingValid()) { + //[direction to vehicle] + int directionToPeerError = osdGetHeadingAngle(CENTIDEGREES_TO_DEGREES(vehicle->calculatedVehicleValues.dir)) + panServoDirOffset - osdGetFlightDirection(); + osdDrawDirCardinal(osdDisplayPort, elemPosX + arrowIndexIndex, elemPosY, directionToPeerError, elemAttr); + } + ////////////////////////////////////////////////////// + + ////////////////////////////////////////////////////// + // Second line, extra info + if (osdConfig()->adsb_warning_style == OSD_ADSB_WARNING_STYLE_EXTENDED) { + // Vehicle type + tfp_sprintf(buff, "%s", getAdsbEmitterTypeString(vehicle->vehicleValues.emitterType)); + + // Vehicle speed + adsbLengthForClearSecondLine = osdFormatVelocityStr(buff + 7, vehicle->vehicleValues.horVelocity, OSD_SPEED_TYPE_GROUND, false); + + // draw values + buff[6] = SYM_BLANK; // space for direction arrow + displayWriteWithAttr(osdDisplayPort, elemPosX, elemPosY + 1, buff, elemAttr); + + // Vehicle direction + if(isImuHeadingValid()) + { + osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX + 6, elemPosY + 1), (float)(CENTIDEGREES_TO_DEGREES(vehicle->vehicleValues.heading) - osdGetFlightDirection() + panServoDirOffset)); + } + + adsbLengthForClearSecondLine += 7; + } + /////////////////////////////////////////////////// + } else { + //clear first line + if(adsbLengthForClearFirstLine > 0) { + memset(buff, SYM_BLANK, constrain(adsbLengthForClearFirstLine, 0, 20)); + displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); + adsbLengthForClearFirstLine = 0; + } + + //clear second line + if(adsbLengthForClearSecondLine > 0) { + memset(buff, SYM_BLANK, constrain(adsbLengthForClearSecondLine, 0, 20)); + displayWrite(osdDisplayPort, elemPosX, elemPosY + 1, buff); + adsbLengthForClearSecondLine = 0; + } + } return true; } case OSD_ADSB_INFO: { buff[0] = SYM_ADSB; - if(getAdsbStatus()->vehiclesMessagesTotal > 0 || getAdsbStatus()->heartbeatMessagesTotal > 0){ - tfp_sprintf(buff + 1, "%2d", getActiveVehiclesCount()); - }else{ + if (getAdsbStatus()->vehiclesMessagesTotal == 0 && getAdsbStatus()->heartbeatMessagesTotal == 0) { buff[1] = '-'; + } else if (!isEnvironmentOkForCalculatingADSBDistanceBearing()) { + buff[1] = 'G'; + } else if (!isImuHeadingValid()) { + buff[1] = 'H'; + } else { + tfp_sprintf(buff + 1, "%1d", getActiveVehiclesCount()); } - break; } @@ -2390,16 +2569,15 @@ static bool osdDrawSingleElement(uint8_t item) if (FLIGHT_MODE(NAV_FW_AUTOLAND)) p = "LAND"; else -#endif -#ifdef USE_GEOZONE - if (FLIGHT_MODE(NAV_SEND_TO)) - p = "AUTO"; - else #endif if (FLIGHT_MODE(FAILSAFE_MODE)) p = "!FS!"; else if (FLIGHT_MODE(MANUAL_MODE)) p = "MANU"; +#ifdef USE_GEOZONE + else if (FLIGHT_MODE(NAV_SEND_TO) && !FLIGHT_MODE(NAV_WP_MODE)) + p = "GEO "; +#endif else if (FLIGHT_MODE(TURTLE_MODE)) p = "TURT"; else if (FLIGHT_MODE(NAV_RTH_MODE)) @@ -2558,14 +2736,11 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_SNR_DB: { - static pt1Filter_t snrFilterState; - static timeMs_t snrUpdated = 0; - int8_t snrFiltered = pt1FilterApply4(&snrFilterState, rxLinkStatistics.uplinkSNR, 0.5f, MS2S(millis() - snrUpdated)); - snrUpdated = millis(); + int8_t snrVal = rxLinkStatistics.uplinkSNR; const char* showsnr = "-20"; const char* hidesnr = " "; - if (snrFiltered > osdConfig()->snr_alarm) { + if (snrVal > osdConfig()->snr_alarm) { if (cmsInMenu) { buff[0] = SYM_SNR; tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB); @@ -2573,12 +2748,12 @@ static bool osdDrawSingleElement(uint8_t item) buff[0] = SYM_BLANK; tfp_sprintf(buff + 1, "%s%c", hidesnr, SYM_BLANK); } - } else if (snrFiltered <= osdConfig()->snr_alarm) { + } else if (snrVal <= osdConfig()->snr_alarm) { buff[0] = SYM_SNR; - if (snrFiltered <= -10 || snrFiltered >= 10) { - tfp_sprintf(buff + 1, "%3d%c", snrFiltered, SYM_DB); + if (snrVal <= -10) { + tfp_sprintf(buff + 1, "%3d%c", snrVal, SYM_DB); } else { - tfp_sprintf(buff + 1, " %2d%c", snrFiltered, SYM_DB); + tfp_sprintf(buff + 1, "%2d%c%c", snrVal, SYM_DB, ' '); } } break; @@ -2648,7 +2823,7 @@ static bool osdDrawSingleElement(uint8_t item) currentPeer->direction = (int16_t )(calculateBearingToDestination(&poi) / 100); // In ° int16_t panServoDirOffset = 0; - if (osdConfig()->pan_servo_pwm2centideg != 0){ + if (osdConfig()->osd_pan_servo_range_decadegrees != 0){ panServoDirOffset = osdGetPanServoOffset(); } @@ -2666,13 +2841,8 @@ static bool osdDrawSingleElement(uint8_t item) displayWriteChar(osdDisplayPort, elemPosX + 2, elemPosY, SYM_HUD_SIGNAL_0 + currentPeer->lq); //[direction to peer] - int directionToPeerError = wrap_180(osdGetHeadingAngle(currentPeer->direction) + panServoDirOffset - (int)DECIDEGREES_TO_DEGREES(osdGetHeading())); - uint16_t iconIndexOffset = constrain(((directionToPeerError + 180) / 30), 0, 12); - if (iconIndexOffset == 12) { - iconIndexOffset = 0; // Directly behind - } - displayWriteChar(osdDisplayPort, elemPosX + 3, elemPosY, SYM_HUD_CARDINAL + iconIndexOffset); - + int directionToPeerError = osdGetHeadingAngle(currentPeer->direction) + panServoDirOffset - (int)DECIDEGREES_TO_DEGREES(osdGetHeading()); + osdDrawDirCardinal(osdDisplayPort, elemPosX + 3, elemPosY, directionToPeerError, elemAttr); //line 2 switch ((osd_unit_e)osdConfig()->units) { @@ -2852,6 +3022,13 @@ static bool osdDrawSingleElement(uint8_t item) return true; } + case OSD_THROTTLE_GAUGE: + { + bool useScaled = navigationIsControllingThrottle(); + osdThrottleGauge(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), getThrottlePercent(useScaled)); + return true; + } + #if defined(USE_BARO) || defined(USE_GPS) case OSD_VARIO: { @@ -2860,7 +3037,7 @@ static bool osdDrawSingleElement(uint8_t item) return true; } - case OSD_VARIO_NUM: + case OSD_VERTICAL_SPEED_INDICATOR: { int16_t value = getEstimatedActualVelocity(Z); char sym; @@ -3101,22 +3278,22 @@ static bool osdDrawSingleElement(uint8_t item) return true; case OSD_RC_EXPO: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "EXP", 0, currentControlRateProfile->stabilized.rcExpo8, 3, 0, ADJUSTMENT_RC_EXPO); + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "EXP", 0, currentControlProfile->stabilized.rcExpo8, 3, 0, ADJUSTMENT_RC_EXPO); return true; case OSD_RC_YAW_EXPO: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "YEX", 0, currentControlRateProfile->stabilized.rcYawExpo8, 3, 0, ADJUSTMENT_RC_YAW_EXPO); + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "YEX", 0, currentControlProfile->stabilized.rcYawExpo8, 3, 0, ADJUSTMENT_RC_YAW_EXPO); return true; case OSD_THROTTLE_EXPO: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TEX", 0, currentControlRateProfile->throttle.rcExpo8, 3, 0, ADJUSTMENT_THROTTLE_EXPO); + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TEX", 0, currentControlProfile->throttle.rcExpo8, 3, 0, ADJUSTMENT_THROTTLE_EXPO); return true; case OSD_PITCH_RATE: displayWrite(osdDisplayPort, elemPosX, elemPosY, "SPR"); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_PITCH]); + tfp_sprintf(buff, "%3d", currentControlProfile->stabilized.rates[FD_PITCH]); if (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); @@ -3126,29 +3303,29 @@ static bool osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX, elemPosY, "SRR"); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlRateProfile->stabilized.rates[FD_ROLL]); + tfp_sprintf(buff, "%3d", currentControlProfile->stabilized.rates[FD_ROLL]); if (isAdjustmentFunctionSelected(ADJUSTMENT_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_RATE)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); return true; case OSD_YAW_RATE: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "SYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE); + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "SYR", 0, currentControlProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE); return true; case OSD_MANUAL_RC_EXPO: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MEX", 0, currentControlRateProfile->manual.rcExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_EXPO); + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MEX", 0, currentControlProfile->manual.rcExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_EXPO); return true; case OSD_MANUAL_RC_YAW_EXPO: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYX", 0, currentControlRateProfile->manual.rcYawExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_YAW_EXPO); + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYX", 0, currentControlProfile->manual.rcYawExpo8, 3, 0, ADJUSTMENT_MANUAL_RC_YAW_EXPO); return true; case OSD_MANUAL_PITCH_RATE: displayWrite(osdDisplayPort, elemPosX, elemPosY, "MPR"); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_PITCH]); + tfp_sprintf(buff, "%3d", currentControlProfile->manual.rates[FD_PITCH]); if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); @@ -3158,14 +3335,14 @@ static bool osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX, elemPosY, "MRR"); elemAttr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlRateProfile->manual.rates[FD_ROLL]); + tfp_sprintf(buff, "%3d", currentControlProfile->manual.rates[FD_ROLL]); if (isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_ROLL_RATE) || isAdjustmentFunctionSelected(ADJUSTMENT_MANUAL_PITCH_ROLL_RATE)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr); return true; case OSD_MANUAL_YAW_RATE: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYR", 0, currentControlRateProfile->stabilized.rates[FD_YAW], 3, 0, ADJUSTMENT_YAW_RATE); + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "MYR", 0, currentControlProfile->manual.rates[FD_YAW], 3, 0, ADJUSTMENT_MANUAL_YAW_RATE); return true; case OSD_NAV_FW_CRUISE_THR: @@ -3245,12 +3422,10 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_AIR_SPEED: { #ifdef USE_PITOT - buff[0] = SYM_AIR; - if (pitotIsHealthy()) { const float airspeed_estimate = getAirspeedEstimate(); - osdFormatVelocityStr(buff + 1, airspeed_estimate, false, false); + osdFormatVelocityStr(buff, airspeed_estimate, OSD_SPEED_TYPE_AIR, false); if ((osdConfig()->airspeed_alarm_min != 0 && airspeed_estimate < osdConfig()->airspeed_alarm_min) || (osdConfig()->airspeed_alarm_max != 0 && airspeed_estimate > osdConfig()->airspeed_alarm_max)) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); @@ -3258,6 +3433,7 @@ static bool osdDrawSingleElement(uint8_t item) } else { + buff[0] = SYM_AIR; strcpy(buff + 1, " X!"); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } @@ -3270,9 +3446,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_AIR_MAX_SPEED: { #ifdef USE_PITOT - buff[0] = SYM_MAX; - buff[1] = SYM_AIR; - osdFormatVelocityStr(buff + 2, stats.max_air_speed, false, false); + osdFormatVelocityStr(buff, stats.max_air_speed, OSD_SPEED_TYPE_AIR, true); #else return false; #endif @@ -3773,7 +3947,7 @@ static bool osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX, elemPosY, "TPA"); attr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%3d", currentControlRateProfile->throttle.dynPID); + tfp_sprintf(buff, "%3d", currentControlProfile->throttle.dynPID); if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA)) { TEXT_ATTRIBUTES_ADD_BLINK(attr); } @@ -3781,7 +3955,7 @@ static bool osdDrawSingleElement(uint8_t item) displayWrite(osdDisplayPort, elemPosX, elemPosY + 1, "BP"); attr = TEXT_ATTRIBUTES_NONE; - tfp_sprintf(buff, "%4d", currentControlRateProfile->throttle.pa_breakpoint); + tfp_sprintf(buff, "%4d", currentControlProfile->throttle.pa_breakpoint); if (isAdjustmentFunctionSelected(ADJUSTMENT_TPA_BREAKPOINT)) { TEXT_ATTRIBUTES_ADD_BLINK(attr); } @@ -3791,7 +3965,7 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_TPA_TIME_CONSTANT: { - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TPA TC", 0, currentControlRateProfile->throttle.fixedWingTauMs, 4, 0, ADJUSTMENT_FW_TPA_TIME_CONSTANT); + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "TPA TC", 0, currentControlProfile->throttle.fixedWingTauMs, 4, 0, ADJUSTMENT_FW_TPA_TIME_CONSTANT); return true; } case OSD_FW_LEVEL_TRIM: @@ -3912,11 +4086,10 @@ static bool osdDrawSingleElement(uint8_t item) { if (navigationPositionEstimateIsHealthy() && isGeozoneActive()) { int16_t panHomeDirOffset = 0; - if (!(osdConfig()->pan_servo_pwm2centideg == 0)){ + if (!(osdConfig()->osd_pan_servo_range_decadegrees == 0)){ panHomeDirOffset = osdGetPanServoOffset(); } - int16_t flightDirection = STATE(AIRPLANE) ? CENTIDEGREES_TO_DEGREES(posControl.actualState.cog) : DECIDEGREES_TO_DEGREES(osdGetHeading()); - int direction = CENTIDEGREES_TO_DEGREES(geozone.directionToNearestZone) - flightDirection + panHomeDirOffset; + int direction = CENTIDEGREES_TO_DEGREES(geozone.directionToNearestZone) - osdGetFlightDirection() + panHomeDirOffset; osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), direction); } else { if (isGeozoneActive()) { @@ -4069,6 +4242,18 @@ uint8_t osdIncElementIndex(uint8_t elementIndex) return elementIndex; } +static void osdDrawAllElements(void) +{ + for (uint8_t element = 0; element < OSD_ITEM_COUNT; element++) { + osdDrawSingleElement(element); + } + + osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); + if (osdConfig()->telemetry>0){ + osdDisplayTelemetry(); + } +} + void osdDrawNextElement(void) { static uint8_t elementIndex = 0; @@ -4107,6 +4292,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .adsb_distance_warning = SETTING_OSD_ADSB_DISTANCE_WARNING_DEFAULT, .adsb_distance_alert = SETTING_OSD_ADSB_DISTANCE_ALERT_DEFAULT, .adsb_ignore_plane_above_me_limit = SETTING_OSD_ADSB_IGNORE_PLANE_ABOVE_ME_LIMIT_DEFAULT, + .adsb_warning_style = SETTING_OSD_ADSB_WARNING_STYLE_DEFAULT, #endif #if defined(USE_SERIALRX_CRSF) || defined(USE_RX_MSP) .snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT, @@ -4130,6 +4316,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .video_system = SETTING_OSD_VIDEO_SYSTEM_DEFAULT, .row_shiftdown = SETTING_OSD_ROW_SHIFTDOWN_DEFAULT, .msp_displayport_fullframe_interval = SETTING_OSD_MSP_DISPLAYPORT_FULLFRAME_INTERVAL_DEFAULT, + .framerate_hz = SETTING_OSD_FRAMERATE_HZ_DEFAULT, .ahi_reverse_roll = SETTING_OSD_AHI_REVERSE_ROLL_DEFAULT, .ahi_max_pitch = SETTING_OSD_AHI_MAX_PITCH_DEFAULT, @@ -4159,7 +4346,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .ahi_pitch_interval = SETTING_OSD_AHI_PITCH_INTERVAL_DEFAULT, .osd_home_position_arm_screen = SETTING_OSD_HOME_POSITION_ARM_SCREEN_DEFAULT, .pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT, - .pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT, + .osd_pan_servo_range_decadegrees = SETTING_OSD_PAN_SERVO_RANGE_DECADEGREES_DEFAULT, .pan_servo_offcentre_warning = SETTING_OSD_PAN_SERVO_OFFCENTRE_WARNING_DEFAULT, .pan_servo_indicator_show_degrees = SETTING_OSD_PAN_SERVO_INDICATOR_SHOW_DEGREES_DEFAULT, .esc_rpm_precision = SETTING_OSD_ESC_RPM_PRECISION_DEFAULT, @@ -4246,10 +4433,11 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_ATTITUDE_ROLL] = OSD_POS(1, 7); osdLayoutsConfig->item_pos[0][OSD_ATTITUDE_PITCH] = OSD_POS(1, 8); + osdLayoutsConfig->item_pos[0][OSD_THROTTLE_GAUGE] = OSD_POS(23, 5); // avoid OSD_VARIO under OSD_CROSSHAIRS osdLayoutsConfig->item_pos[0][OSD_VARIO] = OSD_POS(23, 5); - // OSD_VARIO_NUM at the right of OSD_VARIO - osdLayoutsConfig->item_pos[0][OSD_VARIO_NUM] = OSD_POS(24, 7); + // OSD_VERTICAL_SPEED_INDICATOR at the right of OSD_VARIO + osdLayoutsConfig->item_pos[0][OSD_VERTICAL_SPEED_INDICATOR] = OSD_POS(24, 7); osdLayoutsConfig->item_pos[0][OSD_HOME_DIR] = OSD_POS(14, 11); osdLayoutsConfig->item_pos[0][OSD_ARTIFICIAL_HORIZON] = OSD_POS(8, 6); osdLayoutsConfig->item_pos[0][OSD_HORIZON_SIDEBARS] = OSD_POS(8, 6); @@ -4837,7 +5025,7 @@ uint8_t drawStat_Speed(uint8_t col, uint8_t row, uint8_t statValX) displayWrite(osdDisplayPort, col, row, "MAX/AVG SPEED"); - osdFormatVelocityStr(buff2, stats.max_3D_speed, true, false); + osdFormatVelocityStr(buff2, stats.max_3D_speed, OSD_SPEED_TYPE_3D, false); tfp_sprintf(buff, ": %s/", osdFormatTrimWhiteSpace(buff2)); multiValueXOffset = strlen(buff); displayWrite(osdDisplayPort, statValX, row, buff); @@ -5801,7 +5989,28 @@ static void osdRefresh(timeUs_t currentTimeUs) displayClearScreen(osdDisplayPort); fullRedraw = false; } - osdDrawNextElement(); + + if (osdConfig()->osd_framerate_hz == -1) { + osdDrawNextElement(); + } else { + static uint32_t lastDrawAllTimeUs = 0; + const int8_t hz = osdConfig()->osd_framerate_hz; + const uint32_t drawAllIntervalUs = (hz > 0) ? (1000000 / hz) : 0; + + const bool forceDraw = (drawAllIntervalUs == 0); + const bool intervalExceeded = (currentTimeUs - lastDrawAllTimeUs) >= drawAllIntervalUs; + + if (forceDraw || intervalExceeded) { + osdDrawAllElements(); + lastDrawAllTimeUs = currentTimeUs; + } + + osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); + if (osdConfig()->telemetry>0){ + osdDisplayTelemetry(); + } + } + displayHeartbeat(osdDisplayPort); displayCommitTransaction(osdDisplayPort); #ifdef OSD_CALLS_CMS @@ -6138,7 +6347,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { if (posControl.cruise.multicopterSpeed >= 50.0f) { char buf[6]; - osdFormatVelocityStr(buf, posControl.cruise.multicopterSpeed, false, false); + osdFormatVelocityStr(buf, posControl.cruise.multicopterSpeed, OSD_SPEED_TYPE_GROUND, false); tfp_sprintf(messageBuf, "(SPD %s)", buf); } else { strcpy(messageBuf, "(HOLD)"); @@ -6418,4 +6627,56 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) return elemAttr; } +void osdDrawCustomItem(uint8_t item){ + osdDrawSingleElement(item); +} + +void osdEraseCustomItem(uint8_t item){ + uint8_t customElementIndex = 0; + + uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item]; + uint8_t elemPosX = OSD_X(pos); + uint8_t elemPosY = OSD_Y(pos); + + switch(item){ + case 147: + customElementIndex = 0; + break; + case 148: + customElementIndex = 1; + break; + case 149: + customElementIndex = 2; + break; + case 154: + customElementIndex = 3; + break; + case 155: + customElementIndex = 4; + break; + case 156: + customElementIndex = 5; + break; + case 157: + customElementIndex = 6; + break; + case 158: + customElementIndex = 7; + break; + default: + return; + } + + uint8_t len = customElementLength(customElementIndex); + + for(uint8_t i = 0; i < len; i++){ + displayWriteChar(osdDisplayPort, elemPosX+i, elemPosY, SYM_BLANK); + } + +} + #endif // OSD + +unsigned getCurrentLayout(void){ + return(currentLayout); +} diff --git a/src/main/io/osd.h b/src/main/io/osd.h index b2e94b52729..d644d841a01 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -145,6 +145,31 @@ #define OSD_MSG_GEOZONE_ACTION "PERFORM ACTION IN %s %s" #endif +#ifdef USE_ADSB +static const char* const ADSB_EMITTER_TYPE_STRINGS[] = { + "NOINFO", // 0 - No information about the emitter type + "LIGHT ", // 1 - Light aircraft + "SMALL ", // 2 - Small aircraft + "LARGE ", // 3 - Large aircraft + "HVLARG", // 4 - High vortex large + "HEAVY ", // 5 - Heavy aircraft + "HMANUV", // 6 - Highly maneuverable aircraft + "ROTORC", // 7 - Rotocraft (e.g., helicopter) + "UNASGN", // 8 - Unassigned type + "GLIDER", // 9 - Glider + "LTAIR ", // 10 - Lighter-than-air aircraft + "PARACH", // 11 - Parachute + "ULTLIT", // 12 - Ultra light aircraft + "UNASG2", // 13 - Unassigned 2 + "UAV ", // 14 - Unmanned Aerial Vehicle (drone) + "SPACE ", // 15 - Spacecraft + "UNASG3", // 16 - Unassigned 3 + "EMRSUR", // 17 - Emergency surface vehicle + "SERSUR", // 18 - Service surface vehicle + "POBSTC", // 19 - Point obstacle +}; +#endif + typedef enum { OSD_RSSI_VALUE, OSD_MAIN_BATT_VOLTAGE, @@ -172,7 +197,7 @@ typedef enum { OSD_HOME_DIST, OSD_HEADING, OSD_VARIO, - OSD_VARIO_NUM, + OSD_VERTICAL_SPEED_INDICATOR, OSD_AIR_SPEED, OSD_ONTIME_FLYTIME, OSD_RTC_TIME, @@ -313,6 +338,8 @@ typedef enum { OSD_H_DIST_TO_FENCE, OSD_V_DIST_TO_FENCE, OSD_NAV_FW_ALT_CONTROL_RESPONSE, + OSD_NAV_MIN_GROUND_SPEED, + OSD_THROTTLE_GAUGE, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -355,6 +382,11 @@ typedef enum { OSD_ALIGN_RIGHT } osd_alignment_e; +typedef enum { + OSD_ADSB_WARNING_STYLE_COMPACT, + OSD_ADSB_WARNING_STYLE_EXTENDED, +} osd_adsb_warning_style_e; + typedef enum { OSD_AHI_STYLE_DEFAULT, OSD_AHI_STYLE_LINE, @@ -366,6 +398,13 @@ typedef enum { OSD_CRSF_LQ_TYPE3 } osd_crsf_lq_format_e; +typedef enum { + OSD_SPEED_TYPE_GROUND, + OSD_SPEED_TYPE_AIR, + OSD_SPEED_TYPE_3D, + OSD_SPEED_TYPE_MIN_GROUND, +} osd_SpeedTypes_e; + typedef struct osdLayoutsConfig_s { // Layouts uint16_t item_pos[OSD_LAYOUT_COUNT][OSD_ITEM_COUNT]; @@ -412,6 +451,7 @@ typedef struct osdConfig_s { videoSystem_e video_system; uint8_t row_shiftdown; int16_t msp_displayport_fullframe_interval; + int8_t osd_framerate_hz; // Preferences uint8_t main_voltage_decimals; @@ -464,7 +504,7 @@ typedef struct osdConfig_s { uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. bool osd_home_position_arm_screen; uint8_t pan_servo_index; // Index of the pan servo used for home direction offset - int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm + int8_t osd_pan_servo_range_decadegrees; // Decadegrees of servo rotation uint8_t pan_servo_offcentre_warning; // Degrees around the centre, that is assumed camera is wanted to be facing forwards, but isn't centred bool pan_servo_indicator_show_degrees; // Show the degrees of offset for the pan servo uint8_t crsf_lq_format; @@ -491,9 +531,10 @@ typedef struct osdConfig_s { #endif bool enable_broken_o4_workaround; // If enabled, override STATUS/STATUS_EX messages to work around DJI's broken O4 air unit MSP DisplayPort implementation #ifdef USE_ADSB - uint16_t adsb_distance_warning; // in metres - uint16_t adsb_distance_alert; // in metres - uint16_t adsb_ignore_plane_above_me_limit; // in metres + uint16_t adsb_distance_warning; // in metres + uint16_t adsb_distance_alert; // in metres + uint16_t adsb_ignore_plane_above_me_limit; // in metres + osd_adsb_warning_style_e adsb_warning_style; // adsb warning element style, one or two lines #endif uint8_t radar_peers_display_time; // in seconds #ifdef USE_GEOZONE @@ -507,6 +548,10 @@ PG_DECLARE(osdConfig_t, osdConfig); typedef struct displayPort_s displayPort_t; typedef struct displayCanvas_s displayCanvas_t; +void osdDrawCustomItem(uint8_t item); +void osdEraseCustomItem(uint8_t item); +unsigned getCurrentLayout(void); + void osdInit(displayPort_t *osdDisplayPort); bool osdDisplayIsPAL(void); void osdUpdate(timeUs_t currentTimeUs); @@ -537,7 +582,7 @@ void osdShowEEPROMSavedNotification(void); void osdCrosshairPosition(uint8_t *x, uint8_t *y); bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length, bool leadingZeros); void osdFormatAltitudeSymbol(char *buff, int32_t alt); -void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D, bool _max); +int osdFormatVelocityStr(char* buff, int32_t vel, osd_SpeedTypes_e speedType, bool _max); // Returns a heading angle in degrees normalized to [0, 360). int osdGetHeadingAngle(int angle); diff --git a/src/main/io/osd/custom_elements.c b/src/main/io/osd/custom_elements.c index 15dc8714c9f..2ad5a82885e 100644 --- a/src/main/io/osd/custom_elements.c +++ b/src/main/io/osd/custom_elements.c @@ -33,6 +33,8 @@ PG_REGISTER_ARRAY_WITH_RESET_FN(osdCustomElement_t, MAX_CUSTOM_ELEMENTS, osdCustomElements, PG_OSD_CUSTOM_ELEMENTS_CONFIG, 1); +static uint8_t prevLength[MAX_CUSTOM_ELEMENTS]; + void pgResetFn_osdCustomElements(osdCustomElement_t *instance) { for (int i = 0; i < MAX_CUSTOM_ELEMENTS; i++) { @@ -228,8 +230,6 @@ void customElementDrawElement(char *buff, uint8_t customElementIndex){ return; } - static uint8_t prevLength[MAX_CUSTOM_ELEMENTS]; - uint8_t buffSeek = 0; const osdCustomElement_t* customElement = osdCustomElements(customElementIndex); if(isCustomelementVisible(customElement)) @@ -246,3 +246,7 @@ void customElementDrawElement(char *buff, uint8_t customElementIndex){ } prevLength[customElementIndex] = buffSeek; } + +uint8_t customElementLength(uint8_t customElementIndex){ + return prevLength[customElementIndex] ? prevLength[customElementIndex] : 1; +} diff --git a/src/main/io/osd/custom_elements.h b/src/main/io/osd/custom_elements.h index 8f5ee158811..5a28c91e19d 100644 --- a/src/main/io/osd/custom_elements.h +++ b/src/main/io/osd/custom_elements.h @@ -80,4 +80,5 @@ typedef struct { PG_DECLARE_ARRAY(osdCustomElement_t, MAX_CUSTOM_ELEMENTS, osdCustomElements); -void customElementDrawElement(char *buff, uint8_t customElementIndex); \ No newline at end of file +void customElementDrawElement(char *buff, uint8_t customElementIndex); +uint8_t customElementLength(uint8_t customElementIndex); \ No newline at end of file diff --git a/src/main/io/osd_canvas.c b/src/main/io/osd_canvas.c index 47dd4ee47bc..9500be365eb 100644 --- a/src/main/io/osd_canvas.c +++ b/src/main/io/osd_canvas.c @@ -71,6 +71,67 @@ static void osdCanvasVarioRect(int *y, int *h, displayCanvas_t *canvas, int midY *h = height; } +void osdCanvasDrawThrottleGauge(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, uint8_t thrPos) +{ + UNUSED(display); + + static uint8_t prevThr = 0; + + if (prevThr == (uint8_t)(thrPos / (OSD_THROTTLE_GAUGE_HEIGHT_ROWS * 2))) { + return; + } + + // Make sure we clear the grid buffer + uint8_t gx; + uint8_t gy; + osdDrawPointGetGrid(&gx, &gy, display, canvas, p); + osdGridBufferClearGridRect(gx, gy, 1, OSD_THROTTLE_GAUGE_HEIGHT_ROWS); + + int x = gx * canvas->gridElementWidth; + int w = canvas->gridElementWidth; + int y = gy * canvas->gridElementHeight; + int h = OSD_THROTTLE_GAUGE_HEIGHT_ROWS * canvas->gridElementHeight; + + displayCanvasClearRect(canvas, x, y, w, h); + + if (thrPos >= 100) + displayCanvasDrawCharacter(canvas, x, y, SYM_THR_GAUGE_FULL, DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT); + else if (thrPos >= 90) + displayCanvasDrawCharacter(canvas, x, y, SYM_THR_GAUGE_HALF, DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT); + else + displayCanvasDrawCharacter(canvas, x, y, SYM_THR_GAUGE_EMPTY, DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT); + + if (thrPos >= 80) + displayCanvasDrawCharacter(canvas, x, y + canvas->gridElementHeight, SYM_THR_GAUGE_FULL, DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT); + else if (thrPos >= 70) + displayCanvasDrawCharacter(canvas, x, y + canvas->gridElementHeight, SYM_THR_GAUGE_HALF, DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT); + else + displayCanvasDrawCharacter(canvas, x, y + canvas->gridElementHeight, SYM_THR_GAUGE_EMPTY, DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT); + + if (thrPos >= 60) + displayCanvasDrawCharacter(canvas, x, y + (canvas->gridElementHeight * 2), SYM_THR_GAUGE_FULL, DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT); + else if (thrPos >= 50) + displayCanvasDrawCharacter(canvas, x, y + (canvas->gridElementHeight * 2), SYM_THR_GAUGE_HALF, DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT); + else + displayCanvasDrawCharacter(canvas, x, y + (canvas->gridElementHeight * 2), SYM_THR_GAUGE_EMPTY, DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT); + + if (thrPos >= 40) + displayCanvasDrawCharacter(canvas, x, y + (canvas->gridElementHeight * 3), SYM_THR_GAUGE_FULL, DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT); + else if (thrPos >= 30) + displayCanvasDrawCharacter(canvas, x, y + (canvas->gridElementHeight * 3), SYM_THR_GAUGE_HALF, DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT); + else + displayCanvasDrawCharacter(canvas, x, y + (canvas->gridElementHeight * 3), SYM_THR_GAUGE_EMPTY, DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT); + + if (thrPos >= 20) + displayCanvasDrawCharacter(canvas, x, y + (canvas->gridElementHeight * 4), SYM_THR_GAUGE_FULL, DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT); + else if (thrPos >= 10) + displayCanvasDrawCharacter(canvas, x, y + (canvas->gridElementHeight * 4), SYM_THR_GAUGE_HALF, DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT); + else + displayCanvasDrawCharacter(canvas, x, y + (canvas->gridElementHeight * 4), SYM_THR_GAUGE_EMPTY, DISPLAY_CANVAS_BITMAP_OPT_ERASE_TRANSPARENT); + + prevThr = (uint8_t)(thrPos / (OSD_THROTTLE_GAUGE_HEIGHT_ROWS * 2)); +} + void osdCanvasDrawVario(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float zvel) { UNUSED(display); diff --git a/src/main/io/osd_canvas.h b/src/main/io/osd_canvas.h index 6a8d013e807..0e01d1ab162 100644 --- a/src/main/io/osd_canvas.h +++ b/src/main/io/osd_canvas.h @@ -32,6 +32,7 @@ typedef struct displayPort_s displayPort_t; typedef struct displayCanvas_s displayCanvas_t; typedef struct osdDrawPoint_s osdDrawPoint_t; +void osdCanvasDrawThrottleGauge(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, uint8_t thrPos); void osdCanvasDrawVario(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float zvel); void osdCanvasDrawDirArrow(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float degrees); void osdCanvasDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float pitchAngle, float rollAngle); diff --git a/src/main/io/osd_common.c b/src/main/io/osd_common.c index 6969e181a5d..38563ed5d88 100644 --- a/src/main/io/osd_common.c +++ b/src/main/io/osd_common.c @@ -109,6 +109,23 @@ void osdDrawPointGetPixels(int *px, int *py, const displayPort_t *display, const } } +void osdThrottleGauge(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, uint8_t thrPos) +{ + uint8_t gx; + uint8_t gy; + + #if defined(USE_CANVAS) + if (canvas) { + osdCanvasDrawThrottleGauge(display, canvas, p, thrPos); + } else { +#endif + osdDrawPointGetGrid(&gx, &gy, display, canvas, p); + osdGridDrawThrottleGauge(display, gx, gy, thrPos); +#if defined(USE_CANVAS) + } +#endif +} + void osdDrawVario(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float zvel) { uint8_t gx; @@ -196,10 +213,13 @@ void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas) #endif #ifdef USE_GPS +/* + * 3D speed in cm/s + */ int16_t osdGet3DSpeed(void) { - int16_t vert_speed = getEstimatedActualVelocity(Z); - int16_t hor_speed = gpsSol.groundSpeed; + float vert_speed = getEstimatedActualVelocity(Z); + float hor_speed = (float)gpsSol.groundSpeed; return (int16_t)calc_length_pythagorean_2D(hor_speed, vert_speed); } #endif diff --git a/src/main/io/osd_common.h b/src/main/io/osd_common.h index 0f700c445e4..a032747effc 100644 --- a/src/main/io/osd_common.h +++ b/src/main/io/osd_common.h @@ -52,6 +52,8 @@ int16_t osdGetSpeedFromSelectedSource(void); #define OSD_VARIO_CM_S_PER_ARROW 50 // 1 arrow = 50cm/s #define OSD_VARIO_HEIGHT_ROWS 5 +#define OSD_THROTTLE_GAUGE_HEIGHT_ROWS 5 + #define OSD_AHI_HEIGHT 9 #define OSD_AHI_WIDTH 11 #define OSD_AHI_PREV_SIZE (OSD_AHI_WIDTH > OSD_AHI_HEIGHT ? OSD_AHI_WIDTH : OSD_AHI_HEIGHT) @@ -92,6 +94,7 @@ typedef struct osdDrawPoint_s { void osdDrawPointGetGrid(uint8_t *gx, uint8_t *gy, const displayPort_t *display, const displayCanvas_t *canvas, const osdDrawPoint_t *p); void osdDrawPointGetPixels(int *px, int *py, const displayPort_t *display, const displayCanvas_t *canvas, const osdDrawPoint_t *p); +void osdThrottleGauge(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, uint8_t thrPos); void osdDrawVario(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, float zvel); // Draws an arrow at the given point, where 0 degrees points to the top of the viewport and // positive angles result in clockwise rotation. If eraseBefore is true, the rect surrouing diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index cf4f5f8872d..a8adf663d57 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -44,7 +44,7 @@ #include "fc/fc_core.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/fc_msp.h" #include "fc/fc_msp_box.h" #include "fc/runtime_config.h" @@ -192,7 +192,7 @@ const djiOsdMapping_t djiOSDItemIndexMap[] = { { OSD_HOME_DIR, FEATURE_GPS }, // DJI: OSD_HOME_DIR { OSD_HOME_DIST, FEATURE_GPS }, // DJI: OSD_HOME_DIST { OSD_HEADING, 0 }, // DJI: OSD_NUMERICAL_HEADING - { OSD_VARIO_NUM, 0 }, // DJI: OSD_NUMERICAL_VARIO + { OSD_VERTICAL_SPEED_INDICATOR, 0 }, // DJI: OSD_NUMERICAL_VARIO { -1, 0 }, // DJI: OSD_COMPASS_BAR { OSD_ESC_TEMPERATURE, 0 }, // DJI: OSD_ESC_TEMPERATURE { OSD_ESC_RPM, 0 }, // DJI: OSD_ESC_RPM @@ -805,43 +805,43 @@ static void osdDJIAdjustmentMessage(char *buff, uint8_t adjustmentFunction) { switch (adjustmentFunction) { case ADJUSTMENT_RC_EXPO: - tfp_sprintf(buff, "RCE %d", currentControlRateProfile->stabilized.rcExpo8); + tfp_sprintf(buff, "RCE %d", currentControlProfile->stabilized.rcExpo8); break; case ADJUSTMENT_RC_YAW_EXPO: - tfp_sprintf(buff, "RCYE %3d", currentControlRateProfile->stabilized.rcYawExpo8); + tfp_sprintf(buff, "RCYE %3d", currentControlProfile->stabilized.rcYawExpo8); break; case ADJUSTMENT_MANUAL_RC_EXPO: - tfp_sprintf(buff, "MRCE %3d", currentControlRateProfile->manual.rcExpo8); + tfp_sprintf(buff, "MRCE %3d", currentControlProfile->manual.rcExpo8); break; case ADJUSTMENT_MANUAL_RC_YAW_EXPO: - tfp_sprintf(buff, "MRCYE %3d", currentControlRateProfile->manual.rcYawExpo8); + tfp_sprintf(buff, "MRCYE %3d", currentControlProfile->manual.rcYawExpo8); break; case ADJUSTMENT_THROTTLE_EXPO: - tfp_sprintf(buff, "TE %3d", currentControlRateProfile->throttle.rcExpo8); + tfp_sprintf(buff, "TE %3d", currentControlProfile->throttle.rcExpo8); break; case ADJUSTMENT_PITCH_ROLL_RATE: - tfp_sprintf(buff, "PRR %3d %3d", currentControlRateProfile->stabilized.rates[FD_PITCH], currentControlRateProfile->stabilized.rates[FD_ROLL]); + tfp_sprintf(buff, "PRR %3d %3d", currentControlProfile->stabilized.rates[FD_PITCH], currentControlProfile->stabilized.rates[FD_ROLL]); break; case ADJUSTMENT_PITCH_RATE: - tfp_sprintf(buff, "PR %3d", currentControlRateProfile->stabilized.rates[FD_PITCH]); + tfp_sprintf(buff, "PR %3d", currentControlProfile->stabilized.rates[FD_PITCH]); break; case ADJUSTMENT_ROLL_RATE: - tfp_sprintf(buff, "RR %3d", currentControlRateProfile->stabilized.rates[FD_ROLL]); + tfp_sprintf(buff, "RR %3d", currentControlProfile->stabilized.rates[FD_ROLL]); break; case ADJUSTMENT_MANUAL_PITCH_ROLL_RATE: - tfp_sprintf(buff, "MPRR %3d %3d", currentControlRateProfile->manual.rates[FD_PITCH], currentControlRateProfile->manual.rates[FD_ROLL]); + tfp_sprintf(buff, "MPRR %3d %3d", currentControlProfile->manual.rates[FD_PITCH], currentControlProfile->manual.rates[FD_ROLL]); break; case ADJUSTMENT_MANUAL_PITCH_RATE: - tfp_sprintf(buff, "MPR %3d", currentControlRateProfile->manual.rates[FD_PITCH]); + tfp_sprintf(buff, "MPR %3d", currentControlProfile->manual.rates[FD_PITCH]); break; case ADJUSTMENT_MANUAL_ROLL_RATE: - tfp_sprintf(buff, "MRR %3d", currentControlRateProfile->manual.rates[FD_ROLL]); + tfp_sprintf(buff, "MRR %3d", currentControlProfile->manual.rates[FD_ROLL]); break; case ADJUSTMENT_YAW_RATE: - tfp_sprintf(buff, "YR %3d", currentControlRateProfile->stabilized.rates[FD_YAW]); + tfp_sprintf(buff, "YR %3d", currentControlProfile->stabilized.rates[FD_YAW]); break; case ADJUSTMENT_MANUAL_YAW_RATE: - tfp_sprintf(buff, "MYR %3d", currentControlRateProfile->manual.rates[FD_YAW]); + tfp_sprintf(buff, "MYR %3d", currentControlProfile->manual.rates[FD_YAW]); break; case ADJUSTMENT_PITCH_ROLL_P: tfp_sprintf(buff, "PRP %3d %3d", pidBankMutable()->pid[PID_PITCH].P, pidBankMutable()->pid[PID_ROLL].P); @@ -958,10 +958,10 @@ static void osdDJIAdjustmentMessage(char *buff, uint8_t adjustmentFunction) tfp_sprintf(buff, "MTDPA %4d", navConfigMutable()->fw.minThrottleDownPitchAngle); break; case ADJUSTMENT_TPA: - tfp_sprintf(buff, "TPA %3d", currentControlRateProfile->throttle.dynPID); + tfp_sprintf(buff, "TPA %3d", currentControlProfile->throttle.dynPID); break; case ADJUSTMENT_TPA_BREAKPOINT: - tfp_sprintf(buff, "TPABP %4d", currentControlRateProfile->throttle.pa_breakpoint); + tfp_sprintf(buff, "TPABP %4d", currentControlProfile->throttle.pa_breakpoint); break; case ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS: tfp_sprintf(buff, "CSM %3d", navConfigMutable()->fw.control_smoothness); @@ -1224,7 +1224,7 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100)); if (cmd->cmd == MSP_STATUS_EX) { sbufWriteU8(dst, 3); // PID_PROFILE_COUNT - sbufWriteU8(dst, 1); // getCurrentControlRateProfileIndex() + sbufWriteU8(dst, 1); // getCurrentControlProfileIndex() } else { sbufWriteU16(dst, cycleTime); // gyro cycle time } @@ -1421,23 +1421,23 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms case DJI_MSP_RC_TUNING: sbufWriteU8(dst, 100); // INAV doesn't use rcRate - sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8); + sbufWriteU8(dst, currentControlProfile->stabilized.rcExpo8); for (int i = 0 ; i < 3; i++) { // R,P,Y rates see flight_dynamics_index_t - sbufWriteU8(dst, currentControlRateProfile->stabilized.rates[i]); + sbufWriteU8(dst, currentControlProfile->stabilized.rates[i]); } - sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID); - sbufWriteU8(dst, currentControlRateProfile->throttle.rcMid8); - sbufWriteU8(dst, currentControlRateProfile->throttle.rcExpo8); - sbufWriteU16(dst, currentControlRateProfile->throttle.pa_breakpoint); - sbufWriteU8(dst, currentControlRateProfile->stabilized.rcYawExpo8); + sbufWriteU8(dst, currentControlProfile->throttle.dynPID); + sbufWriteU8(dst, currentControlProfile->throttle.rcMid8); + sbufWriteU8(dst, currentControlProfile->throttle.rcExpo8); + sbufWriteU16(dst, currentControlProfile->throttle.pa_breakpoint); + sbufWriteU8(dst, currentControlProfile->stabilized.rcYawExpo8); sbufWriteU8(dst, 100); // INAV doesn't use rcRate sbufWriteU8(dst, 100); // INAV doesn't use rcRate - sbufWriteU8(dst, currentControlRateProfile->stabilized.rcExpo8); + sbufWriteU8(dst, currentControlProfile->stabilized.rcExpo8); // added in 1.41 sbufWriteU8(dst, 0); - sbufWriteU8(dst, currentControlRateProfile->throttle.dynPID); + sbufWriteU8(dst, currentControlProfile->throttle.dynPID); break; case DJI_MSP_SET_PID: diff --git a/src/main/io/osd_grid.c b/src/main/io/osd_grid.c index 9e79194498d..fae92d195b7 100644 --- a/src/main/io/osd_grid.c +++ b/src/main/io/osd_grid.c @@ -56,6 +56,38 @@ typedef struct osd_sidebar_s { uint8_t idle; } osd_sidebar_t; +void osdGridDrawThrottleGauge(displayPort_t *display, unsigned gx, unsigned gy, uint8_t thrPos) +{ + uint16_t thrChars[] = {SYM_THR_GAUGE_EMPTY, SYM_THR_GAUGE_EMPTY, SYM_THR_GAUGE_EMPTY, SYM_THR_GAUGE_EMPTY, SYM_THR_GAUGE_EMPTY}; + + if (thrPos >= 100) + thrChars[0] = SYM_THR_GAUGE_FULL; + else if (thrPos >= 90) + thrChars[0] = SYM_THR_GAUGE_HALF; + if (thrPos >= 80) + thrChars[1] = SYM_THR_GAUGE_FULL; + else if (thrPos >= 70) + thrChars[1] = SYM_THR_GAUGE_HALF; + if (thrPos >= 60) + thrChars[2] = SYM_THR_GAUGE_FULL; + else if (thrPos >= 50) + thrChars[2] = SYM_THR_GAUGE_HALF; + if (thrPos >= 40) + thrChars[3] = SYM_THR_GAUGE_FULL; + else if (thrPos >= 30) + thrChars[3] = SYM_THR_GAUGE_HALF; + if (thrPos >= 20) + thrChars[4] = SYM_THR_GAUGE_FULL; + else if (thrPos >= 10) + thrChars[4] = SYM_THR_GAUGE_HALF; + + displayWriteChar(display, gx, gy, thrChars[0]); + displayWriteChar(display, gx, gy + 1, thrChars[1]); + displayWriteChar(display, gx, gy + 2, thrChars[2]); + displayWriteChar(display, gx, gy + 3, thrChars[3]); + displayWriteChar(display, gx, gy + 4, thrChars[4]); +} + void osdGridDrawVario(displayPort_t *display, unsigned gx, unsigned gy, float zvel) { int v = zvel / OSD_VARIO_CM_S_PER_ARROW; diff --git a/src/main/io/osd_grid.h b/src/main/io/osd_grid.h index baf69dc9186..c4f0eaa9ee2 100644 --- a/src/main/io/osd_grid.h +++ b/src/main/io/osd_grid.h @@ -28,6 +28,7 @@ typedef struct displayPort_s displayPort_t; +void osdGridDrawThrottleGauge(displayPort_t *display, unsigned gx, unsigned gy, uint8_t thrPos); void osdGridDrawVario(displayPort_t *display, unsigned gx, unsigned gy, float zvel); void osdGridDrawDirArrow(displayPort_t *display, unsigned gx, unsigned gy, float degrees); void osdGridDrawArtificialHorizon(displayPort_t *display, unsigned gx, unsigned gy, float pitchAngle, float rollAngle); diff --git a/src/main/io/osd_hud.c b/src/main/io/osd_hud.c index e95ca5fe5f1..575a7a79d15 100644 --- a/src/main/io/osd_hud.c +++ b/src/main/io/osd_hud.c @@ -137,7 +137,7 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu osdCrosshairPosition(¢er_x, ¢er_y); - if (!(osdConfig()->pan_servo_pwm2centideg == 0)){ + if (!(osdConfig()->osd_pan_servo_range_decadegrees == 0)){ poiDirection = poiDirection + osdGetPanServoOffset(); } diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 59b7191b691..d6b82da02b8 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -106,7 +106,7 @@ const uint32_t baudRates[] = { 0, 1200, 2400, 4800, 9600, 19200, 38400, 57600, 1 #define BAUD_RATE_COUNT ARRAYLEN(baudRates) -PG_REGISTER_WITH_RESET_FN(serialConfig_t, serialConfig, PG_SERIAL_CONFIG, 1); +PG_REGISTER_WITH_RESET_FN(serialConfig_t, serialConfig, PG_SERIAL_CONFIG, 2); void pgResetFn_serialConfig(serialConfig_t *serialConfig) { @@ -151,8 +151,6 @@ void pgResetFn_serialConfig(serialConfig_t *serialConfig) } } #endif - - serialConfig->reboot_character = SETTING_REBOOT_CHARACTER_DEFAULT; } baudRate_e lookupBaudRateIndex(uint32_t baudRate) diff --git a/src/main/io/serial.h b/src/main/io/serial.h index aa9d709472e..a746dc77177 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -133,7 +133,6 @@ typedef struct serialPortConfig_s { typedef struct serialConfig_s { serialPortConfig_t portConfigs[SERIAL_PORT_COUNT]; - uint8_t reboot_character; // which byte is used to reboot. Default 'R', could be changed carefully to something else. } serialConfig_t; PG_DECLARE(serialConfig_t, serialConfig); diff --git a/src/main/io/smartport_master.c b/src/main/io/smartport_master.c index 80b71327cb1..e1114164053 100644 --- a/src/main/io/smartport_master.c +++ b/src/main/io/smartport_master.c @@ -98,7 +98,7 @@ enum #define SMARTPORT_BYTESTUFFING_MARKER 0x7D #define SMARTPORT_BYTESTUFFING_XOR_VALUE 0x20 -#define SMARTPORT_SENSOR_DATA_TIMEOUT 500 // ms +#define SMARTPORT_SENSOR_DATA_TIMEOUT (500*1000) // us #define SMARTPORT_FORWARD_REQUESTS_MAX 10 @@ -128,6 +128,10 @@ typedef struct { timeUs_t altitudeTimestamp; int16_t vario; timeUs_t varioTimestamp; + int16_t current; + timeUs_t currentTimestamp; + int16_t voltage; + timeUs_t voltageTimestamp; } smartportSensorsData_t; typedef struct { @@ -378,6 +382,17 @@ static void decodeVarioData(uint32_t sdata) sensorsData.vario = sdata * 2; // mm/s } +static void decodeCurrentData(uint32_t sdata) +{ + // data comes in 100mA steps + sensorsData.current = ((int16_t)sdata) * 10; +} + +static void decodeVoltageData(uint32_t sdata) +{ + sensorsData.voltage = (int16_t)sdata; +} + static void processSensorPayload(smartPortPayload_t *payload, timeUs_t currentTimeUs) { switch (payload->valueId) { @@ -400,6 +415,16 @@ static void processSensorPayload(smartPortPayload_t *payload, timeUs_t currentTi decodeVarioData(payload->data); sensorsData.varioTimestamp = currentTimeUs; break; + + case DATAID_CURRENT: + decodeCurrentData(payload->data); + sensorsData.currentTimestamp = currentTimeUs; + break; + + case DATAID_VFAS: + decodeVoltageData(payload->data); + sensorsData.voltageTimestamp = currentTimeUs; + break; } sensorPayloadCache[currentPolledPhyID] = *payload; } @@ -568,6 +593,24 @@ vs600Data_t *smartportMasterGetVS600Data(void) return &sensorsData.vs600; } +int16_t *smartportMasterGetCurrentData(void) +{ + if (micros() - sensorsData.currentTimestamp > SMARTPORT_SENSOR_DATA_TIMEOUT) { + return NULL; + } + + return &sensorsData.current; +} + +int16_t *smartportMasterGetVoltageData(void) +{ + if (micros() - sensorsData.voltageTimestamp > SMARTPORT_SENSOR_DATA_TIMEOUT) { + return NULL; + } + + return &sensorsData.voltage; +} + bool smartportMasterPhyIDIsActive(uint8_t phyID) { return phyIDIsActive(phyID); diff --git a/src/main/io/smartport_master.h b/src/main/io/smartport_master.h index 3ea39e4cd4d..a469a063d7b 100644 --- a/src/main/io/smartport_master.h +++ b/src/main/io/smartport_master.h @@ -43,6 +43,11 @@ typedef struct { int16_t voltage[6]; } cellsData_t; +typedef struct { + int16_t amperage; + int16_t vfas; +} batteryData_t; + typedef enum { VS600_BAND_A, VS600_BAND_B, @@ -83,5 +88,7 @@ bool smartportMasterNextForwardResponse(uint8_t phyID, smartPortPayload_t *paylo // Returns latest Cells data or NULL if the data is too old cellsData_t *smartportMasterGetCellsData(void); vs600Data_t *smartportMasterGetVS600Data(void); +int16_t *smartportMasterGetCurrentData(void); +int16_t *smartportMasterGetVoltageData(void); #endif /* USE_SMARTPORT_MASTER */ diff --git a/src/main/msc/usbd_storage_sd_spi.c b/src/main/msc/usbd_storage_sd_spi.c index 4573d156bd8..26a38e00b6a 100644 --- a/src/main/msc/usbd_storage_sd_spi.c +++ b/src/main/msc/usbd_storage_sd_spi.c @@ -29,6 +29,7 @@ /* Includes ------------------------------------------------------------------*/ #include #include +#include #include "platform.h" #include "common/utils.h" @@ -39,6 +40,7 @@ #include "drivers/light_led.h" #include "drivers/io.h" #include "drivers/bus_spi.h" +#include "drivers/time.h" /* Private typedef -----------------------------------------------------------*/ /* Private define ------------------------------------------------------------*/ @@ -56,6 +58,12 @@ #define STORAGE_BLK_NBR 0x10000 #define STORAGE_BLK_SIZ 0x200 +// H7 SDIO DMA requires 32-byte aligned buffers +// USB MSC bot_data buffer is only 16-byte aligned, so we need an intermediate buffer +#if defined(STM32H7) && defined(USE_SDCARD_SDIO) +__attribute__((aligned(32))) static uint8_t alignedBuffer[512]; +#endif + static int8_t STORAGE_Init (uint8_t lun); #ifdef USE_HAL_DRIVER @@ -143,10 +151,21 @@ static int8_t STORAGE_Init (uint8_t lun) { UNUSED(lun); LED0_OFF; - sdcard_init(); - while (sdcard_poll() == 0); + + // Only initialize if not already initialized (e.g., by blackbox) + if (!sdcard_isInitialized()) { + sdcard_init(); + } + + // Poll with timeout to avoid infinite loop + uint32_t timeout = 0; + while (sdcard_poll() == 0 && timeout < 1000) { + timeout++; + delay(1); + } + LED0_ON; - return 0; + return (timeout < 1000) ? 0 : -1; } /******************************************************************************* @@ -213,9 +232,47 @@ static int8_t STORAGE_Read (uint8_t lun, UNUSED(lun); LED1_ON; for (int i = 0; i < blk_len; i++) { - while (sdcard_readBlock(blk_addr + i, buf + (512 * i), NULL, 0) == 0) + uint32_t timeout; +#if defined(STM32H7) && defined(USE_SDCARD_SDIO) + // H7 SDIO DMA requires 32-byte aligned buffers + // USB MSC buffer may not be aligned, so use intermediate buffer + timeout = 0; + while (sdcard_readBlock(blk_addr + i, alignedBuffer, NULL, 0) == 0) { sdcard_poll(); - while (sdcard_poll() == 0); + if (++timeout > 5000) { + LED1_OFF; + return -1; // Timeout waiting for read to start + } + delay(1); + } + timeout = 0; + while (sdcard_poll() == 0) { + if (++timeout > 5000) { + LED1_OFF; + return -1; // Timeout waiting for read to complete + } + delay(1); + } + memcpy(buf + (512 * i), alignedBuffer, 512); +#else + timeout = 0; + while (sdcard_readBlock(blk_addr + i, buf + (512 * i), NULL, 0) == 0) { + sdcard_poll(); + if (++timeout > 5000) { + LED1_OFF; + return -1; // Timeout waiting for read to start + } + delay(1); + } + timeout = 0; + while (sdcard_poll() == 0) { + if (++timeout > 5000) { + LED1_OFF; + return -1; // Timeout waiting for read to complete + } + delay(1); + } +#endif } LED1_OFF; return 0; @@ -235,10 +292,47 @@ static int8_t STORAGE_Write (uint8_t lun, UNUSED(lun); LED1_ON; for (int i = 0; i < blk_len; i++) { + uint32_t timeout; +#if defined(STM32H7) && defined(USE_SDCARD_SDIO) + // H7 SDIO DMA requires 32-byte aligned buffers + // USB MSC buffer may not be aligned, so use intermediate buffer + memcpy(alignedBuffer, buf + (i * 512), 512); + timeout = 0; + while (sdcard_writeBlock(blk_addr + i, alignedBuffer, NULL, 0) != SDCARD_OPERATION_IN_PROGRESS) { + sdcard_poll(); + if (++timeout > 5000) { + LED1_OFF; + return -1; // Timeout waiting for write to start + } + delay(1); + } + timeout = 0; + while (sdcard_poll() == 0) { + if (++timeout > 5000) { + LED1_OFF; + return -1; // Timeout waiting for write to complete + } + delay(1); + } +#else + timeout = 0; while (sdcard_writeBlock(blk_addr + i, buf + (i * 512), NULL, 0) != SDCARD_OPERATION_IN_PROGRESS) { sdcard_poll(); + if (++timeout > 5000) { + LED1_OFF; + return -1; // Timeout waiting for write to start + } + delay(1); } - while (sdcard_poll() == 0); + timeout = 0; + while (sdcard_poll() == 0) { + if (++timeout > 5000) { + LED1_OFF; + return -1; // Timeout waiting for write to complete + } + delay(1); + } +#endif } LED1_OFF; return 0; diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index ac847afeaec..5a3af115f9c 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -273,7 +273,7 @@ #define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom #define MSP_SET_3D 217 //in message Settings needed for reversible ESCs #define MSP_SET_RC_DEADBAND 218 //in message deadbands for yaw alt pitch roll -#define MSP_SET_RESET_CURR_PID 219 //in message resetting the current pid profile to defaults +#define MSP_SET_RESET_CURR_PID 219 //in message resetting the current PID profile to defaults #define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag #define MSP_SET_LED_STRIP_MODECOLOR 221 //in message Set LED strip mode_color settings diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index dcbdeb5e71f..0b893916895 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -46,6 +46,8 @@ #define MSP2_INAV_OSD_PREFERENCES 0x2016 #define MSP2_INAV_OSD_SET_PREFERENCES 0x2017 +#define MSP2_INAV_OSD_UPDATE_POSITION 0x2118 + #define MSP2_INAV_SELECT_BATTERY_PROFILE 0x2018 #define MSP2_INAV_DEBUG 0x2019 @@ -87,6 +89,7 @@ #define MSP2_INAV_MISC2 0x203A #define MSP2_INAV_LOGIC_CONDITIONS_SINGLE 0x203B +#define MSP2_INAV_LOGIC_CONDITIONS_CONFIGURED 0x203C // Returns 8-byte bitmask of non-default logic conditions #define MSP2_INAV_ESC_RPM 0x2040 #define MSP2_INAV_ESC_TELEM 0x2041 @@ -116,7 +119,10 @@ #define MSP2_INAV_SERVO_CONFIG 0x2200 #define MSP2_INAV_SET_SERVO_CONFIG 0x2201 -#define MSP2_INAV_GEOZONE 0x2210 -#define MSP2_INAV_SET_GEOZONE 0x2211 -#define MSP2_INAV_GEOZONE_VERTEX 0x2212 -#define MSP2_INAV_SET_GEOZONE_VERTEX 0x2213 \ No newline at end of file +#define MSP2_INAV_GEOZONE 0x2210 +#define MSP2_INAV_SET_GEOZONE 0x2211 +#define MSP2_INAV_GEOZONE_VERTEX 0x2212 +#define MSP2_INAV_SET_GEOZONE_VERTEX 0x2213 +#define MSP2_INAV_SET_GVAR 0x2214 + +#define MSP2_INAV_FULL_LOCAL_POSE 0x2220 diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index f72ea35da11..32aac7a7bd5 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -428,11 +428,6 @@ static void mspEvaluateNonMspData(mspPort_t * mspPort, uint8_t receivedChar) mspPort->pendingRequest = MSP_PENDING_CLI; return; } - - if (receivedChar == serialConfig()->reboot_character) { - mspPort->pendingRequest = MSP_PENDING_BOOTLOADER; - return; - } } static void mspProcessPendingRequest(mspPort_t * mspPort) @@ -443,10 +438,6 @@ static void mspProcessPendingRequest(mspPort_t * mspPort) } switch(mspPort->pendingRequest) { - case MSP_PENDING_BOOTLOADER: - systemResetToBootloader(); - break; - case MSP_PENDING_CLI: if (!cliMode) { // When we enter CLI mode - disable this MSP port. Don't care about preserving the port since CLI can only be exited via reboot diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 14f4f20274c..e7f0774925e 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3867,6 +3867,15 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData) } } +int isGCSValid(void) +{ + return (ARMING_FLAG(ARMED) && + (posControl.flags.estPosStatus >= EST_TRUSTED) && + posControl.gpsOrigin.valid && + posControl.flags.isGCSAssistedNavigationEnabled && + (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)); +} + void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) { gpsLocation_t wpLLH; @@ -3885,9 +3894,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData) } // WP #255 - special waypoint - directly set desiredPosition // Only valid when armed and in poshold mode - else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) && - ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus == EST_TRUSTED) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled && - (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) { + else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) && isGCSValid()) { // Convert to local coordinates geoConvertGeodeticToLocal(&wpPos.pos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE); @@ -4956,13 +4963,23 @@ void navigationUsePIDs(void) 0.0f ); - navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 300.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f, - NAV_DTERM_CUT_HZ, - 0.0f - ); + if (pidProfile()->fwAltControlUsePos) { + navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f, + 0.0f, + NAV_DTERM_CUT_HZ, + 0.0f + ); + } else { + navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 300.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f, + NAV_DTERM_CUT_HZ, + 0.0f + ); + } navPidInit(&posControl.pids.fw_heading, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].P / 10.0f, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].I / 10.0f, diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 9132974e480..ddc7e23e76d 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -704,6 +704,7 @@ void getEstimatedPositionAndVelocity(navPositionAndVelocity_t * pos); /* Waypoint list access functions */ int getWaypointCount(void); bool isWaypointListValid(void); +int isGCSValid(void); void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData); void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData); void resetWaypointList(void); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 49c693fa0c2..2051f64232f 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -42,7 +42,7 @@ #include "flight/mixer_profile.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" @@ -131,12 +131,12 @@ bool adjustFixedWingAltitudeFromRCInput(void) // Position to velocity controller for Z axis static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) { - static pt1Filter_t velzFilterState; + static pt1Filter_t pitchFilterState; float desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros); // Reduce max allowed climb rate by 2/3 if performing loiter (stall prevention) - if (needToCalculateCircularLoiter && desiredClimbRate > 0.67f * navConfig()->fw.max_auto_climb_rate) { + if (navGetCurrentStateFlags() & NAV_CTL_HOLD && desiredClimbRate > 0.67f * navConfig()->fw.max_auto_climb_rate) { desiredClimbRate = 0.67f * navConfig()->fw.max_auto_climb_rate; } @@ -144,12 +144,61 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle); const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle); - // PID controller to translate desired climb rate error into pitch angle [decideg] - float currentClimbRate = navGetCurrentActualPositionAndVelocity()->vel.z; - float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, desiredClimbRate, currentClimbRate, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, PID_DTERM_FROM_ERROR); + // Default control based on climb rate (velocity) + float targetValue = desiredClimbRate; + float measuredValue = navGetCurrentActualPositionAndVelocity()->vel.z; + + // Optional control based on altitude (position) + if (pidProfile()->fwAltControlUsePos) { + static float currentDesiredPosZ = 0.0f; + static float trackingAltitude = 0.0f; + static bool altitudeRateControlActive = false; + + float desiredAltitude = posControl.desiredState.pos.z; + float currentAltitude = navGetCurrentActualPositionAndVelocity()->pos.z; + + /* Determine if altitude rate control required based on magnitude of change in target altitude. + * No rate control used during trackback to allow max climb rates based on pitch limits */ + if (fabsf(currentDesiredPosZ - desiredAltitude) > 100.0f || !isPitchAdjustmentValid) { + altitudeRateControlActive = desiredClimbRate && fabsf(desiredAltitude - currentAltitude) > navConfig()->fw.max_auto_climb_rate && + !posControl.flags.rthTrackbackActive; + trackingAltitude = currentAltitude; + } + currentDesiredPosZ = desiredAltitude; + + if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT || altitudeRateControlActive) { + /* Adjustment factor based on vertical acceleration used to better control altitude position change + * driving vertical velocity control. Helps avoid lag induced overcontrol by PID loop. */ + static float absLastClimbRate = 0.0f; + float absClimbRate = fabsf(measuredValue); + float accelerationZ = (absClimbRate - absLastClimbRate) / US2S(deltaMicros); + absLastClimbRate = absClimbRate; + float adjustmentFactor = constrainf(scaleRangef(accelerationZ, 0.0f, 200.0f, 1.0f, 0.0f), 0.0f, 1.0f); + + if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) { + posControl.desiredState.pos.z += adjustmentFactor * desiredClimbRate * US2S(deltaMicros); + desiredAltitude = posControl.desiredState.pos.z; + } else { + /* Disable rate control if no longer required, i.e. remaining altitude change is small */ + altitudeRateControlActive = fabsf(desiredAltitude - currentAltitude) > MAX(fabsf(desiredClimbRate), 50.0f); + + /* Tracking altitude used to control altitude rate where changing posControl.desiredState.pos.z not possible */ + trackingAltitude += adjustmentFactor * desiredClimbRate * US2S(deltaMicros); + desiredAltitude = trackingAltitude; + } + } else { + desiredClimbRate = 0; + } + + targetValue = desiredAltitude; + measuredValue = currentAltitude; + } + + // PID controller to translate desired target error (velocity or position) into pitch angle [decideg] + float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, targetValue, measuredValue, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0); // Apply low-pass filter to prevent rapid correction - targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros)); + targetPitchAngle = pt1FilterApply4(&pitchFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros)); // Reconstrain pitch angle (> 0 - climb, < 0 - dive) targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg); @@ -171,6 +220,7 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs) // Check if last correction was not too long ago if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { updateAltitudeVelocityAndPitchController_FW(deltaMicrosPositionUpdate); + isPitchAdjustmentValid = true; } else { // Position update has not occurred in time (first iteration or glitch), reset altitude controller @@ -180,8 +230,6 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs) // Indicate that information is no longer usable posControl.flags.verticalPositionDataConsumed = true; } - - isPitchAdjustmentValid = true; } else { // No valid altitude sensor data, don't adjust pitch automatically, rcCommand[PITCH] is passed through to PID controller @@ -558,10 +606,10 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) previousTimePositionUpdate = currentTimeUs; if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { - float velThrottleBoost = ((navConfig()->general.min_ground_speed * 100.0f) - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate); + float velThrottleBoost = ((getMinGroundSpeed(navConfig()->general.min_ground_speed) * 100.0f) - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate); // If we are in the deadband of 50cm/s - don't update speed boost - if (fabsf(posControl.actualState.velXY - (navConfig()->general.min_ground_speed * 100.0f)) > 50) { + if (fabsf(posControl.actualState.velXY - (getMinGroundSpeed(navConfig()->general.min_ground_speed) * 100.0f)) > 50) { throttleSpeedAdjustment += velThrottleBoost; } @@ -809,7 +857,7 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs) rcCommand[YAW] = 0; } else { rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]); - rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]); + rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlProfile->stabilized.rates[FD_YAW]); } } diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 811186d0bda..bb8f15dc8ba 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -834,14 +834,12 @@ bool isMulticopterLandingDetected(void) } #endif - bool throttleIsBelowMidHover = rcCommand[THROTTLE] < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue())); - /* Basic condition to start looking for landing * Detection active during Failsafe only if throttle below mid hover throttle * and WP mission not active (except landing states). * Also active in non autonomous flight modes but only when thottle low */ bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG)) - || (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && throttleIsBelowMidHover) + || (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && !isMulticopterThrottleAboveMidHover()) || (!navigationIsFlyingAutonomousMode() && throttleStickIsLow()); static timeMs_t landingDetectorStartedAt; @@ -923,6 +921,11 @@ bool isMulticopterLandingDetected(void) } } +bool isMulticopterThrottleAboveMidHover(void) +{ + return rcCommand[THROTTLE] > 0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()); +} + /*----------------------------------------------------------- * Multicopter emergency landing *-----------------------------------------------------------*/ diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c old mode 100755 new mode 100644 index e7f166e89ca..cd861dacd6c --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -256,11 +256,13 @@ void onNewGPSData(void) posEstimator.gps.eph = INAV_GPS_DEFAULT_EPH; posEstimator.gps.epv = INAV_GPS_DEFAULT_EPV; } - - /* Indicate a last valid reading of Pos/Vel */ - posEstimator.gps.lastUpdateTime = currentTimeUs; } + /* Indicate a last valid reading of Pos/Vel - must be updated even on + * first GPS reading after recovery to prevent position estimate from + * timing out and getting stuck at zero (fixes issue #11049) */ + posEstimator.gps.lastUpdateTime = currentTimeUs; + previousLat = gpsSol.llh.lat; previousLon = gpsSol.llh.lon; previousAlt = gpsSol.llh.alt; @@ -368,14 +370,6 @@ static void updateIMUEstimationWeight(const float dt) DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000); } -float navGetAccelerometerWeight(void) -{ - const float accWeightScaled = posEstimator.imu.accWeightFactor; - DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000); - - return accWeightScaled; -} - static void updateIMUTopic(timeUs_t currentTimeUs) { const float dt = US2S(currentTimeUs - posEstimator.imu.lastUpdateTime); @@ -392,25 +386,24 @@ static void updateIMUTopic(timeUs_t currentTimeUs) /* Update acceleration weight based on vibration levels and clipping */ updateIMUEstimationWeight(dt); - fpVector3_t accelBF; + fpVector3_t accelReading; /* Read acceleration data in body frame */ - accelBF.x = imuMeasuredAccelBF.x; - accelBF.y = imuMeasuredAccelBF.y; - accelBF.z = imuMeasuredAccelBF.z; + accelReading.x = imuMeasuredAccelBF.x; + accelReading.y = imuMeasuredAccelBF.y; + accelReading.z = imuMeasuredAccelBF.z; - /* Correct accelerometer bias */ - accelBF.x -= posEstimator.imu.accelBias.x; - accelBF.y -= posEstimator.imu.accelBias.y; - accelBF.z -= posEstimator.imu.accelBias.z; + /* Adjust reading from Body to Earth frame - from Forward-Right-Down to North-East-Up*/ + imuTransformVectorBodyToEarth(&accelReading); - /* Rotate vector to Earth frame - from Forward-Right-Down to North-East-Up*/ - imuTransformVectorBodyToEarth(&accelBF); + /* Apply reading to NEU frame including correction for accelerometer bias */ + posEstimator.imu.accelNEU.x = accelReading.x + posEstimator.imu.accelBias.x; + posEstimator.imu.accelNEU.y = accelReading.y + posEstimator.imu.accelBias.y; + posEstimator.imu.accelNEU.z = accelReading.z + posEstimator.imu.accelBias.z; - /* Read acceleration data in NEU frame from IMU */ - posEstimator.imu.accelNEU.x = accelBF.x; - posEstimator.imu.accelNEU.y = accelBF.y; - posEstimator.imu.accelNEU.z = accelBF.z; + DEBUG_SET(DEBUG_VIBE, 5, posEstimator.imu.accelBias.x); + DEBUG_SET(DEBUG_VIBE, 6, posEstimator.imu.accelBias.y); + DEBUG_SET(DEBUG_VIBE, 7, posEstimator.imu.accelBias.z); /* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */ if (gyroConfig()->init_gyro_cal_enabled) { @@ -583,48 +576,54 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) } if (ctx->newFlags & EST_BARO_VALID && wBaro) { - timeUs_t currentTimeUs = micros(); + bool isAirCushionEffectDetected = false; + static float baroGroundAlt = 0.0f; - if (!ARMING_FLAG(ARMED)) { - posEstimator.state.baroGroundAlt = posEstimator.est.pos.z; - posEstimator.state.isBaroGroundValid = true; - posEstimator.state.baroGroundTimeout = currentTimeUs + 250000; // 0.25 sec - } - else { - if (posEstimator.est.vel.z > 15) { - posEstimator.state.isBaroGroundValid = currentTimeUs > posEstimator.state.baroGroundTimeout ? false: true; + if (STATE(MULTIROTOR)) { + static bool isBaroGroundValid = false; + + if (!ARMING_FLAG(ARMED)) { + baroGroundAlt = posEstimator.baro.alt; + isBaroGroundValid = true; } - else { - posEstimator.state.baroGroundTimeout = currentTimeUs + 250000; // 0.25 sec + else if (isBaroGroundValid) { + // We might be experiencing air cushion effect during takeoff - use sonar or baro ground altitude to detect it + if (isMulticopterThrottleAboveMidHover()) { + // Disable ground effect detection at lift off when est alt and baro alt converge. Always disable if baro alt > 1m. + isBaroGroundValid = fabsf(posEstimator.est.pos.z - posEstimator.baro.alt) > 20.0f && posEstimator.baro.alt < 100.0f; + } + + isAirCushionEffectDetected = (isEstimatedAglTrusted() && posEstimator.surface.alt < 20.0f) || posEstimator.baro.alt < baroGroundAlt + 20.0f; } } - // We might be experiencing air cushion effect during takeoff - use sonar or baro ground altitude to detect it - bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) && - (((ctx->newFlags & EST_SURFACE_VALID) && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) || - ((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt)); - // Altitude - const float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z); + float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z); + + // Disable alt pos correction at point of lift off if ground effect active + if (isAirCushionEffectDetected && isMulticopterThrottleAboveMidHover()) { + baroAltResidual = 0.0f; + } + const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z); const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; + const float w_z_baro_v = positionEstimationConfig()->w_z_baro_v; ctx->estPosCorr.z += baroAltResidual * w_z_baro_p * ctx->dt; - ctx->estVelCorr.z += baroAltResidual * sq(w_z_baro_p) * ctx->dt; - ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_v * ctx->dt; + ctx->estVelCorr.z += baroVelZResidual * w_z_baro_v * ctx->dt; - ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, w_z_baro_p); + ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.baro.epv, fabsf(baroAltResidual)), w_z_baro_p); // Accelerometer bias if (!isAirCushionEffectDetected) { - ctx->accBiasCorr.z -= baroAltResidual * sq(w_z_baro_p); + ctx->accBiasCorr.z += (baroAltResidual * sq(w_z_baro_p) + baroVelZResidual * sq(w_z_baro_v)); } correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed } - if (ctx->newFlags & EST_GPS_Z_VALID && wGps) { - // Reset current estimate to GPS altitude if estimate not valid + if (ctx->newFlags & EST_GPS_Z_VALID && (wGps || !(ctx->newFlags & EST_Z_VALID))) { + // Reset current estimate to GPS altitude if estimate not valid (used for GPS and Baro) if (!(ctx->newFlags & EST_Z_VALID)) { ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z; ctx->estVelCorr.z += posEstimator.gps.vel.z - posEstimator.est.vel.z; @@ -635,19 +634,24 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z); const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z); const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p; + const float w_z_gps_v = positionEstimationConfig()->w_z_gps_v; ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * ctx->dt; - ctx->estVelCorr.z += gpsAltResidual * sq(w_z_gps_p) * ctx->dt; - ctx->estVelCorr.z += gpsVelZResidual * positionEstimationConfig()->w_z_gps_v * ctx->dt; + ctx->estVelCorr.z += gpsVelZResidual * w_z_gps_v * ctx->dt; ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p); // Accelerometer bias - ctx->accBiasCorr.z -= gpsAltResidual * sq(w_z_gps_p); + ctx->accBiasCorr.z += (gpsAltResidual * sq(w_z_gps_p) + gpsVelZResidual * sq(w_z_gps_v)); } correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed } + // Factor corrections for sensor weightings to ensure magnitude consistency + ctx->estPosCorr.z *= 2.0f / (wGps + wBaro); + ctx->estVelCorr.z *= 2.0f / (wGps + wBaro); + ctx->accBiasCorr.z *= 2.0f / (wGps + wBaro); + return correctOK; } @@ -679,17 +683,13 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx) ctx->estPosCorr.x += gpsPosXResidual * w_xy_gps_p * ctx->dt; ctx->estPosCorr.y += gpsPosYResidual * w_xy_gps_p * ctx->dt; - // Velocity from coordinates - ctx->estVelCorr.x += gpsPosXResidual * sq(w_xy_gps_p) * ctx->dt; - ctx->estVelCorr.y += gpsPosYResidual * sq(w_xy_gps_p) * ctx->dt; - // Velocity from direct measurement ctx->estVelCorr.x += gpsVelXResidual * w_xy_gps_v * ctx->dt; ctx->estVelCorr.y += gpsVelYResidual * w_xy_gps_v * ctx->dt; // Accelerometer bias - ctx->accBiasCorr.x -= gpsPosXResidual * sq(w_xy_gps_p); - ctx->accBiasCorr.y -= gpsPosYResidual * sq(w_xy_gps_p); + ctx->accBiasCorr.x += (gpsPosXResidual * sq(w_xy_gps_p) + gpsVelXResidual * sq(w_xy_gps_v)); + ctx->accBiasCorr.y += (gpsPosYResidual * sq(w_xy_gps_p) + gpsVelYResidual * sq(w_xy_gps_v)); /* Adjust EPH */ ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p); @@ -736,7 +736,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) return; } - /* Calculate new EPH and EPV for the case we didn't update postion */ + /* Calculate new EPH and EPV for the case we didn't update position */ ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f); ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f); ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs); @@ -770,9 +770,9 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt; } // Boost the corrections based on accWeight - const float accWeight = navGetAccelerometerWeight(); - vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f/accWeight); - vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f/accWeight); + vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f / posEstimator.imu.accWeightFactor); + vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f / posEstimator.imu.accWeightFactor); + // Apply corrections vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr); vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr); @@ -780,16 +780,14 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs) /* Correct accelerometer bias */ const float w_acc_bias = positionEstimationConfig()->w_acc_bias; if (w_acc_bias > 0.0f) { - const float accelBiasCorrMagnitudeSq = sq(ctx.accBiasCorr.x) + sq(ctx.accBiasCorr.y) + sq(ctx.accBiasCorr.z); - if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) { - /* transform error vector from NEU frame to body frame */ - imuTransformVectorEarthToBody(&ctx.accBiasCorr); - - /* Correct accel bias */ - posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias * ctx.dt; - posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias * ctx.dt; - posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias * ctx.dt; - } + /* Correct accel bias */ + posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias * ctx.dt; + posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias * ctx.dt; + posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias * ctx.dt; + + posEstimator.imu.accelBias.x = constrainf(posEstimator.imu.accelBias.x, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); + posEstimator.imu.accelBias.y = constrainf(posEstimator.imu.accelBias.y, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); + posEstimator.imu.accelBias.z = constrainf(posEstimator.imu.accelBias.z, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE); } /* Update ground course */ diff --git a/src/main/navigation/navigation_pos_estimator_agl.c b/src/main/navigation/navigation_pos_estimator_agl.c index 8180b16d65f..03b5bd8ccf0 100644 --- a/src/main/navigation/navigation_pos_estimator_agl.c +++ b/src/main/navigation/navigation_pos_estimator_agl.c @@ -147,10 +147,9 @@ void estimationCalculateAGL(estimationContext_t * ctx) } // Update estimate - const float accWeight = navGetAccelerometerWeight(); posEstimator.est.aglAlt += posEstimator.est.aglVel * ctx->dt; - posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight; - posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight); + posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * posEstimator.imu.accWeightFactor; + posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * sq(posEstimator.imu.accWeightFactor); // Apply correction if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) { diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 5cbdc81c030..abb389bddcb 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -153,13 +153,6 @@ typedef enum { ALTITUDE_SOURCE_BARO_ONLY, } navDefaultAltitudeSensor_e; -typedef struct { - timeUs_t baroGroundTimeout; - float baroGroundAlt; - bool isBaroGroundValid; -} navPositionEstimatorSTATE_t; - - typedef struct { uint32_t flags; @@ -175,9 +168,6 @@ typedef struct { // Estimate navPositionEstimatorESTIMATE_t est; - - // Extra state variables - navPositionEstimatorSTATE_t state; } navigationPosEstimator_t; typedef struct { @@ -195,5 +185,4 @@ extern navigationPosEstimator_t posEstimator; extern float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w); extern void estimationCalculateAGL(estimationContext_t * ctx); extern bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx); -extern float navGetAccelerometerWeight(void); diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 15fe5e5a99d..f6cf9329e98 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -582,6 +582,8 @@ bool isMulticopterLandingDetected(void); void calculateMulticopterInitialHoldPosition(fpVector3_t * pos); float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros); +bool isMulticopterThrottleAboveMidHover(void); + /* Fixed-wing specific functions */ void setupFixedWingAltitudeController(void); diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index e95acf2c4b5..edba68b8e94 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -47,6 +47,7 @@ #include "flight/imu.h" #include "flight/pid.h" #include "flight/mixer_profile.h" +#include "flight/wind_estimator.h" #include "drivers/io_port_expander.h" #include "drivers/gimbal_common.h" #include "io/osd_common.h" @@ -464,6 +465,12 @@ static int logicConditionCompute( return true; break; + case LOGIC_CONDITION_OVERRIDE_MIN_GROUND_SPEED: + logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_MIN_GROUND_SPEED] = constrain(operandA, navConfig()->general.min_ground_speed, 150); + LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_MIN_GROUND_SPEED); + return true; + break; + case LOGIC_CONDITION_FLIGHT_AXIS_ANGLE_OVERRIDE: if (operandA >= 0 && operandA <= 2) { @@ -722,6 +729,10 @@ static int logicConditionGetFlightOperandValue(int operand) { return gpsSol.groundSpeed; break; + case LOGIC_CONDITION_OPERAND_FLIGHT_MIN_GROUND_SPEED: // m/s + return getMinGroundSpeed(navConfig()->general.min_ground_speed); + break; + //FIXME align with osdGet3DSpeed case LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED: // cm/s return osdGet3DSpeed(); @@ -735,6 +746,64 @@ static int logicConditionGetFlightOperandValue(int operand) { #endif break; + case LOGIC_CONDITION_OPERAND_FLIGHT_HORIZONTAL_WIND_SPEED: // cm/s +#ifdef USE_WIND_ESTIMATOR + { + if (isEstimatedWindSpeedValid()) { + uint16_t angle; + return getEstimatedHorizontalWindSpeed(&angle); + } else + return -1; + } +#else + return -1; +#endif + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_WIND_DIRECTION: // deg 0 - 360; -1 if not valid +#ifdef USE_WIND_ESTIMATOR + { + if (isEstimatedWindSpeedValid()) { + uint16_t windAngle; + getEstimatedHorizontalWindSpeed(&windAngle); + int32_t windHeading = (int32_t)windAngle + 18000; // Correct heading to display correctly. + + while (windHeading < 0) windHeading += 36000; + while (windHeading >= 36000) windHeading -= 36000; + + return (int32_t)CENTIDEGREES_TO_DEGREES(windHeading); + } else + return -1; + } +#else + return -1; +#endif + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_RELATIVE_WIND_OFFSET: // deg -180 to 180; 0 if not valid +#ifdef USE_WIND_ESTIMATOR + { + if (isEstimatedWindSpeedValid()) { + uint16_t windAngle; + getEstimatedHorizontalWindSpeed(&windAngle); + int32_t relativeWindHeading = (int32_t)windAngle + 18000 - DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw); + + while (relativeWindHeading < 0) relativeWindHeading += 36000; + while (relativeWindHeading >= 36000) relativeWindHeading -= 36000; + + relativeWindHeading = -relativeWindHeading; + if (relativeWindHeading <= -18000) + relativeWindHeading = 18000 + (relativeWindHeading + 18000); + + return (int32_t)CENTIDEGREES_TO_DEGREES(relativeWindHeading); + } else + return 0; + } +#else + return 0; +#endif + break; + case LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE: // cm return constrain(getEstimatedActualPosition(Z), INT32_MIN, INT32_MAX); break; @@ -1118,6 +1187,18 @@ uint32_t getLoiterRadius(uint32_t loiterRadius) { #endif } +uint32_t getMinGroundSpeed(uint32_t minGroundSpeed) { +#ifdef USE_PROGRAMMING_FRAMEWORK + if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_MIN_GROUND_SPEED)) { + return constrain(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_MIN_GROUND_SPEED], minGroundSpeed, 150); + } else { + return minGroundSpeed; + } +#else + return minGroundSpeed; +#endif +} + float getFlightAxisAngleOverride(uint8_t axis, float angle) { if (flightAxisOverride[axis].angleTargetActive) { return flightAxisOverride[axis].angleTarget; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index ad35ae9ec6a..74ea96c98ee 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -85,7 +85,8 @@ typedef enum { LOGIC_CONDITION_DISABLE_GPS_FIX = 53, LOGIC_CONDITION_RESET_MAG_CALIBRATION = 54, LOGIC_CONDITION_SET_GIMBAL_SENSITIVITY = 55, - LOGIC_CONDITION_LAST = 56, + LOGIC_CONDITION_OVERRIDE_MIN_GROUND_SPEED = 56, + LOGIC_CONDITION_LAST } logicOperation_e; typedef enum logicOperandType_s { @@ -147,6 +148,10 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_FLOWN_LOITER_RADIUS, // 43 LOGIC_CONDITION_OPERAND_FLIGHT_LQ_DOWNLINK, // 44 LOGIC_CONDITION_OPERAND_FLIGHT_UPLINK_RSSI_DBM, // 45 + LOGIC_CONDITION_OPERAND_FLIGHT_MIN_GROUND_SPEED, // m/s // 46 + LOGIC_CONDITION_OPERAND_FLIGHT_HORIZONTAL_WIND_SPEED, // cm/s // 47 + LOGIC_CONDITION_OPERAND_FLIGHT_WIND_DIRECTION, // deg // 48 + LOGIC_CONDITION_OPERAND_FLIGHT_RELATIVE_WIND_OFFSET, // deg // 49 } logicFlightOperands_e; typedef enum { @@ -201,6 +206,7 @@ typedef enum { #ifdef USE_GPS_FIX_ESTIMATION LOGIC_CONDITION_GLOBAL_FLAG_DISABLE_GPS_FIX = (1 << 11), #endif + LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_MIN_GROUND_SPEED = (1 << 12), } logicConditionsGlobalFlags_t; typedef enum { @@ -262,6 +268,7 @@ float getThrottleScale(float globalThrottleScale); int16_t getRcCommandOverride(int16_t command[], uint8_t axis); int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue); uint32_t getLoiterRadius(uint32_t loiterRadius); +uint32_t getMinGroundSpeed(uint32_t minGroundSpeed); float getFlightAxisAngleOverride(uint8_t axis, float angle); float getFlightAxisRateOverride(uint8_t axis, float rate); bool isFlightAxisAngleOverrideActive(uint8_t axis); diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index a791b2e9dd8..267f8176c69 100755 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -41,7 +41,7 @@ #include "rx/crsf.h" #include "telemetry/crsf.h" -#define CRSF_TIME_NEEDED_PER_FRAME_US 1100 // 700 ms + 400 ms for potential ad-hoc request +#define CRSF_TIME_NEEDED_PER_FRAME_US 1750 // 700 ms + 400 ms for potential ad-hoc request #define CRSF_TIME_BETWEEN_FRAMES_US 6667 // At fastest, frames are sent by the transmitter every 6.667 milliseconds, 150 Hz #define CRSF_DIGITAL_CHANNEL_MIN 172 @@ -55,7 +55,7 @@ STATIC_UNIT_TESTED crsfFrame_t crsfFrame; STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL]; static serialPort_t *serialPort; -static timeUs_t crsfFrameStartAt = 0; +static timeUs_t crsfFrameStartAtUs = 0; static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX]; static uint8_t telemetryBufLen = 0; @@ -141,25 +141,30 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *rxCallbackData) UNUSED(rxCallbackData); static uint8_t crsfFramePosition = 0; - const timeUs_t now = micros(); + const timeUs_t currentTimeUs = microsISR(); #ifdef DEBUG_CRSF_PACKETS debug[2] = now - crsfFrameStartAt; #endif - if (now > crsfFrameStartAt + CRSF_TIME_NEEDED_PER_FRAME_US) { + if (cmpTimeUs(currentTimeUs, crsfFrameStartAtUs) > CRSF_TIME_NEEDED_PER_FRAME_US) { // We've received a character after max time needed to complete a frame, // so this must be the start of a new frame. crsfFramePosition = 0; } if (crsfFramePosition == 0) { - crsfFrameStartAt = now; + crsfFrameStartAtUs = currentTimeUs; } // assume frame is 5 bytes long until we have received the frame length // full frame length includes the length of the address and framelength fields const int fullFrameLength = crsfFramePosition < 3 ? 5 : crsfFrame.frame.frameLength + CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_FRAMELENGTH; + if (fullFrameLength > CRSF_FRAME_SIZE_MAX) { + crsfFramePosition = 0; + return; + } + if (crsfFramePosition < fullFrameLength) { crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c; crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true; @@ -174,8 +179,8 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *rxCallbackData) case CRSF_FRAMETYPE_MSP_REQ: case CRSF_FRAMETYPE_MSP_WRITE: { uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE; - if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) { - crsfScheduleMspResponse(); + if (bufferCrsfMspFrame(frameStart, crsfFrame.frame.frameLength - 4)) { + crsfScheduleMspResponse(crsfFrame.frame.payload[1]); } break; } @@ -289,7 +294,7 @@ void crsfRxSendTelemetryData(void) // check that we are not in bi dir mode or that we are not currently receiving data (ie in the middle of an RX frame) // and that there is time to send the telemetry frame before the next RX frame arrives if (CRSF_PORT_OPTIONS & SERIAL_BIDIR) { - const timeDelta_t timeSinceStartOfFrame = cmpTimeUs(micros(), crsfFrameStartAt); + const timeDelta_t timeSinceStartOfFrame = cmpTimeUs(micros(), crsfFrameStartAtUs); if ((timeSinceStartOfFrame < CRSF_TIME_NEEDED_PER_FRAME_US) || (timeSinceStartOfFrame > CRSF_TIME_BETWEEN_FRAMES_US - CRSF_TIME_NEEDED_PER_FRAME_US)) { return; @@ -300,6 +305,11 @@ void crsfRxSendTelemetryData(void) } } +bool crsfRxIsTelemetryBufEmpty(void) +{ + return telemetryBufLen == 0; +} + bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) { diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 69777a0390c..8d84d52b8e5 100755 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -45,6 +45,7 @@ enum { CRSF_FRAME_GPS_PAYLOAD_SIZE = 15, CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8, + CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE = 2, CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10, CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes. CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6, @@ -86,6 +87,7 @@ typedef enum { CRSF_FRAMETYPE_GPS = 0x02, CRSF_FRAMETYPE_VARIO_SENSOR = 0x07, CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, + CRSF_FRAMETYPE_BAROMETER_ALTITUDE = 0x09, CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, CRSF_FRAMETYPE_ATTITUDE = 0x1E, @@ -128,6 +130,7 @@ typedef union crsfFrame_u { void crsfRxWriteTelemetryData(const void *data, int len); void crsfRxSendTelemetryData(void); +bool crsfRxIsTelemetryBufEmpty(void); struct rxConfig_s; struct rxRuntimeConfig_s; diff --git a/src/main/rx/ibus.c b/src/main/rx/ibus.c index 30e1958c4bc..4dc9b09a996 100755 --- a/src/main/rx/ibus.c +++ b/src/main/rx/ibus.c @@ -171,7 +171,7 @@ static void updateChannelData(void) { for (i = 0, offset = ibusChannelOffset; i < IBUS_MAX_SLOTS; i++, offset += 2) { ibusChannelData[i] = ibus[offset] + ((ibus[offset + 1] & 0x0F) << 8); } - //latest IBUS recievers are using prviously not used 4 bits on every channel to incresse total channel count + //latest IBUS receivers are using previously not used 4 bits on every channel to increase total channel count for (i = IBUS_MAX_SLOTS, offset = ibusChannelOffset + 1; i < IBUS_MAX_CHANNEL; i++, offset += 6) { ibusChannelData[i] = ((ibus[offset] & 0xF0) >> 4) | (ibus[offset + 2] & 0xF0) | ((ibus[offset + 4] & 0xF0) << 4); } diff --git a/src/main/rx/msp_override.c b/src/main/rx/msp_override.c index d302433a918..2941cf82cdc 100755 --- a/src/main/rx/msp_override.c +++ b/src/main/rx/msp_override.c @@ -59,7 +59,7 @@ static timeMs_t validRxDataFailedAt = 0; static timeUs_t rxNextUpdateAtUs = 0; static timeUs_t needRxSignalBefore = 0; -static uint16_t mspOverrideCtrlChannels = 0; // bitmask representing which channels are used to control MSP override +static uint32_t mspOverrideCtrlChannels = 0; // bitmask representing which channels are used to control MSP override static rcChannel_t mspRcChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT]; static rxRuntimeConfig_t rxRuntimeConfigMSP; @@ -221,7 +221,7 @@ bool mspOverrideCalculateChannels(timeUs_t currentTimeUs) void mspOverrideChannels(rcChannel_t *rcChannels) { - for (uint16_t channel = 0, channelMask = 1; channel < rxRuntimeConfigMSP.channelCount; ++channel, channelMask <<= 1) { + for (uint32_t channel = 0, channelMask = 1; channel < rxRuntimeConfigMSP.channelCount; ++channel, channelMask <<= 1) { if (rxConfig()->mspOverrideChannels & ~mspOverrideCtrlChannels & channelMask) { rcChannels[channel].raw = rcChannels[channel].data = mspRcChannels[channel].data; } diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 00ed23ab25c..0531904d063 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -98,7 +98,7 @@ rxLinkStatistics_t rxLinkStatistics; rxRuntimeConfig_t rxRuntimeConfig; static uint8_t rcSampleIndex = 0; -PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 12); +PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 13); #ifndef SERIALRX_PROVIDER #define SERIALRX_PROVIDER 0 diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 64b97b172e2..3ed6add3e48 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -126,7 +126,7 @@ typedef struct rxConfig_s { uint8_t rcFilterFrequency; // RC filter cutoff frequency (smoothness vs response sharpness) uint8_t autoSmooth; // auto smooth rx input (0 = off, 1 = on) uint8_t autoSmoothFactor; // auto smooth rx input factor (1 = no smoothing, 100 = lots of smoothing) - uint16_t mspOverrideChannels; // Channels to override with MSP RC when BOXMSPRCOVERRIDE is active + uint32_t mspOverrideChannels; // Channels to override with MSP RC when BOXMSPRCOVERRIDE is active uint8_t rssi_source; #ifdef USE_SERIALRX_SRXL2 uint8_t srxl2_unit_id; diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index e9167e09120..38d410610e1 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -36,7 +36,7 @@ #include "drivers/time.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/fc_core.h" #include "fc/runtime_config.h" #include "fc/stats.h" @@ -62,6 +62,9 @@ #if defined(USE_FAKE_BATT_SENSOR) #include "sensors/battery_sensor_fake.h" #endif +#if defined(USE_SMARTPORT_MASTER) +#include "io/smartport_master.h" +#endif #define ADCVREF 3300 // in mV (3300 = 3.3V) @@ -120,7 +123,7 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance) .critical = SETTING_BATTERY_CAPACITY_CRITICAL_DEFAULT, }, - .controlRateProfile = 0, + .controlProfile = 0, .motor = { .throttleIdle = SETTING_THROTTLE_IDLE_DEFAULT, @@ -245,8 +248,8 @@ void setBatteryProfile(uint8_t profileIndex) profileIndex = 0; } currentBatteryProfile = batteryProfiles(profileIndex); - if ((currentBatteryProfile->controlRateProfile > 0) && (currentBatteryProfile->controlRateProfile < MAX_CONTROL_RATE_PROFILE_COUNT)) { - setConfigProfile(currentBatteryProfile->controlRateProfile - 1); + if ((currentBatteryProfile->controlProfile > 0) && (currentBatteryProfile->controlProfile <= MAX_CONTROL_PROFILE_COUNT)) { + setConfigProfile(currentBatteryProfile->controlProfile - 1); } } @@ -291,6 +294,17 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected) vbat = fakeBattSensorGetVBat(); break; #endif + +#if defined(USE_SMARTPORT_MASTER) + case VOLTAGE_SENSOR_SMARTPORT: + int16_t * smartportVoltageData = smartportMasterGetVoltageData(); + if (smartportVoltageData) { + vbat = *smartportVoltageData; + } else { + vbat = 0; + } + break; +#endif case VOLTAGE_SENSOR_NONE: default: vbat = 0; @@ -598,7 +612,16 @@ void currentMeterUpdate(timeUs_t timeDelta) } break; #endif - +#if defined(USE_SMARTPORT_MASTER) + case CURRENT_SENSOR_SMARTPORT: + int16_t * smartportCurrentData = smartportMasterGetCurrentData(); + if (smartportCurrentData) { + amperage = *smartportCurrentData; + } else { + amperage = 0; + } + break; +#endif #if defined(USE_FAKE_BATT_SENSOR) case CURRENT_SENSOR_FAKE: amperage = fakeBattSensorGetAmerperage(); diff --git a/src/main/sensors/battery_config_structs.h b/src/main/sensors/battery_config_structs.h index 829ac8d8794..7c4157cb4c0 100644 --- a/src/main/sensors/battery_config_structs.h +++ b/src/main/sensors/battery_config_structs.h @@ -31,14 +31,17 @@ typedef enum { CURRENT_SENSOR_VIRTUAL, CURRENT_SENSOR_FAKE, CURRENT_SENSOR_ESC, - CURRENT_SENSOR_MAX = CURRENT_SENSOR_FAKE + CURRENT_SENSOR_SMARTPORT, + CURRENT_SENSOR_MAX = CURRENT_SENSOR_SMARTPORT } currentSensor_e; typedef enum { VOLTAGE_SENSOR_NONE = 0, VOLTAGE_SENSOR_ADC, VOLTAGE_SENSOR_ESC, - VOLTAGE_SENSOR_FAKE + VOLTAGE_SENSOR_FAKE, + VOLTAGE_SENSOR_SMARTPORT, + VOLTAGE_SENSOR_MAX = VOLTAGE_SENSOR_SMARTPORT } voltageSensor_e; typedef enum { @@ -97,7 +100,7 @@ typedef struct batteryProfile_s { uint32_t critical; // mAh or mWh (see batteryMetersConfig()->capacity_unit) } capacity; - uint8_t controlRateProfile; + uint8_t controlProfile; struct { float throttleIdle; // Throttle IDLE value based on min_command, max_throttle, in percent diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 02a9df69c44..4e87b944168 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -38,6 +38,7 @@ #include "drivers/compass/compass_ist8310.h" #include "drivers/compass/compass_ist8308.h" #include "drivers/compass/compass_qmc5883l.h" +#include "drivers/compass/compass_qmc5883p.h" #include "drivers/compass/compass_mpu9250.h" #include "drivers/compass/compass_lis3mdl.h" #include "drivers/compass/compass_rm3100.h" @@ -107,6 +108,19 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) } FALLTHROUGH; + case MAG_QMC5883P: +#ifdef USE_MAG_QMC5883P + if (qmc5883pDetect(dev)) { + magHardware = MAG_QMC5883P; + break; + } +#endif + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (magHardwareToUse != MAG_AUTODETECT) { + break; + } + FALLTHROUGH; + case MAG_HMC5883: #ifdef USE_MAG_HMC5883 if (hmc5883lDetect(dev)) { diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index c6dd78f7db5..17930678f58 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -36,6 +36,7 @@ typedef enum { MAG_AK8963, MAG_IST8310, MAG_QMC5883, + MAG_QMC5883P, MAG_MPU9250, MAG_IST8308, MAG_LIS3MDL, diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index baf7bed0a61..edcc5b9219d 100755 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -209,9 +209,9 @@ STATIC_PROTOTHREAD(pitotThread) // Init filter pitot.lastMeasurementUs = micros(); - - pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f); - + if(pitotmeterConfig()->pitot_lpf_milli_hz >0){ + pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f); + } while(1) { #ifdef USE_SIMULATOR while (SIMULATOR_HAS_OPTION(HITL_AIRSPEED) && SIMULATOR_HAS_OPTION(HITL_PITOT_FAILURE)) @@ -263,7 +263,11 @@ STATIC_PROTOTHREAD(pitotThread) // NOTE ::filter pressure - apply filter when NOT calibrating for zero !!! currentTimeUs = micros(); - pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs)); + if(pitotmeterConfig()->pitot_lpf_milli_hz >0){ + pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs)); + }else{ + pitot.pressure = pitotPressureTmp; + } pitot.lastMeasurementUs = currentTimeUs; pitot.airSpeed = pitotmeterConfig()->pitot_scale * fast_fsqrtf(2.0f * fabsf(pitot.pressure - pitot.pressureZero) / SSL_AIR_DENSITY) * 100; // cm/s @@ -295,6 +299,9 @@ void pitotUpdate(void) pitotThread(); } +/* + * Airspeed estimate in cm/s + */ float getAirspeedEstimate(void) { return pitot.airSpeed; @@ -305,4 +312,13 @@ bool pitotIsHealthy(void) return (millis() - pitot.lastSeenHealthyMs) < PITOT_HARDWARE_TIMEOUT_MS; } +bool pitotValidForAirspeed(void) +{ + bool ret = false; + ret = pitotIsHealthy() && pitotIsCalibrationComplete(); + if (detectedSensors[SENSOR_INDEX_PITOT] == PITOT_VIRTUAL) { + ret = ret && STATE(GPS_FIX); + } + return ret; +} #endif /* PITOT */ diff --git a/src/main/sensors/pitotmeter.h b/src/main/sensors/pitotmeter.h index aed924f8e4c..68f5a9c1a02 100755 --- a/src/main/sensors/pitotmeter.h +++ b/src/main/sensors/pitotmeter.h @@ -69,5 +69,6 @@ void pitotStartCalibration(void); void pitotUpdate(void); float getAirspeedEstimate(void); bool pitotIsHealthy(void); +bool pitotValidForAirspeed(void); #endif diff --git a/src/main/sensors/temperature.c b/src/main/sensors/temperature.c index 3d5c05f179c..fe5e80c792f 100644 --- a/src/main/sensors/temperature.c +++ b/src/main/sensors/temperature.c @@ -98,7 +98,7 @@ static void newSensorCheckAndEnter(uint8_t type, uint64_t addr) void temperatureInit(void) { - memset(sensorStatus, 0, sizeof(sensorStatus) * sizeof(*sensorStatus)); + memset(sensorStatus, 0, sizeof(sensorStatus)); sensorsSet(SENSOR_TEMP); diff --git a/src/main/target/AOCODARCF7MINI/target.c b/src/main/target/AOCODARCF7MINI/target.c index 72140f50fac..223d51e5381 100644 --- a/src/main/target/AOCODARCF7MINI/target.c +++ b/src/main/target/AOCODARCF7MINI/target.c @@ -33,9 +33,6 @@ #include "drivers/pinio.h" #include "drivers/sensor.h" -BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); -BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); - timerHardware_t timerHardware[] = { // DEF_TIM(TIM1, CH3, PA10, TIM_USE_PPM, 0, 0), // PPM, RX1 diff --git a/src/main/target/AOCODARCF7MINI/target.h b/src/main/target/AOCODARCF7MINI/target.h index 4a5f5068746..387c82b2954 100644 --- a/src/main/target/AOCODARCF7MINI/target.h +++ b/src/main/target/AOCODARCF7MINI/target.h @@ -32,7 +32,6 @@ #define BEEPER_INVERTED // *************** SPI1 Gyro & ACC ******************* -#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS #define USE_SPI #define USE_SPI_DEVICE_1 diff --git a/src/main/target/BLUEBERRYF435WING/CMakeLists.txt b/src/main/target/BLUEBERRYF435WING/CMakeLists.txt new file mode 100644 index 00000000000..ea2bc00da7f --- /dev/null +++ b/src/main/target/BLUEBERRYF435WING/CMakeLists.txt @@ -0,0 +1,2 @@ +target_at32f43x_xGT7(BLUEBERRYF435WING) +target_at32f43x_xGT7(BLUEBERRYF435WING_SD) \ No newline at end of file diff --git a/src/main/target/BLUEBERRYF435WING/config.c b/src/main/target/BLUEBERRYF435WING/config.c new file mode 100644 index 00000000000..2c6de82ad87 --- /dev/null +++ b/src/main/target/BLUEBERRYF435WING/config.c @@ -0,0 +1,49 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 http://www.gnu.org/licenses/. + */ + +#include +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "io/serial.h" +#include "io/piniobox.h" +#include "sensors/gyro.h" + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART7)].functionMask = FUNCTION_RX_SERIAL; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].functionMask = FUNCTION_GPS; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].msp_baudrateIndex = BAUD_115200; + //pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + +#ifdef USE_DYNAMIC_FILTERS + // Disable dynamic notch filter by default (performance optimization for wing) + // This board is performance-constrained and wing aircraft typically don't need + // dynamic notch filtering (designed for multirotor motor noise) + gyroConfigMutable()->dynamicGyroNotchEnabled = 0; +#endif +} diff --git a/src/main/target/BLUEBERRYF435WING/target.c b/src/main/target/BLUEBERRYF435WING/target.c new file mode 100644 index 00000000000..9c05660c1eb --- /dev/null +++ b/src/main/target/BLUEBERRYF435WING/target.c @@ -0,0 +1,49 @@ +/* +* This file is part of Cleanflight. +* +* Cleanflight 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. +* +* Cleanflight 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 Cleanflight. If not, see . +*/ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +timerHardware_t timerHardware[] = { + + DEF_TIM(TMR3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0,0), // PWM 1 + DEF_TIM(TMR3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0,1), // PWM 2 + + DEF_TIM(TMR2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0,2), // PWM 3 + DEF_TIM(TMR2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0,3), // PWM 4 + + DEF_TIM(TMR2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0,4), // PWM 5 + + DEF_TIM(TMR4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 0,5), // PWM 6 UART5 + DEF_TIM(TMR4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0,6), // PWM 7 UART5 + + DEF_TIM(TMR4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0,8), // PWM 8, DMA 7 may be used by ADC + DEF_TIM(TMR4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0,9), // PWM 9 + + DEF_TIM(TMR1, CH1, PA8, TIM_USE_LED, 0,10), //WS2812 // PWM1 - LED MCO1 DMA1 CH2 + + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); + diff --git a/src/main/target/BLUEBERRYF435WING/target.h b/src/main/target/BLUEBERRYF435WING/target.h new file mode 100644 index 00000000000..1dbfedf8a6b --- /dev/null +++ b/src/main/target/BLUEBERRYF435WING/target.h @@ -0,0 +1,184 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "BlueBerry" + +#define USBD_PRODUCT_STRING "BLUEBERRYF435WING" + +/**********swd debuger reserved ***************** + * + * pa13 swdio + * pa14 swclk + * PA15 JTDI + * PB4 JREST + * pb3 swo /DTO + + * other pin + * + * PB2 ->BOOT0 button + * PA8 MCO1 + * PA11 OTG1 D+ DP + * PA10 OTG1 D- DN + * PH0 HEXT IN + * PH1 HEXT OUT + */ + +#define LED0 PC13 +#define LED1 PC14 +#define LED0_INVERTED +#define LED1_INVERTED + +#define BEEPER PC15 +#define BEEPER_INVERTED + +// *************** Gyro & ACC ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 +#define SPI1_NSS_PIN PA4 + +// MPU6500 +#define USE_IMU_MPU6500 +#define IMU_MPU6500_ALIGN CW0_DEG +#define MPU6500_SPI_BUS BUS_SPI1 +#define MPU6500_CS_PIN SPI1_NSS_PIN + +// ICM42605/ICM42688P +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW0_DEG_FLIP + +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN SPI1_NSS_PIN + +// BMI270 +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW0_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN SPI1_NSS_PIN + +// LSM6DXX +#define USE_IMU_LSM6DXX +#define IMU_LSM6DXX_ALIGN CW0_DEG +#define LSM6DXX_CS_PIN SPI1_NSS_PIN +#define LSM6DXX_SPI_BUS BUS_SPI1 + + +// *************** I2C/Baro/Mag/EXT********************* +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PH2 // SCL pad +#define I2C2_SDA PH3 // SDA pad +#define USE_I2C_PULLUP + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C2 +#define USE_BARO_BMP280 +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_ALL +#define DEFAULT_I2C_BUS BUS_I2C2 + +// *************** OSD ***************************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 +#define SPI2_NSS_PIN PB12 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN SPI2_NSS_PIN + +//SDCARD Definations +#if defined(BLUEBERRYF435WING_SD) + #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + #define USE_SDCARD + #define USE_SDCARD_SPI + #define SDCARD_SPI_BUS BUS_SPI2 + #define SDCARD_CS_PIN PB5 +//FLASHFS Definations +#else + #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + #define USE_FLASHFS + #define USE_FLASH_M25P16 + #define M25P16_SPI_BUS BUS_SPI2 + #define M25P16_CS_PIN PB5 + + #define USE_FLASH_W25N01G + #define W25N01G_SPI_BUS BUS_SPI2 + #define W25N01G_CS_PIN PB5 +#endif + +// *************** UART ***************************** +#define USE_VCP +//#define USB_DETECT_PIN PC14 +#define USE_USB_DETECT + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART3 //BLE +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART7 //SBUS +#define UART7_RX_PIN PB3 +#define UART7_TX_PIN PB4 + +#define SERIAL_PORT_COUNT 4 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART7 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC1_DMA_STREAM DMA2_CHANNEL1 +#define ADC_CHANNEL_1_PIN PA0 +#define ADC_CHANNEL_2_PIN PA1 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +#define VBAT_SCALE_DEFAULT 1100 +#define CURRENT_METER_SCALE 300 +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY| FEATURE_VBAT | FEATURE_OSD ) + +#define USE_LED_STRIP +#define WS2811_PIN PA8 //TIM1_CH1 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE BIT(2) +#define TARGET_IO_PORTH BIT(1)|BIT(2)|BIT(3) + +#define MAX_PWM_OUTPUT_PORTS 9 +#define USE_DSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/BOTWINGF405/CMakeLists.txt b/src/main/target/BOTWINGF405/CMakeLists.txt new file mode 100644 index 00000000000..acb4b06df5a --- /dev/null +++ b/src/main/target/BOTWINGF405/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(BOTWINGF405 HSE_MHZ 24) diff --git a/src/main/target/BOTWINGF405/config.c b/src/main/target/BOTWINGF405/config.c new file mode 100644 index 00000000000..9df452ea071 --- /dev/null +++ b/src/main/target/BOTWINGF405/config.c @@ -0,0 +1,19 @@ + + #include + #include + + #include "platform.h" + + #include "fc/fc_msp_box.h" + #include "io/serial.h" + + void targetConfiguration(void) + { + + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_RX_SERIAL; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].functionMask = FUNCTION_ESCSERIAL; + } \ No newline at end of file diff --git a/src/main/target/BOTWINGF405/target.c b/src/main/target/BOTWINGF405/target.c new file mode 100644 index 00000000000..de6c51d4f69 --- /dev/null +++ b/src/main/target/BOTWINGF405/target.c @@ -0,0 +1,24 @@ +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" + + + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 1, 1), // S1 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 1, 1), // S2 + DEF_TIM(TIM3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 1, 0), // S3 + DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 1, 0), // S4 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0), + +}; + + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); + \ No newline at end of file diff --git a/src/main/target/BOTWINGF405/target.h b/src/main/target/BOTWINGF405/target.h new file mode 100644 index 00000000000..f4860c653a5 --- /dev/null +++ b/src/main/target/BOTWINGF405/target.h @@ -0,0 +1,156 @@ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "BLDY" +#define USBD_PRODUCT_STRING "BOTWINGF405" + +/*** Indicators ***/ +#define LED0 PC0 +#define LED1 PC5 + +#define BEEPER PB0 +#define BEEPER_INVERTED + +// *************** SPI ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +/* +* I2C +*/ +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +/* +* Serial +*/ +// *************** UART ***************************** +#define USB_IO +#define USE_VCP +#define SERIAL_PORT_COUNT 7 + +#define USE_UART1 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +/* +* Gyro +*/ +#define USE_IMU_ICM42605 +#define ICM42605_CS_PIN PA4 +#define ICM42605_SPI_BUS BUS_SPI1 +#define IMU_ICM42605_ALIGN CW0_DEG + +/* +* Other +*/ +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + + +/* +* OSD +*/ +#define USE_MAX7456 +#define MAX7456_CS_PIN PA15 +#define MAX7456_SPI_BUS BUS_SPI3 + +/* +* Blackbox +*/ +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN PB12 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART1 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 + +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC3 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 +#define RSSI_ADC_CHANNEL ADC_CHN_2 +#define VBAT_SCALE_DEFAULT 1420 +#define CURRENT_METER_SCALE 206 + + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PB1 // RF Switch +#define PINIO2_PIN PC13 // RF Switch +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED +#define PINIO2_FLAGS PINIO_FLAGS_INVERTED + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PB3 + + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + + + +#define MAX_PWM_OUTPUT_PORTS 4 + + +#define USE_DSHOT +#define USE_SERIALSHOT +#define USE_ESC_SENSOR \ No newline at end of file diff --git a/src/main/target/BOTWINGF722/CMakeLists.txt b/src/main/target/BOTWINGF722/CMakeLists.txt new file mode 100644 index 00000000000..c012c069d9f --- /dev/null +++ b/src/main/target/BOTWINGF722/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(BOTWINGF722) diff --git a/src/main/target/BOTWINGF722/config.c b/src/main/target/BOTWINGF722/config.c new file mode 100644 index 00000000000..8935fb2bd5c --- /dev/null +++ b/src/main/target/BOTWINGF722/config.c @@ -0,0 +1,32 @@ +#include +#include +#include "platform.h" +#include "drivers/pwm_mapping.h" +#include "fc/fc_msp_box.h" +#include "io/piniobox.h" +#include "io/serial.h" +#include "common/axis.h" +#include "config/config_master.h" +#include "config/feature.h" +#include "drivers/sensor.h" +#include "drivers/pwm_esc_detect.h" +#include "drivers/pwm_output.h" +#include "drivers/serial.h" +#include "fc/rc_controls.h" +#include "flight/failsafe.h" +#include "flight/mixer.h" +#include "flight/pid.h" +#include "rx/rx.h" +#include "sensors/sensors.h" +#include "telemetry/telemetry.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].functionMask = FUNCTION_RX_SERIAL; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART3)].msp_baudrateIndex = BAUD_115200; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_ESCSERIAL; + +} diff --git a/src/main/target/BOTWINGF722/target.c b/src/main/target/BOTWINGF722/target.c new file mode 100644 index 00000000000..2af89d6863b --- /dev/null +++ b/src/main/target/BOTWINGF722/target.c @@ -0,0 +1,21 @@ +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" + + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0),// Servo 1 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0),// Servo 2 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), +}; +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BOTWINGF722/target.h b/src/main/target/BOTWINGF722/target.h new file mode 100644 index 00000000000..2d3823f2a04 --- /dev/null +++ b/src/main/target/BOTWINGF722/target.h @@ -0,0 +1,139 @@ +#pragma once +#define TARGET_BOARD_IDENTIFIER "BLDY" +#define USBD_PRODUCT_STRING "BOTWINGF722" +#define USE_TARGET_CONFIG + + +#define LED0 PC4 +#define LED1 PB2 + +// *************** UART ***************************** +#define USE_VCP +#define USB_IO + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 6 + +// *************** Gyro & ACC ********************** +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW0_DEG + +#define USE_SPI +#define USE_SPI_DEVICE_3 +// #define SPI3_NSS_PIN PD2 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +// #define ICM42605_CS_PIN SPI3_NSS_PIN +#define ICM42605_CS_PIN PD2 +#define ICM42605_SPI_BUS BUS_SPI3 + +#define USE_BEEPER +#define BEEPER PB7 +#define BEEPER_INVERTED + +// *************** I2C(Baro & I2C) ************************** +#define USE_I2C +#define USE_I2C_DEVICE_3 +#define I2C3_SCL PA8 +#define I2C3_SDA PC9 + + +// Baro +#define USE_BARO +#define USE_BARO_DPS310 +#define BARO_I2C_BUS BUS_I2C3 + +// Mag +#define USE_MAG +#define USE_MAG_ALL +#define MAG_I2C_BUS BUS_I2C3 + +// *************** Flash ************************** +#define USE_SPI_DEVICE_1 +#define SPI1_NSS_PIN PA4 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_FLASHFS +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN SPI1_NSS_PIN +#define M25P16_SPI_BUS BUS_SPI1 + + + +// *************** OSD ***************************** +#define USE_OSD +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC0 // CURRENT +#define ADC_CHANNEL_2_PIN PC1 // RSSI +#define ADC_CHANNEL_3_PIN PC2 // VBAT +#define VBAT_ADC_CHANNEL ADC_CHN_3 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 +#define RSSI_ADC_CHANNEL ADC_CHN_2 + +// *************** LED ***************************** +#define USE_LED_STRIP +#define WS2811_PIN PB6 + + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_SERIALSHOT + +#define SERIALRX_UART SERIAL_PORT_USART2 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define MAX_PWM_OUTPUT_PORTS 6 + +#define CURRENT_METER_SCALE 102 + +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC3 // RF Switch +#define PINIO2_PIN PC14 // RF Switch +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED +#define PINIO2_FLAGS PINIO_FLAGS_INVERTED + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + diff --git a/src/main/target/BROTHERHOBBYF405V3/CMakeLists.txt b/src/main/target/BROTHERHOBBYF405V3/CMakeLists.txt new file mode 100644 index 00000000000..b392ae14c8c --- /dev/null +++ b/src/main/target/BROTHERHOBBYF405V3/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(BROTHERHOBBYF405V3) diff --git a/src/main/target/BROTHERHOBBYF405V3/config.c b/src/main/target/BROTHERHOBBYF405V3/config.c new file mode 100644 index 00000000000..bdbc7da73c4 --- /dev/null +++ b/src/main/target/BROTHERHOBBYF405V3/config.c @@ -0,0 +1,30 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; +} diff --git a/src/main/target/BROTHERHOBBYF405V3/target.c b/src/main/target/BROTHERHOBBYF405V3/target.c new file mode 100644 index 00000000000..942f10ef203 --- /dev/null +++ b/src/main/target/BROTHERHOBBYF405V3/target.c @@ -0,0 +1,42 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + * + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 2), // S1 - DMA2 Stream 3 + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 - DMA2 Stream 2 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 - DMA1 Stream 1 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 1), // S4 - DMA1 Stream 6 - Alternate (was CH3) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 - DMA1 Stream 3 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 - DMA1 Stream 0 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 - DMA1 Stream 7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 - DMA1 Stream 2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_LED, 0, 0), // LED - DMA2 Stream 7 +}; + + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BROTHERHOBBYF405V3/target.h b/src/main/target/BROTHERHOBBYF405V3/target.h new file mode 100644 index 00000000000..acf1f82a6b2 --- /dev/null +++ b/src/main/target/BROTHERHOBBYF405V3/target.h @@ -0,0 +1,155 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#pragma once + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY ) + + +#define TARGET_BOARD_IDENTIFIER "BR43" +#define USBD_PRODUCT_STRING "BROTHER405V3" +// Beeper +#define USE_BEEPER +#define BEEPER PC15 +#define BEEPER_INVERTED + +// Leds +#define USE_LED_STRIP +#define WS2811_PIN PC9 +#define LED0 PC13 + +// UARTs +#define USB_IO +#define USE_VCP +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA10 +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 +#define USE_UART3 +#define UART3_RX_PIN PC11 +#define UART3_TX_PIN PC10 +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 +#define SERIAL_PORT_COUNT 7 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF + +// SPI +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +// I2C +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +// MAG +#define USE_MAG +#define USE_MAG_ALL + +// ADC +#define ADC_CHANNEL_1_PIN PC0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define ADC_CHANNEL_2_PIN PC1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define ADC_CHANNEL_3_PIN PC5 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define USE_ADC +#define ADC_INSTANCE ADC1 + +// Gyro & ACC +#define USE_IMU_ICM42605 +#define ICM42605_CS_PIN PA4 +#define ICM42605_SPI_BUS BUS_SPI1 +#define IMU_ICM42605_ALIGN CW90_DEG + +// BARO +#define USE_BARO +#define USE_BARO_ALL +#define BARO_I2C_BUS BUS_I2C1 + +// OSD +#define USE_MAX7456 +#define MAX7456_CS_PIN PB12 +#define MAX7456_SPI_BUS BUS_SPI2 + +// Blackbox +#define USE_FLASHFS +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PA15 +#define USE_FLASH_W25M +#define W25M_SPI_BUS BUS_SPI3 +#define W25M_CS_PIN PA15 +#define USE_FLASH_W25M02G +#define W25M02G_SPI_BUS BUS_SPI3 +#define W25M02G_CS_PIN PA15 +#define USE_FLASH_W25M512 +#define W25M512_SPI_BUS BUS_SPI3 +#define W25M512_CS_PIN PA15 +#define USE_FLASH_W25N01G +#define W25N01G_SPI_BUS BUS_SPI3 +#define W25N01G_CS_PIN PA15 + +// PINIO + +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC14 + + +// Others + +#define MAX_PWM_OUTPUT_PORTS 9 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_DSHOT +#define USE_ESC_SENSOR +#define CURRENT_METER_SCALE 386 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTF 0xffff + +#define USE_DSHOT_DMAR + diff --git a/src/main/target/BROTHERHOBBYH743/CMakeLists.txt b/src/main/target/BROTHERHOBBYH743/CMakeLists.txt new file mode 100644 index 00000000000..b40fef552fe --- /dev/null +++ b/src/main/target/BROTHERHOBBYH743/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(BROTHERHOBBYH743) diff --git a/src/main/target/BROTHERHOBBYH743/config.c b/src/main/target/BROTHERHOBBYH743/config.c new file mode 100644 index 00000000000..1065971614a --- /dev/null +++ b/src/main/target/BROTHERHOBBYH743/config.c @@ -0,0 +1,32 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + beeperConfigMutable()->pwmMode = true; +} diff --git a/src/main/target/BROTHERHOBBYH743/target.c b/src/main/target/BROTHERHOBBYH743/target.c new file mode 100644 index 00000000000..f9aff080788 --- /dev/null +++ b/src/main/target/BROTHERHOBBYH743/target.c @@ -0,0 +1,53 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_icm42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42605_2, DEVHW_ICM42605, ICM42605_SPI_BUS_2, ICM42605_CS_PIN_2, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN_2); + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA_NONE + + DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 DMA_NONE + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 9), // LED_2812 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BROTHERHOBBYH743/target.h b/src/main/target/BROTHERHOBBYH743/target.h new file mode 100644 index 00000000000..b069f0ccfd0 --- /dev/null +++ b/src/main/target/BROTHERHOBBYH743/target.h @@ -0,0 +1,187 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 http://www.gnu.org/licenses/. + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "BROT" +#define USBD_PRODUCT_STRING "BROTHERHOBBYH743" + +#define USE_TARGET_CONFIG + +#define LED0 PE9 +#define LED1 PA7 + +#define BEEPER PA15 +#define BEEPER_INVERTED +#define BEEPER_PWM_FREQUENCY 2000 + +// *************** IMU generic *********************** +#define USE_DUAL_GYRO +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +// *************** SPI1 IMU0 ICM42605 **************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PD7 + +#define USE_IMU_ICM42605 + +#define IMU_ICM42605_ALIGN CW90_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PC15 + +// *************** SPI4 IMU1 ICM42605 ************** +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + +#define USE_IMU_ICM42605 + +#define IMU_ICM42605_ALIGN_2 CW0_DEG +#define ICM42605_SPI_BUS_2 BUS_SPI4 +#define ICM42605_CS_PIN_2 PE11 + +// *************** SPI2 OSD *********************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** SPI3 ************************ +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C2 +#define USE_BARO_ALL + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 + +#define USE_UART3 +#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 + +#define USE_UART4 +#define UART4_TX_PIN PB9 +#define UART4_RX_PIN PB8 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_UART7 +#define UART7_TX_PIN PE8 +#define UART7_RX_PIN PE7 + +#define USE_UART8 +#define UART8_TX_PIN PE1 +#define UART8_RX_PIN PE0 + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** SDIO SD BLACKBOX******************* +#define USE_SDCARD +#define USE_SDCARD_SDIO +#define SDCARD_SDIO_DEVICE SDIODEV_1 +#define SDCARD_SDIO_4BIT + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC_CHANNEL_1_PIN PC0 //ADC123 VBAT1 +#define ADC_CHANNEL_2_PIN PC1 //ADC123 CURR1 +#define ADC_CHANNEL_3_PIN PC5 //ADC12 RSSI + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PD10 +#define PINIO2_PIN PD11 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 250 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA (0xffff & ~(BIT(14) | BIT(13))) +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define MAX_PWM_OUTPUT_PORTS 15 +#define USE_DSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/CORVON405V2/CMakeLists.txt b/src/main/target/CORVON405V2/CMakeLists.txt new file mode 100644 index 00000000000..ca4412c385a --- /dev/null +++ b/src/main/target/CORVON405V2/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(CORVON405V2) \ No newline at end of file diff --git a/src/main/target/CORVON405V2/config.c b/src/main/target/CORVON405V2/config.c new file mode 100644 index 00000000000..00ee10a378d --- /dev/null +++ b/src/main/target/CORVON405V2/config.c @@ -0,0 +1,34 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include +#include "platform.h" +#include "config/config_master.h" +#include "config/feature.h" +#include "io/serial.h" +#include "fc/config.h" +#include "sensors/gyro.h" + + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[1].msp_baudrateIndex = BAUD_57600; + serialConfigMutable()->portConfigs[2].functionMask = FUNCTION_MSP_OSD; + serialConfigMutable()->portConfigs[3].functionMask = FUNCTION_GPS; + serialConfigMutable()->portConfigs[5].functionMask = FUNCTION_ESCSERIAL; +} diff --git a/src/main/target/CORVON405V2/target.c b/src/main/target/CORVON405V2/target.c new file mode 100644 index 00000000000..8c93f5394af --- /dev/null +++ b/src/main/target/CORVON405V2/target.c @@ -0,0 +1,37 @@ +/* +* This file is part of Cleanflight. +* +* Cleanflight 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. +* +* Cleanflight 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 Cleanflight. If not, see . +*/ + +#include +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(1,7) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(1,2) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 -D(1,5) + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 D(1,6) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 D(1,4) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 -D(1,5) + DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 + DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM12, CH2, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/CORVON405V2/target.h b/src/main/target/CORVON405V2/target.h new file mode 100644 index 00000000000..685acbe0588 --- /dev/null +++ b/src/main/target/CORVON405V2/target.h @@ -0,0 +1,143 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "C405" + +#define USBD_PRODUCT_STRING "CORVON405V2" + +// *************** LED ********************** +#define LED0 PC5 +#define LED1 PC4 +#define LED2 PA8 + +// *************** SPI: Gyro & ACC & BARO & OSD & SDCARD ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 +#define USE_SPI_DEVICE_3 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_IMU_BMI088 +#define IMU_BMI088_ALIGN CW270_DEG +#define BMI088_SPI_BUS BUS_SPI2 +#define BMI088_GYRO_CS_PIN PC14 +#define BMI088_ACC_CS_PIN PC13 + +#define USE_BARO +#define USE_BARO_SPL06 +#define SPL06_SPI_BUS BUS_SPI2 +#define SPL06_CS_PIN PA4 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PB12 + +#define USE_BLACKBOX +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PC9 +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN NONE + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 +#define INVERTER_PIN_UART6_RX PC15 + +#define SERIAL_PORT_COUNT 7 //VCP, UART1, UART2, UART3, UART4, UART5, UART6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART6 +#define USE_UART_INVERTER + +// *************** I2C **************************** +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 +#define DEFAULT_I2C_BUS BUS_I2C1 + +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL + +// *************** ENABLE OPTICAL FLOW & RANGEFINDER ***************************** +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define USE_OPFLOW +#define USE_OPFLOW_MSP + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define VBAT_SCALE_DEFAULT 2121 +#define CURRENT_METER_SCALE 402 + +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_OSD | FEATURE_TELEMETRY) + +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 10 diff --git a/src/main/target/CORVON743V1/CMakeLists.txt b/src/main/target/CORVON743V1/CMakeLists.txt new file mode 100644 index 00000000000..a828286f3d5 --- /dev/null +++ b/src/main/target/CORVON743V1/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(CORVON743V1) diff --git a/src/main/target/CORVON743V1/config.c b/src/main/target/CORVON743V1/config.c new file mode 100644 index 00000000000..6f5be655693 --- /dev/null +++ b/src/main/target/CORVON743V1/config.c @@ -0,0 +1,34 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include +#include "platform.h" +#include "config/config_master.h" +#include "config/feature.h" +#include "io/serial.h" +#include "fc/config.h" +#include "sensors/gyro.h" + + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[1].msp_baudrateIndex = BAUD_57600; + serialConfigMutable()->portConfigs[2].functionMask = FUNCTION_MSP_OSD; + serialConfigMutable()->portConfigs[3].functionMask = FUNCTION_GPS; + serialConfigMutable()->portConfigs[6].functionMask = FUNCTION_ESCSERIAL; +} diff --git a/src/main/target/CORVON743V1/target.c b/src/main/target/CORVON743V1/target.c new file mode 100644 index 00000000000..139fad35772 --- /dev/null +++ b/src/main/target/CORVON743V1/target.c @@ -0,0 +1,42 @@ +/* +* This file is part of Cleanflight. +* +* Cleanflight 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. +* +* Cleanflight 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 Cleanflight. If not, see . +*/ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM1, CH4, PE14, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM1, CH3, PE13, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA_NONE +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/CORVON743V1/target.h b/src/main/target/CORVON743V1/target.h new file mode 100644 index 00000000000..fa5ba03d735 --- /dev/null +++ b/src/main/target/CORVON743V1/target.h @@ -0,0 +1,148 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "C743" + +#define USBD_PRODUCT_STRING "CORVON743V1" + +// *************** LED ********************** +#define LED0 PE5 +#define LED1 PE6 +#define LED2 PE4 + +// *************** SPI: Gyro & ACC & OSD ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define SPI2_SCK_PIN PD3 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define USE_IMU_BMI088 +#define IMU_BMI088_ALIGN CW270_DEG +#define BMI088_SPI_BUS BUS_SPI2 +#define BMI088_GYRO_CS_PIN PD5 +#define BMI088_ACC_CS_PIN PD4 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI1 +#define MAX7456_CS_PIN PB12 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PD9 +#define UART3_TX_PIN PD8 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 +#define INVERTER_PIN_UART6_RX PD0 + +#define USE_UART7 +#define UART7_RX_PIN PE7 + +#define USE_UART8 +#define UART8_RX_PIN PE0 +#define UART8_TX_PIN PE1 + +#define SERIAL_PORT_COUNT 8 //VCP, UART1, UART2, UART3, UART4, UART6, UART7, UART8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART6 + +// *************** I2C: BARO & MAG **************************** +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define USE_I2C_DEVICE_2 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_BARO +#define USE_BARO_DPS310 +#define BARO_I2C_BUS BUS_I2C2 + +#define USE_MAG + +#ifdef CORVON743V1_EXTMAG +// External compass +#define MAG_I2C_BUS BUS_I2C1 +#else +// Onboard compass +#define MAG_I2C_BUS BUS_I2C2 +#endif +#define USE_MAG_ALL + +// *************** ENABLE OPTICAL FLOW & RANGEFINDER ***************************** +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define USE_OPFLOW +#define USE_OPFLOW_MSP + +// *************** SDIO SD BLACKBOX******************* +#define USE_SDCARD +#define USE_SDCARD_SDIO +#define SDCARD_SDIO_DEVICE SDIODEV_1 +#define SDCARD_SDIO_4BIT +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define VBAT_SCALE_DEFAULT 2121 +#define CURRENT_METER_SCALE 402 + +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_OSD | FEATURE_TELEMETRY) + +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define MAX_PWM_OUTPUT_PORTS 10 diff --git a/src/main/target/DAKEFPVF435/CMakeLists.txt b/src/main/target/DAKEFPVF435/CMakeLists.txt new file mode 100644 index 00000000000..ecf44c554c9 --- /dev/null +++ b/src/main/target/DAKEFPVF435/CMakeLists.txt @@ -0,0 +1 @@ +target_at32f43x_xGT7(DAKEFPVF435) \ No newline at end of file diff --git a/src/main/target/DAKEFPVF435/config.c b/src/main/target/DAKEFPVF435/config.c new file mode 100644 index 00000000000..a07a1f16e1a --- /dev/null +++ b/src/main/target/DAKEFPVF435/config.c @@ -0,0 +1,32 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include + +#include + +#include "io/serial.h" +#include "rx/rx.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; +} diff --git a/src/main/target/DAKEFPVF435/target.c b/src/main/target/DAKEFPVF435/target.c new file mode 100644 index 00000000000..708e522a2ac --- /dev/null +++ b/src/main/target/DAKEFPVF435/target.c @@ -0,0 +1,64 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 http://www.gnu.org/licenses/. + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +// # pin A00: TIM2 CH1 (AF1) +// # pin A01: TIM2 CH2 (AF1) +// # pin A02: TIM2 CH3 (AF1) +// # pin A03: TIM2 CH4 (AF1) +// # pin A08: TIM1 CH1 (AF1) +// # pin A09: TIM1 CH2 (AF1) +// # pin A10: TIM1 CH3 (AF1) +// # pin C09: TIM8 CH4 (AF3) +// # pin H03: TIM5 CH2 (AF2) +// # pin H02: TIM5 CH1 (AF2) +// # pin C08: TIM3 CH3 (AF2) +// # pin C05: TIM9 CH2 (AF3) + +timerHardware_t timerHardware[] = { + + DEF_TIM(TMR2, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0,0), // M1 + DEF_TIM(TMR2, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0,1), // M2 + DEF_TIM(TMR2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0,2), // M3 + DEF_TIM(TMR2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0,3), // M4 + DEF_TIM(TMR1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0,4), // M5 + DEF_TIM(TMR1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0,5), // M6 + DEF_TIM(TMR1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0,6), // M7 + DEF_TIM(TMR8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0,8), // M8 + DEF_TIM(TMR5, CH2, PH3, TIM_USE_OUTPUT_AUTO, 0,9), // S1 + DEF_TIM(TMR5, CH1, PH2, TIM_USE_OUTPUT_AUTO, 0,10),// S2 + + DEF_TIM(TMR3, CH3, PC8, TIM_USE_LED | TIM_USE_ANY, 0, 11), // LED_STRIP +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); + diff --git a/src/main/target/DAKEFPVF435/target.h b/src/main/target/DAKEFPVF435/target.h new file mode 100644 index 00000000000..75e5252bf88 --- /dev/null +++ b/src/main/target/DAKEFPVF435/target.h @@ -0,0 +1,174 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 http://www.gnu.org/licenses/. + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "DAK4" +#define USBD_PRODUCT_STRING "DAKEFPV F435" + +#define LED0 PA15 + +#define BEEPER PC3 +#define BEEPER_INVERTED + + +// Buses + +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 +// #define USE_I2C_PULLUP + +#define DEFAULT_I2C_BUS BUS_I2C1 + +// Gyro +#define USE_IMU_MPU6500 +#define IMU_MPU6500_ALIGN CW90_DEG +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_BUS BUS_SPI1 + +// ICM42605/ICM42688P +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PA4 + +//Baro +#define USE_BARO +#define USE_BARO_BMP280 +#define BMP280_I2C_BUS DEFAULT_I2C_BUS + +// Other sensors + +#define USE_BARO +#define BARO_I2C_BUS DEFAULT_I2C_BUS +#define USE_BARO_ALL + +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL + +// OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// Flash + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PB1 + +#define USE_FLASH_W25N01G +#define W25N01G_SPI_BUS BUS_SPI3 +#define W25N01G_CS_PIN PB1 + +// UARTs +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PB6 +#define UART1_RX_PIN PB7 + +// USART3 RX only +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PC13 // PC13 is just for compilation pass + +#define USE_UART4 +#define UART4_TX_PIN PC10 +#define UART4_RX_PIN PC11 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART5 +#define GPS_UART SERIAL_PORT_USART1 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define RSSI_ADC_CHANNEL ADC_CHN_2 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY ) + +#define USE_LED_STRIP +#define WS2811_PIN PC8 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff +#define TARGET_IO_PORTH 0xffff + +#define USE_DSHOT +#define USE_ESC_SENSOR +#define MAX_PWM_OUTPUT_PORTS 11 + +// PINIO +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PB0 +#define PINIO2_PIN PB10 + +// VBAT 10K/160K +#define VBAT_SCALE_DEFAULT 1094 diff --git a/src/main/target/DAKEFPVF722/target.h b/src/main/target/DAKEFPVF722/target.h index 9fd3e5b8c65..502cfe98a06 100644 --- a/src/main/target/DAKEFPVF722/target.h +++ b/src/main/target/DAKEFPVF722/target.h @@ -77,6 +77,9 @@ #define USE_BARO_BMP280 #define BMP280_SPI_BUS BUS_SPI2 #define BMP280_CS_PIN PA13 +#define USE_BARO_DPS310 +#define DPS310_SPI_BUS BUS_SPI2 +#define DPS310_CS_PIN PA13 #define USE_BARO_SPL06 #define SPL06_SPI_BUS BUS_SPI2 diff --git a/src/main/target/DAKEFPVH743/CMakeLists.txt b/src/main/target/DAKEFPVH743/CMakeLists.txt new file mode 100644 index 00000000000..c63f34aab66 --- /dev/null +++ b/src/main/target/DAKEFPVH743/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(DAKEFPVH743) diff --git a/src/main/target/DAKEFPVH743/config.c b/src/main/target/DAKEFPVH743/config.c new file mode 100644 index 00000000000..2b54fd66ec4 --- /dev/null +++ b/src/main/target/DAKEFPVH743/config.c @@ -0,0 +1,33 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include + +#include + +#include "io/serial.h" +#include "rx/rx.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + pinioBoxConfigMutable()->permanentId[2] = BOX_PERMANENT_ID_USER3; +} diff --git a/src/main/target/DAKEFPVH743/target.c b/src/main/target/DAKEFPVH743/target.c new file mode 100644 index 00000000000..1465bef05d2 --- /dev/null +++ b/src/main/target/DAKEFPVH743/target.c @@ -0,0 +1,54 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + + #include + + #include + #include "drivers/io.h" + #include "drivers/pwm_mapping.h" + #include "drivers/timer.h" + #include "drivers/bus.h" + #include "drivers/sensor.h" + + #include "drivers/pwm_output.h" + #include "common/maths.h" + #include "fc/config.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_1_ICM42605, DEVHW_ICM42605, IMU_1_SPI_BUS, IMU_1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_1_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_2_ICM42605, DEVHW_ICM42605, IMU_2_SPI_BUS, IMU_2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_2_ALIGN); + + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 1), // M2 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 2), // M3 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 3), // M4 + + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 4), // M5 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 5), // M6 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 6), // M7 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 7), // M8 + + DEF_TIM(TIM15, CH1, PE5, TIM_USE_OUTPUT_AUTO, 0, 7), // S1 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 DMA None + DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 10), // S3 + DEF_TIM(TIM3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 11), // S4 + + DEF_TIM(TIM1, CH1, PE9, TIM_USE_LED, 0, 9), // LED +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/DAKEFPVH743/target.h b/src/main/target/DAKEFPVH743/target.h new file mode 100644 index 00000000000..29e1154d34a --- /dev/null +++ b/src/main/target/DAKEFPVH743/target.h @@ -0,0 +1,198 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "DAK7" +#define USBD_PRODUCT_STRING "DAKEFPV H743" + +#define LED0 PD10 +#define LED1 PD11 +#define LED2 PA8 + +#define BEEPER PC2 +#define BEEPER_INVERTED + +// Buses +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define DEFAULT_I2C_BUS BUS_I2C2 + +// GYRO +#define USE_DUAL_GYRO +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS // Don't use common busdev descriptors for IMU +#define USE_IMU_MPU6500 +#define USE_IMU_MPU6000 + +#define USE_IMU_ICM42605 +#define USE_IMU_BMI270 + + +// Enable dual gyro mode +#define USE_DUAL_GYRO + +// Primary IMU options +#define IMU_1_CS_PIN PA4 +#define IMU_1_SPI_BUS BUS_SPI1 +#define IMU_1_ALIGN CW0_DEG + +// Secondary IMU options +#define IMU_2_CS_PIN PB1 +#define IMU_2_SPI_BUS BUS_SPI1 +#define IMU_2_ALIGN CW180_DEG + + +// M25P256 flash +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PA15 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// CAMERA_CONTROL +#define CAMERA_CONTROL_PIN PB8 + +// Serial ports +#define USE_VCP + +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 +#define USE_UART6 +#define USE_UART7 +#define USE_UART8 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 +#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 +#define UART4_TX_PIN PD1 +#define UART4_RX_PIN PD0 +#define UART5_TX_PIN PB6 +#define UART5_RX_PIN PB5 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 +#define UART7_TX_PIN PE8 +#define UART7_RX_PIN PE7 +#define UART8_TX_PIN PE1 +#define UART8_RX_PIN PE0 + +#define SERIAL_PORT_COUNT 9 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART5 +#define GPS_UART SERIAL_PORT_USART1 + +//Baro +#define USE_BARO +#define USE_BARO_BMP280 +#define USE_BARO_DPS310 +#define BARO_I2C_BUS DEFAULT_I2C_BUS + +// Mag +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS + +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS + +// unkonw +#define PITOT_I2C_BUS DEFAULT_I2C_BUS + + +// ADC +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC_CHANNEL_1_PIN PC1 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define ADC_CHANNEL_2_PIN PC0 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define ADC_CHANNEL_3_PIN PC5 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +// Additional ADC settings +// #define ADC1_DMA_OPT 9 +#define ADC1_DMA_STREAM DMA2_Stream3 +#define VBAT_SCALE_DEFAULT 1600 + + +#define USE_LED_STRIP +#define WS2811_PIN PE9 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_LED_STRIP | FEATURE_GPS) + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define USE_DSHOT +#define USE_DSHOT_DMAR +#define USE_ESC_SENSOR + +#define MAX_PWM_OUTPUT_PORTS 12 + +// PINIO +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PE2 +#define PINIO2_PIN PE3 +#define PINIO3_PIN PE4 +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED diff --git a/src/main/target/DAKEFPVH743PRO/CMakeLists.txt b/src/main/target/DAKEFPVH743PRO/CMakeLists.txt new file mode 100644 index 00000000000..94c73b28b2d --- /dev/null +++ b/src/main/target/DAKEFPVH743PRO/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(DAKEFPVH743PRO) diff --git a/src/main/target/DAKEFPVH743PRO/config.c b/src/main/target/DAKEFPVH743PRO/config.c new file mode 100644 index 00000000000..2b54fd66ec4 --- /dev/null +++ b/src/main/target/DAKEFPVH743PRO/config.c @@ -0,0 +1,33 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include + +#include + +#include "io/serial.h" +#include "rx/rx.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + pinioBoxConfigMutable()->permanentId[2] = BOX_PERMANENT_ID_USER3; +} diff --git a/src/main/target/DAKEFPVH743PRO/target.c b/src/main/target/DAKEFPVH743PRO/target.c new file mode 100644 index 00000000000..ad80082917b --- /dev/null +++ b/src/main/target/DAKEFPVH743PRO/target.c @@ -0,0 +1,61 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + + #include + + #include + #include "drivers/io.h" + #include "drivers/pwm_mapping.h" + #include "drivers/timer.h" + #include "drivers/bus.h" + #include "drivers/sensor.h" + + #include "drivers/pwm_output.h" + #include "common/maths.h" + #include "fc/config.h" + // Board hardware definitions - IMU1 slot + +BUSDEV_REGISTER_SPI_TAG(busdev_1_mpu6000, DEVHW_MPU6000, IMU_1_SPI_BUS, IMU_1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_1_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_1_mpu6500, DEVHW_MPU6500, IMU_1_SPI_BUS, IMU_1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_1_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_1_ICM42605, DEVHW_ICM42605, IMU_1_SPI_BUS, IMU_1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_1_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_1_BMI270, DEVHW_BMI270, IMU_1_SPI_BUS, IMU_1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_1_ALIGN); + +// // Board hardware definitions - IMU2 slot +BUSDEV_REGISTER_SPI_TAG(busdev_2_mpu6000, DEVHW_MPU6000, IMU_2_SPI_BUS, IMU_2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_2_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_2_mpu6500, DEVHW_MPU6500, IMU_2_SPI_BUS, IMU_2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_2_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_2_ICM42605, DEVHW_ICM42605, IMU_2_SPI_BUS, IMU_2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_2_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_2_BMI270, DEVHW_BMI270, IMU_2_SPI_BUS, IMU_2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU_2_ALIGN); + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM2, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 + DEF_TIM(TIM2, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 1), // M2 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 2), // M3 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 3), // M4 + DEF_TIM(TIM1, CH1, PE9 , TIM_USE_OUTPUT_AUTO, 0, 4), // M5 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 5), // M6 + DEF_TIM(TIM8, CH3, PC8 , TIM_USE_OUTPUT_AUTO, 0, 6), // M7 + DEF_TIM(TIM8, CH4, PC9 , TIM_USE_OUTPUT_AUTO, 0, 7), // M8 DMA None + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 DMA None + DEF_TIM(TIM4, CH3, PD14, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + DEF_TIM(TIM15, CH1, PE5, TIM_USE_ANY, 0, 0), // CAMERA_CONTROL_PIN + DEF_TIM(TIM15, CH2, PE6, TIM_USE_ANY, 0, 0), // GYRO_1_CLKIN_PIN + DEF_TIM(TIM3, CH3, PB0, TIM_USE_LED, 0, 8), // LED_2812 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/DAKEFPVH743PRO/target.h b/src/main/target/DAKEFPVH743PRO/target.h new file mode 100644 index 00000000000..379fd1b9464 --- /dev/null +++ b/src/main/target/DAKEFPVH743PRO/target.h @@ -0,0 +1,188 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "DAK7" +#define USBD_PRODUCT_STRING "DAKEFPV H743PRO" + +#define LED0 PD10 +#define LED1 PD11 +#define LED2 PA8 + +#define BEEPER PC2 +#define BEEPER_INVERTED +#define USE_DSHOT_DMAR +// Buses +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define DEFAULT_I2C_BUS BUS_I2C2 + +// GYRO +#define USE_DUAL_GYRO +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS // Don't use common busdev descriptors for IMU +#define USE_IMU_MPU6500 +#define USE_IMU_MPU6000 + +#define USE_IMU_ICM42605 +#define USE_IMU_BMI270 + +// IMU_1 is verified to work on OBF4V6 and Omnibus Fireworks board +# define IMU_1_CS_PIN PA4 +# define IMU_1_SPI_BUS BUS_SPI1 +# define IMU_1_ALIGN CW0_DEG +// IMU_2 is sketchy and was not verified on actual hardware +# define IMU_2_CS_PIN PB1 +# define IMU_2_SPI_BUS BUS_SPI4 +# define IMU_2_ALIGN CW180_DEG + +// M25P256 flash +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PA15 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// CAMERA_CONTROL +#define CAMERA_CONTROL_PIN PE5 + +// Serial ports +#define USE_VCP + +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 +#define USE_UART6 +#define USE_UART7 +#define USE_UART8 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 +#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 +#define UART4_TX_PIN PB8 +#define UART4_RX_PIN PB9 +#define UART5_TX_PIN PB6 +#define UART5_RX_PIN PB5 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 +#define UART7_TX_PIN PE8 +#define UART7_RX_PIN PE7 +#define UART8_TX_PIN PE1 +#define UART8_RX_PIN PE0 + +#define SERIAL_PORT_COUNT 9 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART5 +#define GPS_UART SERIAL_PORT_USART1 + +//Baro +#define USE_BARO +#define USE_BARO_BMP280 +#define USE_BARO_DPS310 +#define BARO_I2C_BUS DEFAULT_I2C_BUS + +// Mag +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS + +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS + +// unkonw +#define PITOT_I2C_BUS DEFAULT_I2C_BUS + +// ADC +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC5 +#define ADC_CHANNEL_3_PIN PC0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define RSSI_ADC_CHANNEL ADC_CHN_2 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 + +#define USE_LED_STRIP +#define WS2811_PIN PB0 + +// unkonw +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_LED_STRIP | FEATURE_GPS) + +// unkonw +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define MAX_PWM_OUTPUT_PORTS 12 + +// PINIO +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PE2 +#define PINIO2_PIN PE3 +#define PINIO3_PIN PE4 + +// VBAT 10K/160K +#define VBAT_SCALE_DEFAULT 1600 diff --git a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/CMakeLists.txt b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/CMakeLists.txt new file mode 100644 index 00000000000..6da40b8ae6d --- /dev/null +++ b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(FLYINGRCF4WINGMINI_NOT_RECOMMENDED) diff --git a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/config.c b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/config.c new file mode 100644 index 00000000000..faa0a9406c9 --- /dev/null +++ b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/config.c @@ -0,0 +1,27 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include "platform.h" +#include "fc/fc_msp_box.h" +#include "fc/config.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + +} diff --git a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c new file mode 100644 index 00000000000..d86bbb17379 --- /dev/null +++ b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.c @@ -0,0 +1,42 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/sensor.h" +#include "drivers/timer_def_stm32f4xx.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(2,7,7) UP217 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(2,2,0) UP217 + DEF_TIM(TIM1, CH3N, PB15, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 D(2,6,0) UP256 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S4 D(2,1,6) UP256 + + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 D(1,7,3) UP173 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 D(1,1,3) UP173 + + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // 2812LED D(1,2,5) + + DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h new file mode 100644 index 00000000000..cf98fcaca14 --- /dev/null +++ b/src/main/target/FLYINGRCF4WINGMINI_NOT_RECOMMENDED/target.h @@ -0,0 +1,112 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once +#define USE_TARGET_CONFIG + +#define TARGET_BOARD_IDENTIFIER "FRF4WM" +#define USBD_PRODUCT_STRING "FLYINGRCF4WINGMINI_NOT_RECOMMENDED" + +#define LED0 PA14 //Blue +#define LED1 PA13 //Green + +// *************** SPI1 IMU & OSD ******************* +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PB4 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG_FLIP +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PC14 + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB7 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_SPL06 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA2 +#define SOFTSERIAL_1_RX_PIN PA2 + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART1 + +// *************** ADC *************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream4 +#define ADC_CHANNEL_1_PIN PC4 +#define VBAT_ADC_CHANNEL ADC_CHN_1 + +// *************** others ************************ +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define VBAT_SCALE_DEFAULT 2100 +#define CURRENT_METER_SCALE 150 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 7 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_DSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/FLYSPARKF4V4/.gitattributes b/src/main/target/FLYSPARKF4V4/.gitattributes new file mode 100644 index 00000000000..6313b56c578 --- /dev/null +++ b/src/main/target/FLYSPARKF4V4/.gitattributes @@ -0,0 +1 @@ +* text=auto eol=lf diff --git a/src/main/target/FLYSPARKF4V4/CMakeLists.txt b/src/main/target/FLYSPARKF4V4/CMakeLists.txt new file mode 100644 index 00000000000..228de8d4f43 --- /dev/null +++ b/src/main/target/FLYSPARKF4V4/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(FlySparkF4v1) diff --git a/src/main/target/FLYSPARKF4V4/config.c b/src/main/target/FLYSPARKF4V4/config.c new file mode 100644 index 00000000000..a21244ed8bc --- /dev/null +++ b/src/main/target/FLYSPARKF4V4/config.c @@ -0,0 +1,39 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 http://www.gnu.org/licenses/. + */ + +#include +#include "platform.h" +#include "fc/fc_msp_box.h" +#include "io/piniobox.h" +#include "io/serial.h" + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200; + + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART5)].functionMask = FUNCTION_ESCSERIAL; + + pinioBoxConfigMutable()->permanentId[0] = BOXARM; +} diff --git a/src/main/target/FLYSPARKF4V4/target.c b/src/main/target/FLYSPARKF4V4/target.c new file mode 100644 index 00000000000..2d398bd1bbe --- /dev/null +++ b/src/main/target/FLYSPARKF4V4/target.c @@ -0,0 +1,53 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 http://www.gnu.org/licenses/. + */ +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 1, 0), // S1 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 1, 0), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 1, 0), // S3 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 1, 0), // S4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 1, 0), // S5 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 1, 0), // S6 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 1, 0), // S7 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 1, 0), // S8 + + DEF_TIM(TIM12, CH2, PB15, TIM_USE_OUTPUT_AUTO, 1, 0), // S9 + DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 1, 0), // S10 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), //2812LED D(1,5,3) +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FLYSPARKF4V4/target.h b/src/main/target/FLYSPARKF4V4/target.h new file mode 100644 index 00000000000..9a6619f0793 --- /dev/null +++ b/src/main/target/FLYSPARKF4V4/target.h @@ -0,0 +1,168 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 http://www.gnu.org/licenses/. + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "FSF4" +#define USBD_PRODUCT_STRING "FlySparkF4v1" + +/*** Indicators ***/ +#define LED0 PC13 //Blue + +#define BEEPER PC15 +#define BEEPER_INVERTED + +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PB11 +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +// Internally routed to Bluetooth +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 // Not broken out +#define UART5_RX_PIN PD2 //ESC TLM + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA2 +#define SOFTSERIAL_1_RX_PIN PA2 + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** Gyro & ACC ********************** +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG +#define ICM42605_CS_PIN PA4 +#define ICM42605_SPI_BUS BUS_SPI1 + +// *************** I2C(Baro & I2C) ************************** +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +#define USE_RANGEFINDER +#define TEMPERATURE_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +// *************** Internal SD card ************************** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PC14 + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** OSD ***************************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** ADC ***************************** + +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC5 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +// *************** LED ***************************** +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_BLACKBOX | FEATURE_TELEMETRY) +#define CURRENT_METER_SCALE 400 + +#define USE_DSHOT +#define USE_SERIALSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define MAX_PWM_OUTPUT_PORTS 10 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) diff --git a/src/main/target/HAPPYMODELF405/CMakeLists.txt b/src/main/target/HAPPYMODELF405/CMakeLists.txt new file mode 100644 index 00000000000..63b5dc2ef78 --- /dev/null +++ b/src/main/target/HAPPYMODELF405/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(HAPPYMODELF405) diff --git a/src/main/target/HAPPYMODELF405/target.c b/src/main/target/HAPPYMODELF405/target.c new file mode 100644 index 00000000000..93be00233e5 --- /dev/null +++ b/src/main/target/HAPPYMODELF405/target.c @@ -0,0 +1,36 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include +#include "drivers/io.h" +#include "drivers/timer.h" + +timerHardware_t timerHardware[] = { + // DEF_TIM(TIM4, CH3, PB8, TIM_USE_PPM, 0, 0), // PPM + + // Motors + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT D1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT D1_ST2 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S3_OUT D1_ST6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S4_OUT D1_ST1 + + // LED strip + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/HAPPYMODELF405/target.h b/src/main/target/HAPPYMODELF405/target.h new file mode 100644 index 00000000000..abdb3957567 --- /dev/null +++ b/src/main/target/HAPPYMODELF405/target.h @@ -0,0 +1,129 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "HMF4" +#define USBD_PRODUCT_STRING "HAPPYMODEL F405" + +#define LED0 PB5 + +#define BEEPER PB4 +#define BEEPER_INVERTED + +// Buses +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C1_SCL PB10 +#define I2C1_SDA PB11 + + +// Gyro +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW0_DEG +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW0_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PA4 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW0_DEG +#define BMI270_CS_PIN PA4 +#define BMI270_SPI_BUS BUS_SPI1 + +//Baro +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C2 +#define USE_BARO_BMP280 +#define USE_BARO_SPL06 + +// M25P16 flash +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PA15 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// Serial ports +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 4 + +// Mag +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_ALL + +// ADC +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 +#define VBAT_ADC_CHANNEL ADC_CHN_2 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 + +#define CURRENT_METER_SCALE 470 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY ) + +#define USE_LED_STRIP +#define WS2811_PIN PB6 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define MAX_PWM_OUTPUT_PORTS 4 diff --git a/src/main/target/HUMMINGBIRD_FC305/CMakeLists.txt b/src/main/target/HUMMINGBIRD_FC305/CMakeLists.txt new file mode 100644 index 00000000000..176b1cac746 --- /dev/null +++ b/src/main/target/HUMMINGBIRD_FC305/CMakeLists.txt @@ -0,0 +1,2 @@ +target_stm32f722xe(HUMMINGBIRD_FC305) + diff --git a/src/main/target/HUMMINGBIRD_FC305/config.c b/src/main/target/HUMMINGBIRD_FC305/config.c new file mode 100644 index 00000000000..f024a26d54c --- /dev/null +++ b/src/main/target/HUMMINGBIRD_FC305/config.c @@ -0,0 +1,33 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include +#include "platform.h" + +#include "config/config_master.h" +#include "config/feature.h" +#include "sensors/barometer.h" +#include "io/serial.h" +#include "io/piniobox.h" + + +void targetConfiguration(void) +{ + barometerConfigMutable()->baro_hardware = BARO_SPL06; + serialConfigMutable()->portConfigs[3].functionMask = FUNCTION_ESCSERIAL; + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; +} diff --git a/src/main/target/HUMMINGBIRD_FC305/target.c b/src/main/target/HUMMINGBIRD_FC305/target.c new file mode 100644 index 00000000000..005d5a16990 --- /dev/null +++ b/src/main/target/HUMMINGBIRD_FC305/target.c @@ -0,0 +1,53 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include + +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/bus.h" +#include "drivers/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); +BUSDEV_REGISTER_SPI(busdev_sdcard_spi, DEVHW_SDCARD, SDCARD_SPI_BUS, SDCARD_CS_PIN, NONE, DEVFLAGS_SPI_MODE_0, 0); +BUSDEV_REGISTER_SPI(busdev_max7456, DEVHW_MAX7456, MAX7456_SPI_BUS, MAX7456_CS_PIN, NONE, DEVFLAGS_USE_RAW_REGISTERS, 0); +BUSDEV_REGISTER_I2C(busdev_spl06, DEVHW_SPL06, SPL06_I2C_BUS, SPL06_I2C_ADDR, NONE, DEVFLAGS_NONE, 0); + +BUSDEV_REGISTER_I2C(busdev_qmc5883, DEVHW_QMC5883, MAG_I2C_BUS, 0x0D, NONE, DEVFLAGS_NONE, 0); +BUSDEV_REGISTER_I2C(busdev_hmc5883, DEVHW_HMC5883, MAG_I2C_BUS, 0x1E, NONE, DEVFLAGS_NONE, 0); +BUSDEV_REGISTER_I2C(busdev_vcm5883, DEVHW_VCM5883, MAG_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE, 0); +BUSDEV_REGISTER_I2C(busdev_mag3110, DEVHW_MAG3110, MAG_I2C_BUS, 0x0E, NONE, DEVFLAGS_NONE, 0); +BUSDEV_REGISTER_I2C(busdev_ist8310_0, DEVHW_IST8310_0, MAG_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE, 0); +BUSDEV_REGISTER_I2C(busdev_ist8310_1, DEVHW_IST8310_1, MAG_I2C_BUS, 0x0E, NONE, DEVFLAGS_NONE, 0); +BUSDEV_REGISTER_I2C(busdev_ist8308, DEVHW_IST8308, MAG_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE, 0); +BUSDEV_REGISTER_I2C(busdev_mlx90393, DEVHW_MLX90393, MAG_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE, 0); +BUSDEV_REGISTER_I2C(busdev_lis3mdl, DEVHW_LIS3MDL, MAG_I2C_BUS, 0x1E, NONE, DEVFLAGS_NONE, 0); +BUSDEV_REGISTER_I2C(busdev_ak8975, DEVHW_AK8975, MAG_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE, 0); +BUSDEV_REGISTER_I2C(busdev_ak8963, DEVHW_AK8963, MAG_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE, 0); + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 1),//WS2812B +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/HUMMINGBIRD_FC305/target.h b/src/main/target/HUMMINGBIRD_FC305/target.h new file mode 100644 index 00000000000..6fd5fa705f0 --- /dev/null +++ b/src/main/target/HUMMINGBIRD_FC305/target.h @@ -0,0 +1,161 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "HBRD" +#define USBD_PRODUCT_STRING "HUMMINGBIRD_FC305" + +#define USE_TARGET_HARDWARE_DESCRIPTORS +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#define LED0 PA15 +#define BEEPER PC15 +#define BEEPER_INVERTED + +// *** PINIO *** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC14 //9V EN +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED + +// *************** SPI2 Gyro & ACC ******************* +#define USE_SPI +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 +#define SPI2_NSS_PIN PB12 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW0_DEG +#define ICM42605_SPI_BUS BUS_SPI2 +#define ICM42605_CS_PIN SPI2_NSS_PIN + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_BARO +#define USE_BARO_SPL06 +#define SPL06_I2C_ADDR (0x76) +#define SPL06_I2C_BUS BUS_I2C1 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +// *************** SPI3 OSD*********************** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB2 +#define SPI3_SCK_AF GPIO_AF6_SPI3 +#define SPI3_MISO_AF GPIO_AF6_SPI3 +#define SPI3_MOSI_AF GPIO_AF7_SPI3 + +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_CS_PIN PC13 + +// *************** SPI1 Flash_SD *********************** +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 +#define SPI1_NSS_PIN PA4 + +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI1 +#define SDCARD_CS_PIN SPI1_NSS_PIN +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PB7 +#define UART1_TX_PIN NONE + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB10 +#define UART3_TX_PIN NONE + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART6 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL) + +#define USE_RANGEFINDER_MSP +#define USE_OPFLOW +#define USE_OPFLOW_MSP + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define MAX_PWM_OUTPUT_PORTS 4 +#define USE_DSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/JHEMCUF435/CMakeLists.txt b/src/main/target/JHEMCUF435/CMakeLists.txt new file mode 100644 index 00000000000..6cd33fc16a7 --- /dev/null +++ b/src/main/target/JHEMCUF435/CMakeLists.txt @@ -0,0 +1 @@ +target_at32f43x_xGT7(JHEMCUF435) \ No newline at end of file diff --git a/src/main/target/JHEMCUF435/target.c b/src/main/target/JHEMCUF435/target.c new file mode 100644 index 00000000000..b306832024e --- /dev/null +++ b/src/main/target/JHEMCUF435/target.c @@ -0,0 +1,40 @@ +/* +* This file is part of Cleanflight. +* +* Cleanflight 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. +* +* Cleanflight 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 Cleanflight. If not, see . +*/ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/sensor.h" + +timerHardware_t timerHardware[] = { + + DEF_TIM(TMR4, CH1, PB6, TIM_USE_MOTOR, 0, 0), // TMR3_CH3 motor 1 + DEF_TIM(TMR4, CH2, PB7, TIM_USE_MOTOR, 0,10), // TMR3_CH4 motor 2 + DEF_TIM(TMR2, CH4, PA3, TIM_USE_MOTOR, 0, 7), // TMR2_CH4 motor 3 + DEF_TIM(TMR2, CH3, PA2, TIM_USE_MOTOR, 0, 6), // TMR2_CH3 motor 4 + + DEF_TIM(TMR3, CH4, PB1, TIM_USE_LED, 0, 2), // TMR4_CH1 LED_STRIP + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); + diff --git a/src/main/target/JHEMCUF435/target.h b/src/main/target/JHEMCUF435/target.h new file mode 100644 index 00000000000..db165c558b8 --- /dev/null +++ b/src/main/target/JHEMCUF435/target.h @@ -0,0 +1,196 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "J435" + +#define USBD_PRODUCT_STRING "JHEMCUF435" + + +#define LED0 PC13 +#define LED0_INVERTED + +#define BEEPER PC15 +#define BEEPER_INVERTED + +#define ENABLE_DSHOT + +// *************** Gyro & ACC ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 +#define SPI1_NSS_PIN PA4 + +// MPU6500 +#define USE_IMU_MPU6500 +#define IMU_MPU6500_ALIGN CW0_DEG +#define MPU6500_SPI_BUS BUS_SPI1 +#define MPU6500_CS_PIN SPI1_NSS_PIN + +// ICM42605/ICM42688P +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN SPI1_NSS_PIN + +// BMI270 +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW0_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN SPI1_NSS_PIN + +// LSM6DXX +#define USE_IMU_LSM6DXX +#define IMU_LSM6DXX_ALIGN CW0_DEG +#define LSM6DXX_CS_PIN SPI1_NSS_PIN +#define LSM6DXX_SPI_BUS BUS_SPI1 + + +// *************** I2C/Baro/Mag/EXT********************* +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PH2 // SCL pad +#define I2C2_SDA PH3 // SDA pad +#define USE_I2C_PULLUP + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C2 +#define USE_BARO_BMP280 +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_ALL +#define DEFAULT_I2C_BUS BUS_I2C2 + + +// *************** OSD ***************************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 +#define SPI2_NSS_PIN PB12 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN SPI2_NSS_PIN + + +// *************** BLACKBOX ************************** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define M25P16_CS_PIN PA15 +#define M25P16_SPI_BUS BUS_SPI3 +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP +#define USE_USB_DETECT + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +// UART2 TX does not work due currently unknown software issue +// Leaving this info here for future work. +/* +#define USE_UART2 +#define UART2_RX_AF 6 +#define UART2_TX_AF 8 +#define UART2_RX_PIN PB0 +#define UART2_TX_PIN PA8 +*/ + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PC11 +#define UART4_TX_PIN PC10 + +#define USE_UART5 +#define UART5_RX_PIN PB8 +#define UART5_TX_PIN PB9 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define USE_UART7 +//#define USE_UART7_PIN_SWAP +#define UART7_RX_PIN PC0 +#define UART7_TX_PIN PC1 + +#define USE_UART8 +#define UART8_RX_PIN PC3 +#define UART8_TX_PIN PC2 + +#define SERIAL_PORT_COUNT 8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART5 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC1_DMA_STREAM DMA2_CHANNEL5 +#define ADC_CHANNEL_1_PIN PA0 +#define ADC_CHANNEL_2_PIN PA1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +//**************************************************** + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_LED_STRIP ) +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC + + + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PB1 + + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE BIT(2) +#define TARGET_IO_PORTH BIT(1)|BIT(2)|BIT(3) + +#define MAX_PWM_OUTPUT_PORTS 4 + + +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_ESCSERIAL diff --git a/src/main/target/JHEMCUF435AIO/CMakeLists.txt b/src/main/target/JHEMCUF435AIO/CMakeLists.txt new file mode 100644 index 00000000000..17e94271a85 --- /dev/null +++ b/src/main/target/JHEMCUF435AIO/CMakeLists.txt @@ -0,0 +1 @@ +target_at32f43x_xGT7(JHEMF435AIO) diff --git a/src/main/target/JHEMCUF435AIO/target.c b/src/main/target/JHEMCUF435AIO/target.c new file mode 100644 index 00000000000..3df86550d5b --- /dev/null +++ b/src/main/target/JHEMCUF435AIO/target.c @@ -0,0 +1,41 @@ +/* +* This file is part of Cleanflight. +* +* Cleanflight 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. +* +* Cleanflight 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 Cleanflight. If not, see . +*/ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +timerHardware_t timerHardware[] = { + + DEF_TIM(TMR4, CH1, PB6, TIM_USE_MOTOR, 0, 0), // TMR3_CH3 motor 1 + DEF_TIM(TMR4, CH2, PB7, TIM_USE_MOTOR, 0,10), // TMR3_CH4 motor 2 + DEF_TIM(TMR2, CH4, PA3, TIM_USE_MOTOR, 0, 7), // TMR2_CH4 motor 3 + DEF_TIM(TMR2, CH3, PA2, TIM_USE_MOTOR, 0, 6), // TMR2_CH3 motor 4 + + DEF_TIM(TMR3, CH4, PB1, TIM_USE_LED, 0, 2), // TMR4_CH1 LED_STRIP + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); + diff --git a/src/main/target/JHEMCUF435AIO/target.h b/src/main/target/JHEMCUF435AIO/target.h new file mode 100644 index 00000000000..371b2da527e --- /dev/null +++ b/src/main/target/JHEMCUF435AIO/target.h @@ -0,0 +1,177 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "J35A" + +#define USBD_PRODUCT_STRING "JHEMF435AIO" + + +#define LED0 PC13 +#define LED0_INVERTED + +#define BEEPER PC15 +#define BEEPER_INVERTED + +#define ENABLE_DSHOT + +// *************** Gyro & ACC ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 +#define SPI1_NSS_PIN PA4 + +// MPU6500 +#define USE_IMU_MPU6500 +#define IMU_MPU6500_ALIGN CW0_DEG +#define MPU6500_SPI_BUS BUS_SPI1 +#define MPU6500_CS_PIN SPI1_NSS_PIN + +// ICM42605/ICM42688P +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN SPI1_NSS_PIN + +// BMI270 +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW0_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN SPI1_NSS_PIN + +// LSM6DXX +#define USE_IMU_LSM6DXX +#define IMU_LSM6DXX_ALIGN CW0_DEG +#define LSM6DXX_CS_PIN SPI1_NSS_PIN +#define LSM6DXX_SPI_BUS BUS_SPI1 + + +// *************** I2C/Baro/Mag/EXT********************* +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PH2 // SCL pad +#define I2C2_SDA PH3 // SDA pad +#define USE_I2C_PULLUP + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C2 +#define USE_BARO_BMP280 +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_ALL +#define DEFAULT_I2C_BUS BUS_I2C2 + + +// *************** OSD ***************************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 +#define SPI2_NSS_PIN PB12 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN SPI2_NSS_PIN + + +// *************** BLACKBOX ************************** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define M25P16_CS_PIN PA15 +#define M25P16_SPI_BUS BUS_SPI3 +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP +#define USE_USB_DETECT + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_AF 6 +#define UART2_TX_AF 8 +#define UART2_RX_PIN PB0 +#define UART2_TX_PIN PA8 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART5 +#define UART5_RX_PIN PB8 +#define UART5_TX_PIN PB9 + + +#define SERIAL_PORT_COUNT 5 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART5 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC1_DMA_STREAM DMA2_CHANNEL5 +#define ADC_CHANNEL_1_PIN PA0 +#define ADC_CHANNEL_2_PIN PA1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +//**************************************************** + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_LED_STRIP ) +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC + + + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PB1 + + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE BIT(2) +#define TARGET_IO_PORTH BIT(1)|BIT(2)|BIT(3) + +#define MAX_PWM_OUTPUT_PORTS 4 + + +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_ESCSERIAL +#define USE_RPM_FILTER diff --git a/src/main/target/NEXUSX/CMakeLists.txt b/src/main/target/NEXUSX/CMakeLists.txt new file mode 100644 index 00000000000..c83b426b374 --- /dev/null +++ b/src/main/target/NEXUSX/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(NEXUSX) diff --git a/src/main/target/NEXUSX/README.md b/src/main/target/NEXUSX/README.md new file mode 100644 index 00000000000..be7d8fd1e63 --- /dev/null +++ b/src/main/target/NEXUSX/README.md @@ -0,0 +1,64 @@ +Radiomaster NEXUS X and NEXUS XR +================================ + +Flight controllers originally designed for helicopters using Rotorflight. +Based on STM32F722RET6. Both NEXUS X and XR share the same target in iNav. + +Built-in periperals +------------------- + +Both models contain a ICM42688P IMU, a SPL06 barometer and a W25N02KVZEIR blackbox memory chip. + +The NEXUS XR also contains a serial ELRS receiver based on the RP4TD-M using ESP32 and two SX1281. +It is connected to the main STM32F7 Flight Controller on UART5. +None of the external connections route to the receiver, they are all connected to the STM32F7 Flight Controller. +The receiver can be disabled using USER1, which controls a pinio on pin PC8. + +Pin configuration +----------------- + +The RPM, TLM, AUX and SBUS pins are Servo/Motor outputs by default. However, when UART1 or UART2 are assigned a function in the ports tab, the pins will become a UART instead. See the table below. + +| Marking on the case | Both UART1 and UART2 unused | UART1 in use | UART2 in use | Both UART1 and UART2 in use | +|---------------------|----------------------------------------------------------------------------------------|----------------------------------------------------------------------------------------|----------------------------------------------------------------------------------------|----------------------------------------------------------------------------------------| +| S1 | Output S1 | Output S1 | Output S1 | Output S1 | +| S2 | Output S2 | Output S2 | Output S2 | Output S2 | +| S3 | Output S3 | Output S3 | Output S3 | Output S3 | +| TAIL | Output S4 | Output S4 | Output S4 | Output S4 | +| ESC | Output S5 | Output S5 | Output S5 | Output S5 | +| RPM | Output S6 | Output S6 | UART2 TX | UART2 TX | +| TLM | Output S7 | Output S7 | UART2 RX | UART2 RX | +| AUX | Output S8 | UART1 TX | Output S6 | UART1 TX | +| SBUS | Output S9 | UART1 RX | Output S7 | UART1 RX | +| A | UART4
pin order:
TX, RX, 5V, GND | UART4
pin order:
TX, RX, 5V, GND | UART4
pin order:
TX, RX, 5V, GND | UART4
pin order:
TX, RX, 5V, GND | +| B | UART6
pin order:
TX, RX, 5V, GND | UART6
pin order:
TX, RX, 5V, GND | UART6
pin order:
TX, RX, 5V, GND | UART6
pin order:
TX, RX, 5V, GND | +| C | I2C
pin order:
SCL, SDA, 5V, GND
or
UART3
pin order:
TX, RX, 5V, GND | I2C
pin order:
SCL, SDA, 5V, GND
or
UART3
pin order:
TX, RX, 5V, GND | I2C
pin order:
SCL, SDA, 5V, GND
or
UART3
pin order:
TX, RX, 5V, GND | I2C
pin order:
SCL, SDA, 5V, GND
or
UART3
pin order:
TX, RX, 5V, GND | +| EXT-V | battery voltage
max 60V
pin order:
Vbat, GND | battery voltage
max 60V
pin order:
Vbat, GND | battery voltage
max 60V
pin order:
Vbat, GND | battery voltage
max 60V
pin order:
Vbat, GND | +| built-in ELRS | UART5 | UART5 | UART5 | UART5 | + +All pin orders are from left to right, when looking at the connector on the flight controller. +**Note that the pin order for "A", "B" and "C" is incorrect on radiomaster's website, it has RX and TX swapped.** + +Hardware layout +--------------- + + +| Marking on the case | STM32 pin | Servo | UART | I2C | +|---------------------|-----------|------------------------------:|---------------:|---------:| +| S1 | PB4 | TIM3CH1 | n/a | n/a | +| S2 | PB5 | TIM3CH2 | n/a | n/a | +| S3 | PB0 | TIM3CH3 | n/a | n/a | +| TAIL | PA15 | TIM2CH1 | n/a | n/a | +| ESC | PA9 | TIM1CH2 | UART1 TX | n/a | +| RPM | PA2 | TIM2CH3
TIM5CH3
TIM9CH1 | UART2 TX | n/a | +| TLM | PA3 | TIM2CH4
TIM5CH4
TIM9CH2 | UART2 RX | n/a | +| AUX | PB6 | TIM4CH1 | UART1 TX | I2C1 SCL | +| SBUS | PB7 | TIM4CH2 | UART1 RX | I2C1 SDA | +| A | PA1/PA0 | TIM1
TIM5 | UART2
UART4 | n/a | +| B | PC7/PC6 | TIM3
TIM8 | UART6 | n/a | +| C | PB11/PB10 | TIM2 | UART3 | I2C2 | +| EXT-V | PC0 | n/a | n/a | n/a | +| built-in ELRS | PC12/PD2 | n/a | UART5 | n/a | + +The pinout is extremely similar to the F7C reference design from Rotorflight. +https://github.com/rotorflight/rotorflight-ref-design/blob/master/Reference-Design-F7C.md \ No newline at end of file diff --git a/src/main/target/NEXUSX/config.c b/src/main/target/NEXUSX/config.c new file mode 100644 index 00000000000..0d5b4a88276 --- /dev/null +++ b/src/main/target/NEXUSX/config.c @@ -0,0 +1,42 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute 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 http://www.gnu.org/licenses/. + */ + +#include + +#include "platform.h" + +#include "drivers/pwm_mapping.h" + +#include "fc/fc_msp_box.h" + +#include "io/piniobox.h" +#include "io/serial.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + + // default "ESC" pin to be a motor + timerOverridesMutable(timer2id(TIM1))->outputMode = OUTPUT_MODE_MOTORS; +} diff --git a/src/main/target/NEXUSX/target.c b/src/main/target/NEXUSX/target.c new file mode 100644 index 00000000000..799dc762322 --- /dev/null +++ b/src/main/target/NEXUSX/target.c @@ -0,0 +1,43 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "target.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "S1" + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "S2" + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "S3" + + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "TAIL" + + DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "ESC" + + DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "RPM", clashes with UART2 TX + DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "TLM", clashes with UART2 RX + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "AUX", clashes with UART1 TX and I2C1 SCL + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // labelled "SBUS", clashes with UART1 RX and I2C1 SDA +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/NEXUSX/target.h b/src/main/target/NEXUSX/target.h new file mode 100644 index 00000000000..24c035b4cae --- /dev/null +++ b/src/main/target/NEXUSX/target.h @@ -0,0 +1,159 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "F7C5" + +#define USBD_PRODUCT_STRING "NEXUSX" + +#define LED0 PC10 +#define LED1 PC11 + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_ICM42605 // is actually ICM42688P +#define IMU_ICM42605_ALIGN CW180_DEG +#define ICM42605_CS_PIN PA4 +#define ICM42605_EXTI_PIN PB8 +#define ICM42605_SPI_BUS BUS_SPI1 + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_3 +#define I2C3_SCL PA8 +#define I2C3_SDA PC9 + +//#define USE_I2C_DEVICE_1 // clashes with UART1 +//#define I2C1_SCL PB6 +//#define I2C1_SDA PB7 + +#define USE_I2C_DEVICE_2 // clashes with UART3 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 +#define I2C_DEVICE_2_SHARES_UART3 +#define DEFAULT_I2C BUS_I2C2 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C3 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS DEFAULT_I2C + +#define PITOT_I2C_BUS DEFAULT_I2C + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS DEFAULT_I2C + +// *************** SPI2 Blackbox ******************* +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_FLASHFS +#define USE_FLASH_W25N02K +#define W25N02K_SPI_BUS BUS_SPI2 +#define W25N02K_CS_PIN PB12 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 // clashes with I2C1 +#define UART1_TX_PIN PB6 // pin labelled "AUX" +#define UART1_RX_PIN PB7 // pin labelled "SBUS" + +#define USE_UART2 +#define UART2_TX_PIN PA2 // pin labelled as "RPM" +#define UART2_RX_PIN PA3 // pin labelled as "TLM" + +#define USE_UART3 +// port labelled "C" +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define USE_UART4 +// port labelled "A" +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +// port for NEXUS XR internal ELRS receiver +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_UART6 +// port labelled "B" +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART5 + +#define SENSORS_SET (SENSOR_ACC|SENSOR_BARO) + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 + +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC0 + +#define VBAT_ADC_CHANNEL ADC_CHN_3 // port labelled "EXT-V" +//BEC ADC is ADC_CHN_2 +//BUS ADC is ADC_CHN_1 + +#define VBAT_SCALE_DEFAULT 2474 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC8 // enable pin for internal ELRS receiver +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED // turn on by default + +#define DEFAULT_FEATURES (FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_TX_PROF_SEL) + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define MAX_PWM_OUTPUT_PORTS 9 + +#define USE_DSHOT +#define USE_SERIALSHOT +#define USE_ESC_SENSOR +#define USE_SMARTPORT_MASTER // no internal current sensor, enable SMARTPORT_MASTER so external ones can be used + +#define USE_DSHOT_DMAR diff --git a/src/main/target/OMNIBUSF4/CMakeLists.txt b/src/main/target/OMNIBUSF4/CMakeLists.txt index a6ccb483bf5..0e2bf2002c0 100644 --- a/src/main/target/OMNIBUSF4/CMakeLists.txt +++ b/src/main/target/OMNIBUSF4/CMakeLists.txt @@ -9,3 +9,4 @@ target_stm32f405xg(OMNIBUSF4V3_S6_SS) # OMNIBUSF4V3 is a (almost identical) variant of OMNIBUSF4PRO target, # except for an inverter on UART6. target_stm32f405xg(OMNIBUSF4V3) +target_stm32f405xg(OMNIBUSF4V3_ICM SKIP_RELEASES) diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 2c5a27afbb6..fcd89fb0e46 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -30,6 +30,8 @@ #define TARGET_BOARD_IDENTIFIER "OBSD" #elif defined(OMNIBUSF4V3) #define TARGET_BOARD_IDENTIFIER "OB43" +#elif defined(OMNIBUSF4V3_ICM) +#define TARGET_BOARD_IDENTIFIER "OB4I" #elif defined(DYSF4PRO) #define TARGET_BOARD_IDENTIFIER "DYS4" #elif defined(DYSF4PROV2) @@ -67,7 +69,14 @@ #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_BUS BUS_SPI1 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) +#if defined(OMNIBUSF4V3_ICM) + #define USE_IMU_ICM42605 + #define IMU_ICM42605_ALIGN CW180_DEG + #define ICM42605_CS_PIN PA4 + #define ICM42605_SPI_BUS BUS_SPI1 +#endif + +#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_ICM) #define USE_IMU_MPU6000 #define IMU_MPU6000_ALIGN CW270_DEG #else @@ -75,8 +84,8 @@ #define IMU_MPU6000_ALIGN CW180_DEG #endif -// Support for OMNIBUS F4 PRO CORNER - it has ICM20608 instead of MPU6000 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) +// Support for OMNIBUS F4 PRO CORNER - it has MPU6500/ICM20608 instead of MPU6000 +#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_ICM) #define MPU6500_CS_PIN MPU6000_CS_PIN #define MPU6500_SPI_BUS MPU6000_SPI_BUS #define USE_IMU_MPU6500 @@ -97,7 +106,7 @@ #define USE_BARO -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) +#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_ICM) #define USE_BARO_BMP280 #define BMP280_SPI_BUS BUS_SPI3 #define BMP280_CS_PIN PB3 // v1 @@ -121,7 +130,7 @@ #define VBUS_SENSING_PIN PC5 #define VBUS_SENSING_ENABLED -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) +#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_ICM) #define USE_UART_INVERTER #endif @@ -140,12 +149,12 @@ #define USE_UART6 #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 -#if defined(OMNIBUSF4V3) +#if defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_ICM) #define INVERTER_PIN_UART6_RX PC8 #define INVERTER_PIN_UART6_TX PC9 #endif -#if defined(OMNIBUSF4V3) && !(defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)) +#if (defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_ICM)) && !(defined(OMNIBUSF4V3_S6_SS) || defined(OMNIBUSF4V3_S5S6_SS) || defined(OMNIBUSF4V3_S5_S6_2SS)) #define USE_SOFTSERIAL1 #define SOFTSERIAL_1_RX_PIN PC6 // shared with UART6 TX #define SOFTSERIAL_1_TX_PIN PC6 // shared with UART6 TX @@ -193,7 +202,7 @@ #define USE_SPI_DEVICE_1 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) +#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_ICM) #define USE_SPI_DEVICE_2 #define SPI2_NSS_PIN PB12 #define SPI2_SCK_PIN PB13 @@ -202,7 +211,7 @@ #endif #define USE_SPI_DEVICE_3 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) +#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_ICM) #define SPI3_NSS_PIN PA15 #else #define SPI3_NSS_PIN PB3 @@ -215,7 +224,7 @@ #define MAX7456_SPI_BUS BUS_SPI3 #define MAX7456_CS_PIN PA15 -#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) +#if defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_ICM) #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT #define USE_SDCARD #define USE_SDCARD_SPI @@ -250,7 +259,7 @@ #define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) #define USE_LED_STRIP -#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !defined(OMNIBUSF4PRO_LEDSTRIPM5) +#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3) || defined(OMNIBUSF4V3_ICM)) && !defined(OMNIBUSF4PRO_LEDSTRIPM5) #define WS2811_PIN PB6 #else #define WS2811_PIN PA1 diff --git a/src/main/target/ORBITH743/CMakeLists.txt b/src/main/target/ORBITH743/CMakeLists.txt new file mode 100644 index 00000000000..4445a9c6905 --- /dev/null +++ b/src/main/target/ORBITH743/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(ORBITH743) diff --git a/src/main/target/ORBITH743/config.c b/src/main/target/ORBITH743/config.c new file mode 100644 index 00000000000..80de8a800ec --- /dev/null +++ b/src/main/target/ORBITH743/config.c @@ -0,0 +1,51 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight 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. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include +#include +#include +#include "fc/fc_msp_box.h" +#include "io/piniobox.h" +#include "io/serial.h" +#include "common/axis.h" +#include "config/config_master.h" +#include "config/feature.h" +#include "drivers/sensor.h" +#include "drivers/pwm_esc_detect.h" +#include "drivers/pwm_output.h" +#include "drivers/pwm_mapping.h" +#include "drivers/serial.h" +#include "fc/rc_controls.h" +#include "flight/failsafe.h" +#include "flight/mixer.h" +#include "flight/pid.h" +#include "rx/rx.h" +#include "io/serial.h" +#include "sensors/battery.h" +#include "sensors/sensors.h" +#include "telemetry/telemetry.h" + + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_ESCSERIAL; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART6)].functionMask = FUNCTION_GPS; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART5)].functionMask = FUNCTION_RX_SERIAL; +} diff --git a/src/main/target/ORBITH743/target.c b/src/main/target/ORBITH743/target.c new file mode 100644 index 00000000000..59478412c50 --- /dev/null +++ b/src/main/target/ORBITH743/target.c @@ -0,0 +1,54 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_1, DEVHW_ICM42605, ICM42688_SPI_BUS_1, ICM42688_CS_PIN_1, NONE, 0, DEVFLAGS_NONE, IMU_ICM42688_ALIGN_1); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_2, DEVHW_ICM42605, ICM42688_SPI_BUS_2, ICM42688_CS_PIN_2, NONE, 0, DEVFLAGS_NONE, IMU_ICM42688_ALIGN_2); + + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 + + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DMA_NONE + DEF_TIM(TIM4, CH1, PD12, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 DMA_NONE + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 14), // LED_2812 + DEF_TIM(TIM12, CH2, PB15, TIM_USE_PPM, 0, 0), // BEEPER PWM + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ORBITH743/target.h b/src/main/target/ORBITH743/target.h new file mode 100644 index 00000000000..674240b85b0 --- /dev/null +++ b/src/main/target/ORBITH743/target.h @@ -0,0 +1,200 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "ORB7" +#define USBD_PRODUCT_STRING "ORBITH743" + +#define USE_TARGET_CONFIG + +#define LED0 PE3 +#define LED1 PE4 + +#define BEEPER PB7 +#define BEEPER_INVERTED + + +// *************** SPI ***************************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 +#define USE_SPI_DEVICE_3 +#define USE_SPI_DEVICE_4 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define SPI2_SCK_PIN PB10 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + +// *************** I2C ************************** +#define USE_I2C +#define USE_I2C_DEVICE_1 + +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +// *************** UART ***************************** +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 +#define USE_UART6 +#define USE_UART7 +#define USE_UART8 + +#define UART1_TX_PIN PB14 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 + +#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 + +#define UART4_TX_PIN PD1 +#define UART4_RX_PIN PD0 + +#define UART5_TX_PIN PB13 +#define UART5_RX_PIN PB12 + +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define UART7_TX_PIN PE8 +#define UART7_RX_PIN PE7 + +#define UART8_TX_PIN PE1 +#define UART8_RX_PIN PE0 + +#define SERIAL_PORT_COUNT 9 //VCP, UART1, UART2, UART3, UART4, UART5, UART6, UART7, UART8 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART5 + + +// *************** Gyro & ACC ********************** +#define USE_DUAL_GYRO +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#define USE_IMU_ICM42605 + +#define IMU_ICM42688_ALIGN_1 CW270_DEG +#define ICM42688_CS_PIN_1 PE11 +#define ICM42688_SPI_BUS_1 BUS_SPI1 +#define ICM42688_EXTI_PIN_1 PE10 + +#define IMU_ICM42688_ALIGN_2 CW270_DEG +#define ICM42688_CS_PIN_2 PE9 +#define ICM42688_SPI_BUS_2 BUS_SPI4 +#define ICM42688_EXTI_PIN_2 PB2 + + +// *************** OSD ***************************** +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PE6 + +// *************** FLASH *************************** +#define W25N02K_SPI_BUS BUS_SPI3 +#define W25N02K_CS_PIN PD3 + +#define USE_BLACKBOX +#define USE_FLASHFS +#define USE_FLASH_W25N02K +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** Baro/Mag ********************* + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +//#define DPS310_I2C_ADDR 0x77 // 0x77 is for test board +#define DPS310_I2C_ADDR 0x76 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 + + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC_CHANNEL_1_PIN PC0 //ADC123 VBAT1 +#define ADC_CHANNEL_2_PIN PC1 //ADC123 CURR1 +#define ADC_CHANNEL_3_PIN PC5 //ADC12 RSSI +#define ADC_CHANNEL_4_PIN PC4 //ADC12 VBAT2 +#define ADC_CHANNEL_5_PIN PA4 //ADC12 CURR2 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PE2 // VTX power switcher +#define PINIO2_PIN PD2 // 2xCamera switcher +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED +#define PINIO2_FLAGS PINIO_FLAGS_INVERTED + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 125 +#define VBAT_SCALE_DEFAULT 1010 + + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define MAX_PWM_OUTPUT_PORTS 15 +#define USE_DSHOT +#define USE_ESC_SENSOR diff --git a/src/main/target/RADIOLINKF405/CMakeLists.txt b/src/main/target/RADIOLINKF405/CMakeLists.txt new file mode 100644 index 00000000000..66c4ee4b9c8 --- /dev/null +++ b/src/main/target/RADIOLINKF405/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(RADIOLINKF405) diff --git a/src/main/target/RADIOLINKF405/config.c b/src/main/target/RADIOLINKF405/config.c new file mode 100644 index 00000000000..99af52f35b4 --- /dev/null +++ b/src/main/target/RADIOLINKF405/config.c @@ -0,0 +1,27 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include "platform.h" +#include "fc/fc_msp_box.h" +#include "fc/config.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; +} diff --git a/src/main/target/RADIOLINKF405/target.c b/src/main/target/RADIOLINKF405/target.c new file mode 100644 index 00000000000..8216ab069de --- /dev/null +++ b/src/main/target/RADIOLINKF405/target.c @@ -0,0 +1,40 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/sensor.h" +#include "drivers/timer_def_stm32f4xx.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), + + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), + + DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), // 2812LED D(1,2,5) + +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/RADIOLINKF405/target.h b/src/main/target/RADIOLINKF405/target.h new file mode 100644 index 00000000000..6665a46a4f8 --- /dev/null +++ b/src/main/target/RADIOLINKF405/target.h @@ -0,0 +1,169 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once +#define USE_TARGET_CONFIG + +#define TARGET_BOARD_IDENTIFIER "RALI" +#define USBD_PRODUCT_STRING "RADIOLINKF405" + +#define LED0 PB0 //Blue + +#define BEEPER PB8 +#define BEEPER_INVERTED + +// *************** SPI1 IMU & OSD ******************* +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 +#define SPI1_NSS_PIN PA4 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW180_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN SPI1_NSS_PIN + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW90_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN SPI1_NSS_PIN + +#define USE_IMU_MPU6500 +#define IMU_MPU6500_ALIGN CW90_DEG +#define MPU6500_SPI_BUS BUS_SPI1 +#define MPU6500_CS_PIN SPI1_NSS_PIN + +// *************** SPI2 OSD **************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 +#define SPI2_NSS_PIN PB12 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN SPI2_NSS_PIN + +// *************** SPI3 Flash/SD Card **************** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 +#define SPI3_NSS_PIN PC0 + +#define W25N01G_SPI_BUS BUS_SPI3 +#define W25N01G_CS_PIN SPI3_NSS_PIN +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN SPI3_NSS_PIN + +#define USE_BLACKBOX +#define USE_FLASHFS +#define USE_FLASH_W25N01G +#define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC *************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream4 +#define ADC_CHANNEL_1_PIN PC1 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC3 +#define VBAT_ADC_CHANNEL ADC_CHN_2 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC5 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PB1 + +// *************** others ************************ +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define VBAT_SCALE_DEFAULT 1100 +#define CURRENT_METER_SCALE 150 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 8 + +#define USE_SERIAL_3WAY_BLHELI_INTERFACE +#define USE_DSHOT +#define USE_DSHOT_DMAR +#define USE_ESC_SENSOR + diff --git a/src/main/target/SITL/sim/realFlight.c b/src/main/target/SITL/sim/realFlight.c index a8d241425d1..a88d20a612d 100644 --- a/src/main/target/SITL/sim/realFlight.c +++ b/src/main/target/SITL/sim/realFlight.c @@ -344,9 +344,9 @@ static void exchangeData(void) altitude, (int16_t)roundf(rfValues.m_groundspeed_MPS * 100), course, - 0,//(int16_t)roundf(rfValues.m_velocityWorldV_MPS * 100), //not sure about the direction - 0,//(int16_t)roundf(-rfValues.m_velocityWorldU_MPS * 100), - 0,//(int16_t)roundf(rfValues.m_velocityWorldW_MPS * 100), + (int16_t)roundf(rfValues.m_velocityWorldV_MPS * 100), //direction seems ok + (int16_t)roundf(-rfValues.m_velocityWorldU_MPS * 100),//direction seems ok + (int16_t)roundf(rfValues.m_velocityWorldW_MPS * 100),//direction not sure 0 ); diff --git a/src/main/target/SITL/sim/xplane.c b/src/main/target/SITL/sim/xplane.c index d629c2130e3..612f4a54a0a 100644 --- a/src/main/target/SITL/sim/xplane.c +++ b/src/main/target/SITL/sim/xplane.c @@ -70,7 +70,7 @@ static struct sockaddr_storage serverAddr; static socklen_t serverAddrLen; static int sockFd; static pthread_t listenThread; -static bool initalized = false; +static bool initialized = false; static bool useImu = false; static float lattitude = 0; @@ -426,11 +426,11 @@ static void* listenWorker(void* arg) constrainToInt16(north.z * 1024.0f) ); - if (!initalized) { + if (!initialized) { ENABLE_ARMING_FLAG(SIMULATOR_MODE_SITL); // Aircraft can wobble on the runway and prevents calibration of the accelerometer ENABLE_STATE(ACCELEROMETER_CALIBRATED); - initalized = true; + initialized = true; } unlockMainPID(); @@ -480,7 +480,7 @@ bool simXPlaneInit(char* ip, int port, uint8_t* mapping, uint8_t mapCount, bool return false; } - while (!initalized) { + while (!initialized) { registerDref(DREF_LATITUDE, "sim/flightmodel/position/latitude", 100); registerDref(DREF_LONGITUDE, "sim/flightmodel/position/longitude", 100); registerDref(DREF_ELEVATION, "sim/flightmodel/position/elevation", 100); diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c index e473c636981..bb34f2cd665 100644 --- a/src/main/target/SITL/target.c +++ b/src/main/target/SITL/target.c @@ -50,6 +50,7 @@ #include "drivers/pwm_mapping.h" #include "drivers/timer.h" #include "drivers/serial.h" +#include "drivers/serial_tcp.h" #include "config/config_streamer.h" #include "build/version.h" @@ -209,6 +210,7 @@ void printCmdLineOptions(void) fprintf(stderr, "--stopbits=[None|One|Two] Serial receiver stopbits (default: One).\n"); fprintf(stderr, "--parity=[Even|None|Odd] Serial receiver parity (default: None).\n"); fprintf(stderr, "--fcproxy Use inav/betaflight FC as a proxy for serial receiver.\n"); + fprintf(stderr, "--tcpbaseport=[port] Base TCP port for UART sockets (default: 5760)\n"); fprintf(stderr, "--chanmap=[mapstring] Channel mapping. Maps INAVs motor and servo PWM outputs to the virtual receiver output in the simulator.\n"); fprintf(stderr, " The mapstring has the following format: M(otor)|S(servo)-,... All numbers must have two digits\n"); fprintf(stderr, " For example: Map motor 1 to virtal receiver output 1, servo 1 to output 2 and servo 2 to output 3:\n"); @@ -239,6 +241,7 @@ void parseArguments(int argc, char *argv[]) {"stopbits", required_argument, 0, '3'}, {"parity", required_argument, 0, '4'}, {"fcproxy", no_argument, 0, '5'}, + {"tcpbaseport", required_argument, 0, '6'}, {NULL, 0, NULL, 0} }; @@ -324,6 +327,16 @@ void parseArguments(int argc, char *argv[]) case '5': serialFCProxy = true; break; + case '6': { + char *endptr = NULL; + long basePort = strtol(optarg, &endptr, 10); + if ((endptr == NULL) || (*endptr != '\0') || basePort <= 0 || basePort > UINT16_MAX || basePort + SERIAL_PORT_COUNT - 1 > UINT16_MAX) { + fprintf(stderr, "[tcpbaseport] Invalid argument\n."); + exit(0); + } + tcpBasePort = (uint16_t)basePort; + break; + } default: printCmdLineOptions(); diff --git a/src/main/target/SKYSTARSF405V2/CMakeLists.txt b/src/main/target/SKYSTARSF405V2/CMakeLists.txt new file mode 100644 index 00000000000..c02bee3acf5 --- /dev/null +++ b/src/main/target/SKYSTARSF405V2/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(SKYSTARSF405V2) diff --git a/src/main/target/SKYSTARSF405V2/config.c b/src/main/target/SKYSTARSF405V2/config.c new file mode 100644 index 00000000000..7f747d11d21 --- /dev/null +++ b/src/main/target/SKYSTARSF405V2/config.c @@ -0,0 +1,29 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include "platform.h" +#include "fc/fc_msp_box.h" +#include "fc/config.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + beeperConfigMutable()->pwmMode = false; +} diff --git a/src/main/target/SKYSTARSF405V2/target.c b/src/main/target/SKYSTARSF405V2/target.c new file mode 100644 index 00000000000..22b353bcc60 --- /dev/null +++ b/src/main/target/SKYSTARSF405V2/target.c @@ -0,0 +1,36 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#include +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // M3 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // M4 + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_LED, 0, 0), // LED +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SKYSTARSF405V2/target.h b/src/main/target/SKYSTARSF405V2/target.h new file mode 100644 index 00000000000..1fe4e2614c4 --- /dev/null +++ b/src/main/target/SKYSTARSF405V2/target.h @@ -0,0 +1,160 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + */ + +#pragma once +#define USE_TARGET_CONFIG + + +#define TARGET_BOARD_IDENTIFIER "SK4V2" +#define USBD_PRODUCT_STRING "SKYSTARSF405V2" + +#define LED0 PA14 +#define LED1 PA13 + +#define BEEPER PC13 +#define BEEPER_INVERTED + +// *************** SPI1 IMU ******************* +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define ICM42605_CS_PIN PA4 +#define ICM42605_SPI_BUS BUS_SPI1 +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW270_DEG + +// *************** SPI2 OSD ******************* +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** SPI3 Flash **************** +#define USE_FLASHFS +#define USE_FLASH_M25P16 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PA15 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +#define USE_I2C_DEVICE_3 +#define I2C3_SCL PA8 +#define I2C3_SDA PC9 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C3 +#define USE_BARO_SPL06 + + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC *************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC2 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO2_PIN PC3 + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PC8 + +// *************** others ************************ +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define VBAT_SCALE_DEFAULT 1100 +#define CURRENT_METER_SCALE 150 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define MAX_PWM_OUTPUT_PORTS 7 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_DSHOT +#define USE_DSHOT_DMAR +#define USE_ESC_SENSOR diff --git a/src/main/target/SKYSTARSH743HD/target.c b/src/main/target/SKYSTARSH743HD/target.c index 296fc51bd35..bbf9e3fb1df 100644 --- a/src/main/target/SKYSTARSH743HD/target.c +++ b/src/main/target/SKYSTARSH743HD/target.c @@ -27,7 +27,10 @@ #include "drivers/sensor.h" BUSDEV_REGISTER_SPI_TAG(busdev_bmi270_1, DEVHW_BMI270, IMU1_SPI_BUS, IMU1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU1_ALIGN); -BUSDEV_REGISTER_SPI_TAG(busdev_bmi270_2, DEVHW_BMI270, IMU2_SPI_BUS, IMU2_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU2_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_bmi270_2, DEVHW_BMI270, IMU2_SPI_BUS, IMU2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU2_ALIGN); + +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_1, DEVHW_ICM42605, IMU1_SPI_BUS, IMU1_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU1_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_icm42688_2, DEVHW_ICM42605, IMU2_SPI_BUS, IMU2_CS_PIN, NONE, 1, DEVFLAGS_NONE, IMU2_ALIGN); timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 diff --git a/src/main/target/SKYSTARSH743HD/target.h b/src/main/target/SKYSTARSH743HD/target.h index 6e830664c5d..f047cdd1e50 100644 --- a/src/main/target/SKYSTARSH743HD/target.h +++ b/src/main/target/SKYSTARSH743HD/target.h @@ -35,6 +35,7 @@ #define USE_DUAL_GYRO #define USE_TARGET_IMU_HARDWARE_DESCRIPTORS #define USE_IMU_BMI270 +#define USE_IMU_ICM42605 // *****IMU1 BMI270 ON SPI4 ************** #define IMU1_ALIGN CW0_DEG #define IMU1_SPI_BUS BUS_SPI4 diff --git a/src/main/target/WARPF7/CMakeLists.txt b/src/main/target/WARPF7/CMakeLists.txt new file mode 100644 index 00000000000..8b624676a52 --- /dev/null +++ b/src/main/target/WARPF7/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(WARPF7) diff --git a/src/main/target/WARPF7/config.c b/src/main/target/WARPF7/config.c new file mode 100644 index 00000000000..cd5aa30d718 --- /dev/null +++ b/src/main/target/WARPF7/config.c @@ -0,0 +1,33 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + +} + diff --git a/src/main/target/WARPF7/target.c b/src/main/target/WARPF7/target.c new file mode 100644 index 00000000000..0d5e35b9255 --- /dev/null +++ b/src/main/target/WARPF7/target.c @@ -0,0 +1,62 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +//#include "drivers/sensor.h" + +// Format: DEF_TIM (tim, ch, pin, usage, flags, dmavar) +timerHardware_t timerHardware[] = { + // DEF_TIM(TIM1, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM8, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), + + // DEF_TIM(TIM1, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), + //DEF_TIM(TIM8, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), + + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 + + DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 + + // DEF_TIM(TIM3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), + + DEF_TIM(TIM3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 + //DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), + + // DEF_TIM(TIM3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0, 0), + + // DEF_TIM(TIM3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), + + // DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/WARPF7/target.h b/src/main/target/WARPF7/target.h new file mode 100644 index 00000000000..e06d6e2ae1d --- /dev/null +++ b/src/main/target/WARPF7/target.h @@ -0,0 +1,148 @@ +/* + * This file is part of INAV. + * + * INAV 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. + * + * INAV 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 INAV. If not, see . + * + * This target has been autgenerated by bf2inav.py + */ + +#pragma once + +//#define USE_TARGET_CONFIG + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY ) + +#define TARGET_BOARD_IDENTIFIER "WARP" +#define USBD_PRODUCT_STRING "WARPF7" + +// Beeper +#define USE_BEEPER +#define BEEPER PA15 +#define BEEPER_INVERTED + +// Leds +#define USE_LED_STRIP +#define WS2811_PIN PA8 +#define LED0 PC3 +#define LED1 PB2 + +// UARTs +#define USB_IO +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PC11 +#define UART3_TX_PIN PC10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF + +// SPI +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA7 +#define SPI1_MOSI_PIN PA6 +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB15 +#define SPI2_MOSI_PIN PB14 +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB5 +#define SPI3_MOSI_PIN PB4 + +// I2C +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +// MAG +#define USE_MAG +#define USE_MAG_ALL +#define MAG_I2C_BUS BUS_I2C1 + +// ADC +#define ADC_CHANNEL_1_PIN PC2 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define ADC_CHANNEL_2_PIN PC1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define ADC_CHANNEL_3_PIN PC0 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define USE_ADC +#define ADC_INSTANCE ADC1 + + +// Gyro & ACC +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW270_DEG +#define ICM42605_CS_PIN PA4 +#define ICM42605_SPI_BUS BUS_SPI1 + +// BARO +#define USE_BARO +#define USE_BARO_BMP280 +#define USE_BARO_SPI_BMP280 +#define BMP280_SPI_BUS BUS_SPI3 +#define BMP280_CS_PIN PC13 + +// OSD +#define USE_MAX7456 +#define MAX7456_CS_PIN PB12 +#define MAX7456_SPI_BUS BUS_SPI2 + +// Blackbox +#define USE_FLASHFS +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PB6 + +// PINIO +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PC14 +#define PINIO2_PIN PC15 + +// Others +#define MAX_PWM_OUTPUT_PORTS 8 +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define TARGET_IO_PORTA 0xFFFF +#define TARGET_IO_PORTB 0xFFFF +#define TARGET_IO_PORTC 0xFFFF +#define TARGET_IO_PORTD 0xFFFF +#define TARGET_IO_PORTE 0xFFFF +#define TARGET_IO_PORTF 0xFFFF diff --git a/src/main/target/YUPIF4/config.c b/src/main/target/YUPIF4/config.c index 0e82c20e93a..7840ff26fbe 100644 --- a/src/main/target/YUPIF4/config.c +++ b/src/main/target/YUPIF4/config.c @@ -23,7 +23,7 @@ #include "drivers/pwm_output.h" #include "blackbox/blackbox.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "io/serial.h" diff --git a/src/main/target/YUPIF7/config.c b/src/main/target/YUPIF7/config.c index 9d5077edba5..f9af134930c 100644 --- a/src/main/target/YUPIF7/config.c +++ b/src/main/target/YUPIF7/config.c @@ -23,7 +23,7 @@ #include "drivers/pwm_output.h" #include "blackbox/blackbox.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "io/serial.h" diff --git a/src/main/target/common.h b/src/main/target/common.h index af0de4bf30f..45eb12ac4bc 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -155,7 +155,7 @@ #define USE_TELEMETRY_JETIEXBUS // These are rather exotic serial protocols #define USE_RX_MSP -//#define USE_MSP_RC_OVERRIDE +#define USE_MSP_RC_OVERRIDE #define USE_SERIALRX_CRSF #define USE_SERIAL_PASSTHROUGH #define NAV_MAX_WAYPOINTS 120 diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 9d595d36fc8..aac3db564cc 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -213,6 +213,13 @@ BUSDEV_REGISTER_I2C(busdev_qmc5883, DEVHW_QMC5883, QMC5883_I2C_BUS, 0x0D, NONE, DEVFLAGS_NONE, 0); #endif +#if defined(USE_MAG_QMC5883P) + #if !defined(QMC5883P_I2C_BUS) + #define QMC5883P_I2C_BUS MAG_I2C_BUS + #endif + BUSDEV_REGISTER_I2C(busdev_qmc5883p, DEVHW_QMC5883P, QMC5883P_I2C_BUS, 0x2C, NONE, DEVFLAGS_NONE, 0); +#endif + #if defined(USE_MAG_AK8963) #if defined(AK8963_SPI_BUS) BUSDEV_REGISTER_SPI(busdev_ak8963, DEVHW_AK8963, AK8963_SPI_BUS, AK8963_CS_PIN, NONE, DEVFLAGS_NONE, 0); @@ -410,7 +417,11 @@ #endif #if defined(USE_FLASH_W25N01G) - BUSDEV_REGISTER_SPI(busdev_w25n01g, DEVHW_W25N01G, W25N01G_SPI_BUS, W25N01G_CS_PIN, NONE, DEVFLAGS_NONE, 0); + BUSDEV_REGISTER_SPI(busdev_w25n01g, DEVHW_W25N, W25N01G_SPI_BUS, W25N01G_CS_PIN, NONE, DEVFLAGS_NONE, 0); +#endif + +#if defined(USE_FLASH_W25N02K) + BUSDEV_REGISTER_SPI(busdev_w25n02k, DEVHW_W25N, W25N02K_SPI_BUS, W25N02K_CS_PIN, NONE, DEVFLAGS_NONE, 0); #endif #if defined(USE_SDCARD) && defined(USE_SDCARD_SPI) diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 104898c92f5..ccd14d35a4a 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -85,6 +85,7 @@ extern uint8_t __config_end; #define USE_MAG_LIS3MDL #define USE_MAG_MAG3110 #define USE_MAG_QMC5883 +#define USE_MAG_QMC5883P //#if (MCU_FLASH_SIZE > 512) #define USE_MAG_AK8963 diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 8b7f289db20..d86822ada7a 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -107,7 +107,14 @@ bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength) bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn) { - bool requestHandled = false; + static bool replyPending = false; + if (replyPending) { + if (crsfRxIsTelemetryBufEmpty()) { + replyPending = sendMspReply(payloadSize, responseFn); + } + return replyPending; + } + if (!mspRxBuffer.len) { return false; } @@ -115,17 +122,21 @@ bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn) while (true) { const int mspFrameLength = mspRxBuffer.bytes[pos]; if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength)) { - requestHandled |= sendMspReply(payloadSize, responseFn); + if (crsfRxIsTelemetryBufEmpty()) { + replyPending = sendMspReply(payloadSize, responseFn); + } else { + replyPending = true; + } } pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength; ATOMIC_BLOCK(NVIC_PRIO_SERIALUART) { if (pos >= mspRxBuffer.len) { mspRxBuffer.len = 0; - return requestHandled; + return replyPending ; } } } - return requestHandled; + return replyPending; } #endif @@ -258,6 +269,33 @@ static void crsfFrameBatterySensor(sbuf_t *dst) crsfSerialize8(dst, batteryRemainingPercentage); } +const int32_t ALT_MIN_DM = 10000; +const int32_t ALT_THRESHOLD_DM = 0x8000 - ALT_MIN_DM; +const int32_t ALT_MAX_DM = 0x7ffe * 10 - 5; + +/* +0x09 Barometer altitude and vertical speed +Payload: +uint16_t altitude_packed ( dm - 10000 ) +*/ +static void crsfBarometerAltitude(sbuf_t *dst) +{ + int32_t altitude_dm = lrintf(getEstimatedActualPosition(Z) / 10); + uint16_t altitude_packed; + if (altitude_dm < -ALT_MIN_DM) { + altitude_packed = 0; + } else if (altitude_dm > ALT_MAX_DM) { + altitude_packed = 0xfffe; + } else if (altitude_dm < ALT_THRESHOLD_DM) { + altitude_packed = altitude_dm + ALT_MIN_DM; + } else { + altitude_packed = ((altitude_dm + 5) / 10) | 0x8000; + } + sbufWriteU8(dst, CRSF_FRAME_BAROMETER_ALTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); + crsfSerialize8(dst, CRSF_FRAMETYPE_BAROMETER_ALTITUDE); + crsfSerialize16(dst, altitude_packed); +} + typedef enum { CRSF_ACTIVE_ANTENNA1 = 0, CRSF_ACTIVE_ANTENNA2 = 1 @@ -322,22 +360,34 @@ static void crsfFrameFlightMode(sbuf_t *dst) sbufWriteU8(dst, 0); crsfSerialize8(dst, CRSF_FRAMETYPE_FLIGHT_MODE); + static uint8_t hrstSent = 0; + // use same logic as OSD, so telemetry displays same flight text as OSD when armed const char *flightMode = "OK"; if (ARMING_FLAG(ARMED)) { - if (STATE(AIRMODE_ACTIVE)) { - flightMode = "AIR"; - } else { - flightMode = "ACRO"; - } + flightMode = "ACRO"; +#ifdef USE_FW_AUTOLAND + if (FLIGHT_MODE(NAV_FW_AUTOLAND)) { + flightMode = "LAND"; + } else +#endif if (FLIGHT_MODE(FAILSAFE_MODE)) { flightMode = "!FS!"; - } else if (IS_RC_MODE_ACTIVE(BOXHOMERESET) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE)) { + } else if (IS_RC_MODE_ACTIVE(BOXHOMERESET) && hrstSent < 4 && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE)) { flightMode = "HRST"; + hrstSent++; } else if (FLIGHT_MODE(MANUAL_MODE)) { flightMode = "MANU"; +#ifdef USE_GEOZONE + } else if (FLIGHT_MODE(NAV_SEND_TO) && !FLIGHT_MODE(NAV_WP_MODE)) { + flightMode = "GEO"; +#endif + } else if (FLIGHT_MODE(TURTLE_MODE)) { + flightMode = "TURT"; } else if (FLIGHT_MODE(NAV_RTH_MODE)) { - flightMode = "RTH"; + flightMode = isWaypointMissionRTHActive() ? "WRTH" : "RTH"; + } else if (FLIGHT_MODE(NAV_POSHOLD_MODE) && STATE(AIRPLANE)) { + flightMode = "LOTR"; } else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) { flightMode = "HOLD"; } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) { @@ -346,7 +396,7 @@ static void crsfFrameFlightMode(sbuf_t *dst) flightMode = "CRSH"; } else if (FLIGHT_MODE(NAV_WP_MODE)) { flightMode = "WP"; - } else if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) { + } else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) { flightMode = "AH"; } else if (FLIGHT_MODE(ANGLE_MODE)) { flightMode = "ANGL"; @@ -354,10 +404,6 @@ static void crsfFrameFlightMode(sbuf_t *dst) flightMode = "HOR"; } else if (FLIGHT_MODE(ANGLEHOLD_MODE)) { flightMode = "ANGH"; -#ifdef USE_FW_AUTOLAND - } else if (FLIGHT_MODE(NAV_FW_AUTOLAND)) { - flightMode = "LAND"; -#endif } #ifdef USE_GPS } else if (feature(FEATURE_GPS) && navConfig()->general.flags.extra_arming_safety && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) { @@ -367,6 +413,9 @@ static void crsfFrameFlightMode(sbuf_t *dst) flightMode = "!ERR"; } + if (!IS_RC_MODE_ACTIVE(BOXHOMERESET) && hrstSent > 0) + hrstSent = 0; + crsfSerializeData(dst, (const uint8_t*)flightMode, strlen(flightMode)); crsfSerialize8(dst, 0); // zero terminator for string // write in the length @@ -415,6 +464,7 @@ typedef enum { CRSF_FRAME_FLIGHT_MODE_INDEX, CRSF_FRAME_GPS_INDEX, CRSF_FRAME_VARIO_SENSOR_INDEX, + CRSF_FRAME_BAROMETER_ALTITUDE_INDEX, CRSF_SCHEDULE_COUNT_MAX } crsfFrameTypeIndex_e; @@ -425,28 +475,36 @@ static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; static bool mspReplyPending; -void crsfScheduleMspResponse(void) +//Id of the last receiver MSP frame over CRSF. Needed to send response with correct frame ID +static uint8_t mspRequestOriginID = 0; + +void crsfScheduleMspResponse(uint8_t requestOriginID) { mspReplyPending = true; + mspRequestOriginID = requestOriginID; } -void crsfSendMspResponse(uint8_t *payload) +void crsfSendMspResponse(uint8_t *payload, const uint8_t payloadSize) { sbuf_t crsfPayloadBuf; sbuf_t *dst = &crsfPayloadBuf; crsfInitializeFrame(dst); - sbufWriteU8(dst, CRSF_FRAME_TX_MSP_FRAME_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); + sbufWriteU8(dst, payloadSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); crsfSerialize8(dst, CRSF_FRAMETYPE_MSP_RESP); - crsfSerialize8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER); + crsfSerialize8(dst, mspRequestOriginID); crsfSerialize8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); - crsfSerializeData(dst, (const uint8_t*)payload, CRSF_FRAME_TX_MSP_FRAME_SIZE); + crsfSerializeData(dst, (const uint8_t*)payload, payloadSize); crsfFinalize(dst); } #endif static void processCrsf(void) { + if (!crsfRxIsTelemetryBufEmpty()) { + return; // do nothing if telemetry ouptut buffer is not empty yet. + } + static uint8_t crsfScheduleIndex = 0; const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex]; @@ -481,6 +539,11 @@ static void processCrsf(void) crsfFrameVarioSensor(dst); crsfFinalize(dst); } + if (currentSchedule & BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX)) { + crsfInitializeFrame(dst); + crsfBarometerAltitude(dst); + crsfFinalize(dst); + } #endif crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount; } @@ -514,6 +577,11 @@ void initCrsfTelemetry(void) if (sensors(SENSOR_BARO) || (STATE(FIXED_WING_LEGACY) && feature(FEATURE_GPS))) { crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX); } +#endif +#ifdef USE_BARO + if (sensors(SENSOR_BARO)) { + crsfSchedule[index++] = BV(CRSF_FRAME_BAROMETER_ALTITUDE_INDEX); + } #endif crsfScheduleCount = (uint8_t)index; } diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index 6ffa4df3588..bfe9b9e4f3c 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -27,7 +27,7 @@ void initCrsfTelemetry(void); bool checkCrsfTelemetryState(void); void handleCrsfTelemetry(timeUs_t currentTimeUs); void crsfScheduleDeviceInfoResponse(void); -void crsfScheduleMspResponse(void); +void crsfScheduleMspResponse(uint8_t requestOriginID); int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType); #if defined(USE_MSP_OVER_TELEMETRY) void initCrsfMspBuffer(void); diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 92ddeb7a990..d5084b32a62 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -183,7 +183,9 @@ static mavlink_message_t mavSendMsg; static mavlink_message_t mavRecvMsg; static mavlink_status_t mavRecvStatus; +// Set mavSystemId from telemetryConfig()->mavlink.sysid static uint8_t mavSystemId = 1; +static uint8_t mavAutopilotType; static uint8_t mavComponentId = MAV_COMP_ID_AUTOPILOT1; static APM_COPTER_MODE inavToArduCopterMap(flightModeForTelemetry_e flightMode) @@ -196,7 +198,14 @@ static APM_COPTER_MODE inavToArduCopterMap(flightModeForTelemetry_e flightMode) case FLM_HORIZON: return COPTER_MODE_STABILIZE; case FLM_ANGLEHOLD: return COPTER_MODE_STABILIZE; case FLM_ALTITUDE_HOLD: return COPTER_MODE_ALT_HOLD; - case FLM_POSITION_HOLD: return COPTER_MODE_POSHOLD; + case FLM_POSITION_HOLD: + { + if (isGCSValid()) { + return COPTER_MODE_GUIDED; + } else { + return COPTER_MODE_POSHOLD; + } + } case FLM_RTH: return COPTER_MODE_RTL; case FLM_MISSION: return COPTER_MODE_AUTO; case FLM_LAUNCH: return COPTER_MODE_THROW; @@ -226,7 +235,14 @@ static APM_PLANE_MODE inavToArduPlaneMap(flightModeForTelemetry_e flightMode) case FLM_HORIZON: return PLANE_MODE_STABILIZE; case FLM_ANGLEHOLD: return PLANE_MODE_STABILIZE; case FLM_ALTITUDE_HOLD: return PLANE_MODE_FLY_BY_WIRE_B; - case FLM_POSITION_HOLD: return PLANE_MODE_LOITER; + case FLM_POSITION_HOLD: + { + if (isGCSValid()) { + return PLANE_MODE_GUIDED; + } else { + return PLANE_MODE_LOITER; + } + } case FLM_RTH: return PLANE_MODE_RTL; case FLM_MISSION: return PLANE_MODE_AUTO; case FLM_CRUISE: return PLANE_MODE_CRUISE; @@ -296,6 +312,8 @@ void configureMAVLinkTelemetryPort(void) } mavlinkPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_MAVLINK, NULL, NULL, baudRates[baudRateIndex], TELEMETRY_MAVLINK_PORT_MODE, SERIAL_NOT_INVERTED); + mavAutopilotType = telemetryConfig()->mavlink.autopilot_type; + mavSystemId = telemetryConfig()->mavlink.sysid; if (!mavlinkPort) { return; @@ -486,7 +504,7 @@ void mavlinkSendSystemStatus(void) // errors_count3 Autopilot-specific errors 0, // errors_count4 Autopilot-specific errors - 0); + 0, 0, 0, 0); mavlinkSendMessage(); } @@ -795,11 +813,18 @@ void mavlinkSendHUDAndHeartbeat(void) mavSystemState = MAV_STATE_STANDBY; } + uint8_t mavType; + if (mavAutopilotType == MAVLINK_AUTOPILOT_ARDUPILOT) { + mavType = MAV_AUTOPILOT_ARDUPILOTMEGA; + } else { + mavType = MAV_AUTOPILOT_GENERIC; + } + mavlink_msg_heartbeat_pack(mavSystemId, mavComponentId, &mavSendMsg, // type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) mavSystemType, // autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM - MAV_AUTOPILOT_GENERIC, + mavType, // base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h mavModes, // custom_mode A bitfield for use for autopilot-specific flags. @@ -944,7 +969,7 @@ static bool handleIncoming_MISSION_CLEAR_ALL(void) // Check if this message is for us if (msg.target_system == mavSystemId) { resetWaypointList(); - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); mavlinkSendMessage(); return true; } @@ -971,12 +996,12 @@ static bool handleIncoming_MISSION_COUNT(void) return true; } else if (ARMING_FLAG(ARMED)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); mavlinkSendMessage(); return true; } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_NO_SPACE, MAV_MISSION_TYPE_MISSION); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_NO_SPACE, MAV_MISSION_TYPE_MISSION, 0); mavlinkSendMessage(); return true; } @@ -994,19 +1019,46 @@ static bool handleIncoming_MISSION_ITEM(void) if (msg.target_system == mavSystemId) { // Check supported values first if (ARMING_FLAG(ARMED)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION); - mavlinkSendMessage(); - return true; + // Legacy Mission Planner BS for GUIDED + if (isGCSValid() && (msg.command == MAV_CMD_NAV_WAYPOINT) && (msg.current == 2)) { + if (!(msg.frame == MAV_FRAME_GLOBAL)) { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, + mavRecvMsg.sysid, mavRecvMsg.compid, + MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } + + navWaypoint_t wp; + wp.action = NAV_WP_ACTION_WAYPOINT; + wp.lat = (int32_t)(msg.x * 1e7f); + wp.lon = (int32_t)(msg.y * 1e7f); + wp.alt = (int32_t)(msg.z * 100.0f); + wp.p1 = 0; + wp.p2 = 0; + wp.p3 = 0; + setWaypoint(255, &wp); + + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, + mavRecvMsg.sysid, mavRecvMsg.compid, + MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } else { + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION, 0); + mavlinkSendMessage(); + return true; + } } if ((msg.autocontinue == 0) || (msg.command != MAV_CMD_NAV_WAYPOINT && msg.command != MAV_CMD_NAV_RETURN_TO_LAUNCH)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION, 0); mavlinkSendMessage(); return true; } if ((msg.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) && !(msg.frame == MAV_FRAME_MISSION && msg.command == MAV_CMD_NAV_RETURN_TO_LAUNCH)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION, 0); mavlinkSendMessage(); return true; } @@ -1028,11 +1080,11 @@ static bool handleIncoming_MISSION_ITEM(void) if (incomingMissionWpSequence >= incomingMissionWpCount) { if (isWaypointListValid()) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION, 0); mavlinkSendMessage(); } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID, MAV_MISSION_TYPE_MISSION); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID, MAV_MISSION_TYPE_MISSION, 0); mavlinkSendMessage(); } } @@ -1043,7 +1095,7 @@ static bool handleIncoming_MISSION_ITEM(void) } else { // Wrong sequence number received - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); mavlinkSendMessage(); } @@ -1060,7 +1112,7 @@ static bool handleIncoming_MISSION_REQUEST_LIST(void) // Check if this message is for us if (msg.target_system == mavSystemId) { - mavlink_msg_mission_count_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, getWaypointCount(), MAV_MISSION_TYPE_MISSION); + mavlink_msg_mission_count_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, getWaypointCount(), MAV_MISSION_TYPE_MISSION, 0); mavlinkSendMessage(); return true; } @@ -1095,7 +1147,7 @@ static bool handleIncoming_MISSION_REQUEST(void) mavlinkSendMessage(); } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION, 0); mavlinkSendMessage(); } @@ -1105,6 +1157,80 @@ static bool handleIncoming_MISSION_REQUEST(void) return false; } + +static bool handleIncoming_COMMAND_INT(void) +{ + mavlink_command_int_t msg; + mavlink_msg_command_int_decode(&mavRecvMsg, &msg); + + if (msg.target_system == mavSystemId) { + + if (msg.command == MAV_CMD_DO_REPOSITION) { + + if (!(msg.frame == MAV_FRAME_GLOBAL)) { //|| msg.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || msg.frame == MAV_FRAME_GLOBAL_TERRAIN_ALT)) { + + mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, + msg.command, + MAV_RESULT_UNSUPPORTED, + 0, // progress + 0, // result_param2 + mavRecvMsg.sysid, + mavRecvMsg.compid); + mavlinkSendMessage(); + return true; + } + + if (isGCSValid()) { + navWaypoint_t wp; + wp.action = NAV_WP_ACTION_WAYPOINT; + wp.lat = (int32_t)msg.x; + wp.lon = (int32_t)msg.y; + wp.alt = msg.z * 100.0f; + if (!isnan(msg.param4) && msg.param4 >= 0.0f && msg.param4 < 360.0f) { + wp.p1 = (int16_t)msg.param4; + } else { + wp.p1 = 0; + } + wp.p2 = 0; // TODO: Alt modes + wp.p3 = 0; + wp.flag = 0; + + setWaypoint(255, &wp); + + mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, + msg.command, + MAV_RESULT_ACCEPTED, + 0, // progress + 0, // result_param2 + mavRecvMsg.sysid, + mavRecvMsg.compid); + mavlinkSendMessage(); + } else { + mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, + msg.command, + MAV_RESULT_DENIED, + 0, + 0, + mavRecvMsg.sysid, + mavRecvMsg.compid); + mavlinkSendMessage(); + } + } else { + mavlink_msg_command_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, + msg.command, + MAV_RESULT_UNSUPPORTED, + 0, + 0, + mavRecvMsg.sysid, + mavRecvMsg.compid); + mavlinkSendMessage(); + } + return true; + } + return false; +} + + static bool handleIncoming_RC_CHANNELS_OVERRIDE(void) { mavlink_rc_channels_override_t msg; mavlink_msg_rc_channels_override_decode(&mavRecvMsg, &msg); @@ -1186,9 +1312,10 @@ static bool handleIncoming_ADSB_VEHICLE(void) { adsbVehicleValues_t* vehicle = getVehicleForFill(); if(vehicle != NULL){ vehicle->icao = msg.ICAO_address; - vehicle->lat = msg.lat; - vehicle->lon = msg.lon; + vehicle->gps.lat = msg.lat; + vehicle->gps.lon = msg.lon; vehicle->alt = (int32_t)(msg.altitude / 10); + vehicle->horVelocity = msg.hor_velocity; vehicle->heading = msg.heading; vehicle->flags = msg.flags; vehicle->altitudeType = msg.altitude_type; @@ -1199,25 +1326,6 @@ static bool handleIncoming_ADSB_VEHICLE(void) { adsbNewVehicle(vehicle); } - //debug vehicle - /* if(vehicle != NULL){ - - char name[9] = "DUMMY "; - - vehicle->icao = 666; - vehicle->lat = 492383514; - vehicle->lon = 165148681; - vehicle->alt = 100000; - vehicle->heading = 180; - vehicle->flags = ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_COORDS; - vehicle->altitudeType = 0; - memcpy(&(vehicle->callsign), name, sizeof(vehicle->callsign)); - vehicle->emitterType = 6; - vehicle->tslc = 0; - - adsbNewVehicle(vehicle); - }*/ - return true; } #endif @@ -1243,6 +1351,13 @@ static bool processMAVLinkIncomingTelemetry(void) return handleIncoming_MISSION_ITEM(); case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: return handleIncoming_MISSION_REQUEST_LIST(); + + //TODO: + //case MAVLINK_MSG_ID_COMMAND_LONG; //up to 7 float parameters + //return handleIncoming_COMMAND_LONG(); + + case MAVLINK_MSG_ID_COMMAND_INT: //7 parameters: parameters 1-4, 7 are floats, and parameters 5,6 are scaled integers + return handleIncoming_COMMAND_INT(); case MAVLINK_MSG_ID_MISSION_REQUEST: return handleIncoming_MISSION_REQUEST(); case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c index 117d98b3ed4..4c60a90e707 100644 --- a/src/main/telemetry/msp_shared.c +++ b/src/main/telemetry/msp_shared.c @@ -14,25 +14,49 @@ #include "msp/msp.h" -#include "telemetry/crsf.h" #include "telemetry/msp_shared.h" -#include "telemetry/smartport.h" - -#define TELEMETRY_MSP_VERSION 1 -#define TELEMETRY_MSP_VER_SHIFT 5 -#define TELEMETRY_MSP_VER_MASK (0x7 << TELEMETRY_MSP_VER_SHIFT) -#define TELEMETRY_MSP_ERROR_FLAG (1 << 5) -#define TELEMETRY_MSP_START_FLAG (1 << 4) -#define TELEMETRY_MSP_SEQ_MASK 0x0F + +#define TELEMETRY_MSP_VERSION 2 #define TELEMETRY_MSP_RES_ERROR (-10) +enum { // constants for status of msp-over-telemetry frame + TELEMETRY_MSP_SEQ_MASK = 0x0f, // 0b00001111, // sequence number mask + TELEMETRY_MSP_VER_MASK = 0x60, // 0b01100000, // MSP version mask + TELEMETRY_MSP_START_MASK = 0x10, // 0b00010000, // bit of starting frame (if 1, the frame is a first/single chunk of msp-frame) + TELEMETRY_MSP_ERROR_MASK = 0x80, // 0b10000000, // Error bit (1 if error) + TELEMETRY_MSP_VER_SHIFT = 5, // MSP version shift +}; + enum { TELEMETRY_MSP_VER_MISMATCH=0, TELEMETRY_MSP_CRC_ERROR=1, - TELEMETRY_MSP_ERROR=2 + TELEMETRY_MSP_ERROR=2, + TELEMETRY_MSP_REQUEST_IS_TOO_BIG = 3, +}; + +enum { // minimum length for a frame. + MIN_LENGTH_CHUNK = 2, // status + at_least_one_byte + MIN_LENGTH_REQUEST_V1 = 3, // status + length + ID + MIN_LENGTH_REQUEST_V2 = 6, // status + flag + ID_lo + ID_hi + size_lo + size_hi }; -STATIC_UNIT_TESTED uint8_t checksum = 0; +enum { // byte position(index) in msp-over-telemetry request payload + // MSPv1 + MSP_INDEX_STATUS = 0, // status byte + MSP_INDEX_SIZE_V1 = MSP_INDEX_STATUS + 1, // MSPv1 payload size + MSP_INDEX_ID_V1 = MSP_INDEX_SIZE_V1 + 1, // MSPv1 ID/command/function byte + MSP_INDEX_PAYLOAD_V1 = MSP_INDEX_ID_V1 + 1, // MSPv1 Payload start / CRC for zero payload + + // MSPv2 + MSP_INDEX_FLAG_V2 = MSP_INDEX_SIZE_V1, // MSPv2 flags byte + MSP_INDEX_ID_LO = MSP_INDEX_ID_V1, // MSPv2 Lo byte of ID/command/function + MSP_INDEX_ID_HI = MSP_INDEX_ID_LO + 1, // MSPv2 Hi byte of ID/command/function + MSP_INDEX_SIZE_V2_LO = MSP_INDEX_ID_HI + 1, // MSPv2 Lo byte of payload size + MSP_INDEX_SIZE_V2_HI = MSP_INDEX_SIZE_V2_LO + 1, // MSPv2 Hi byte of payload size + MSP_INDEX_PAYLOAD_V2 = MSP_INDEX_SIZE_V2_HI + 1, // MSPv2 first byte of payload itself +}; + +static uint8_t lastRequestVersion; // MSP version of last request. Temporary solution. It's better to keep it in requestPacket. STATIC_UNIT_TESTED mspPackage_t mspPackage; static mspRxBuffer_t mspRxBuffer; static mspTxBuffer_t mspTxBuffer; @@ -80,7 +104,7 @@ void sendMspErrorResponse(uint8_t error, int16_t cmd) sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); } -bool handleMspFrame(uint8_t *frameStart, int frameLength) +bool handleMspFrame(uint8_t *const frameStart, const int payloadLength) { static uint8_t mspStarted = 0; static uint8_t lastSeq = 0; @@ -93,75 +117,90 @@ bool handleMspFrame(uint8_t *frameStart, int frameLength) initSharedMsp(); } - mspPacket_t *packet = mspPackage.requestPacket; - sbuf_t *frameBuf = sbufInit(&mspPackage.requestFrame, frameStart, frameStart + (uint8_t)frameLength); - sbuf_t *rxBuf = &mspPackage.requestPacket->buf; - const uint8_t header = sbufReadU8(frameBuf); - const uint8_t seqNumber = header & TELEMETRY_MSP_SEQ_MASK; - const uint8_t version = (header & TELEMETRY_MSP_VER_MASK) >> TELEMETRY_MSP_VER_SHIFT; + if (payloadLength < MIN_LENGTH_CHUNK) { + return false; // prevent analyzing garbage data + } + + mspPacket_t *requestPacket = mspPackage.requestPacket; + + const uint8_t status = frameStart[MSP_INDEX_STATUS]; + const uint8_t seqNumber = status & TELEMETRY_MSP_SEQ_MASK; + lastRequestVersion = (status & TELEMETRY_MSP_VER_MASK) >> TELEMETRY_MSP_VER_SHIFT; - if (version != TELEMETRY_MSP_VERSION) { + if (lastRequestVersion > TELEMETRY_MSP_VERSION) { sendMspErrorResponse(TELEMETRY_MSP_VER_MISMATCH, 0); return true; } - if (header & TELEMETRY_MSP_START_FLAG) { - // first packet in sequence - uint8_t mspPayloadSize = sbufReadU8(frameBuf); + if (status & TELEMETRY_MSP_START_MASK) { // first packet in sequence + uint16_t mspPayloadSize; - packet->cmd = sbufReadU8(frameBuf); - packet->result = 0; - packet->buf.ptr = mspPackage.requestBuffer; - packet->buf.end = mspPackage.requestBuffer + mspPayloadSize; + if (lastRequestVersion == 1) { // MSPv1 - checksum = mspPayloadSize ^ packet->cmd; + mspPayloadSize = frameStart[MSP_INDEX_SIZE_V1]; + requestPacket->cmd = frameStart[MSP_INDEX_ID_V1]; + sbufInit(&mspPackage.requestFrame, frameStart + MSP_INDEX_PAYLOAD_V1, frameStart + payloadLength); + + } else { // MSPv2 + if (payloadLength < MIN_LENGTH_REQUEST_V2) { + return false; // prevent analyzing garbage data + } + requestPacket->flags = frameStart[MSP_INDEX_FLAG_V2]; + requestPacket->cmd = *(uint16_t *) &frameStart[MSP_INDEX_ID_LO]; + mspPayloadSize = *(uint16_t *) &frameStart[MSP_INDEX_SIZE_V2_LO]; + sbufInit(&mspPackage.requestFrame, frameStart + MSP_INDEX_PAYLOAD_V2, frameStart + payloadLength); + } + + if (mspPayloadSize <= sizeof(mspRxBuffer)) { // prevent buffer overrun + requestPacket->result = 0; + requestPacket->buf.ptr = mspPackage.requestBuffer; + requestPacket->buf.end = mspPackage.requestBuffer + mspPayloadSize; + mspStarted = 1; + } else { // this MSP packet is too big to fit in the buffer. + sendMspErrorResponse(TELEMETRY_MSP_REQUEST_IS_TOO_BIG, mspPackage.requestPacket->cmd); + return true; + } mspStarted = 1; - } else if (!mspStarted) { - // no start packet yet, throw this one away - return false; - } else if (((lastSeq + 1) & TELEMETRY_MSP_SEQ_MASK) != seqNumber) { - // packet loss detected! - mspStarted = 0; - return false; + } else { // second onward chunk + if (!mspStarted) { // no start packet yet, throw this one away + return false; + } else { + if (((lastSeq + 1) & TELEMETRY_MSP_SEQ_MASK) != seqNumber) { + // packet loss detected! + mspStarted = 0; + return false; + } + } + sbufInit(&mspPackage.requestFrame, frameStart + 1, frameStart + payloadLength); } - const uint8_t bufferBytesRemaining = sbufBytesRemaining(rxBuf); - const uint8_t frameBytesRemaining = sbufBytesRemaining(frameBuf); - uint8_t payload[frameBytesRemaining]; + const uint8_t payloadExpecting = sbufBytesRemaining(&requestPacket->buf); + const uint8_t payloadIncoming = sbufBytesRemaining(&mspPackage.requestFrame); + uint8_t payload[payloadIncoming]; - if (bufferBytesRemaining >= frameBytesRemaining) { - sbufReadData(frameBuf, payload, frameBytesRemaining); - sbufAdvance(frameBuf, frameBytesRemaining); - sbufWriteData(rxBuf, payload, frameBytesRemaining); + if (payloadExpecting > payloadIncoming) { + sbufReadData(&mspPackage.requestFrame, payload, payloadIncoming); + sbufAdvance(&mspPackage.requestFrame, payloadIncoming); + sbufWriteData(&requestPacket->buf, payload, payloadIncoming); lastSeq = seqNumber; return false; - } else { - sbufReadData(frameBuf, payload, bufferBytesRemaining); - sbufAdvance(frameBuf, bufferBytesRemaining); - sbufWriteData(rxBuf, payload, bufferBytesRemaining); - sbufSwitchToReader(rxBuf, mspPackage.requestBuffer); - while (sbufBytesRemaining(rxBuf)) { - checksum ^= sbufReadU8(rxBuf); - } - - if (checksum != *frameBuf->ptr) { - mspStarted = 0; - sendMspErrorResponse(TELEMETRY_MSP_CRC_ERROR, packet->cmd); - return true; - } } mspStarted = 0; - sbufSwitchToReader(rxBuf, mspPackage.requestBuffer); + + sbufReadData(&mspPackage.requestFrame, payload, payloadExpecting); + sbufAdvance(&mspPackage.requestFrame, payloadExpecting); + sbufWriteData(&requestPacket->buf, payload, payloadExpecting); + sbufSwitchToReader(&requestPacket->buf, mspPackage.requestBuffer); processMspPacket(); return true; } bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn) { - static uint8_t checksum = 0; static uint8_t seq = 0; + static bool headerSent = false; uint8_t payloadOut[payloadSize]; sbuf_t payload; @@ -169,20 +208,27 @@ bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn) sbuf_t *txBuf = &mspPackage.responsePacket->buf; // detect first reply packet - if (txBuf->ptr == mspPackage.responseBuffer) { + if (!headerSent) { // header - uint8_t head = TELEMETRY_MSP_START_FLAG | (seq++ & TELEMETRY_MSP_SEQ_MASK); + uint8_t status = TELEMETRY_MSP_START_MASK | (seq++ & TELEMETRY_MSP_SEQ_MASK) | (lastRequestVersion << TELEMETRY_MSP_VER_SHIFT);; if (mspPackage.responsePacket->result < 0) { - head |= TELEMETRY_MSP_ERROR_FLAG; + status |= TELEMETRY_MSP_ERROR_MASK; } - sbufWriteU8(payloadBuf, head); - - uint8_t size = sbufBytesRemaining(txBuf); - sbufWriteU8(payloadBuf, size); + sbufWriteU8(payloadBuf, status); + + const uint8_t size = sbufBytesRemaining(txBuf); + if (lastRequestVersion == 1) { // MSPv1 + sbufWriteU8(payloadBuf, size); + sbufWriteU8(payloadBuf, mspPackage.responsePacket->cmd); + } else { // MSPv2 + sbufWriteU8(payloadBuf, mspPackage.responsePacket->flags); // MSPv2 flags + sbufWriteU16(payloadBuf, mspPackage.responsePacket->cmd); // command is 16 bit in MSPv2 + sbufWriteU16(payloadBuf, (uint16_t) size); // size is 16 bit in MSPv2 + } + headerSent = true; } else { - // header - sbufWriteU8(payloadBuf, (seq++ & TELEMETRY_MSP_SEQ_MASK)); + sbufWriteU8(payloadBuf, (seq++ & TELEMETRY_MSP_SEQ_MASK) | (lastRequestVersion << TELEMETRY_MSP_VER_SHIFT)); // header without 'start' flag } const uint8_t bufferBytesRemaining = sbufBytesRemaining(txBuf); @@ -194,31 +240,19 @@ bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn) sbufReadData(txBuf, frame, payloadBytesRemaining); sbufAdvance(txBuf, payloadBytesRemaining); sbufWriteData(payloadBuf, frame, payloadBytesRemaining); - responseFn(payloadOut); + responseFn(payloadOut, payloadSize); return true; - - } else { - - sbufReadData(txBuf, frame, bufferBytesRemaining); - sbufAdvance(txBuf, bufferBytesRemaining); - sbufWriteData(payloadBuf, frame, bufferBytesRemaining); - sbufSwitchToReader(txBuf, mspPackage.responseBuffer); - - checksum = sbufBytesRemaining(txBuf) ^ mspPackage.responsePacket->cmd; - - while (sbufBytesRemaining(txBuf)) { - checksum ^= sbufReadU8(txBuf); - } - sbufWriteU8(payloadBuf, checksum); - - while (sbufBytesRemaining(payloadBuf)>1) { - sbufWriteU8(payloadBuf, 0); - } - } - responseFn(payloadOut); + // last/only chunk + sbufReadData(txBuf, frame, bufferBytesRemaining); + sbufAdvance(txBuf, bufferBytesRemaining); + sbufWriteData(payloadBuf, frame, bufferBytesRemaining); + sbufSwitchToReader(txBuf, mspPackage.responseBuffer); + + responseFn(payloadOut, payloadBuf->ptr - payloadOut); + headerSent = false; // <-- added: reset for the next response return false; } diff --git a/src/main/telemetry/msp_shared.h b/src/main/telemetry/msp_shared.h index fad6b912d39..b554a92bf47 100644 --- a/src/main/telemetry/msp_shared.h +++ b/src/main/telemetry/msp_shared.h @@ -4,7 +4,7 @@ #include "telemetry/crsf.h" #include "telemetry/smartport.h" -typedef void (*mspResponseFnPtr)(uint8_t *payload); +typedef void (*mspResponseFnPtr)(uint8_t *payload, const uint8_t payloadSize); struct mspPacket_s; typedef struct mspPackage_s { @@ -26,5 +26,5 @@ typedef union mspTxBuffer_u { } mspTxBuffer_t; void initSharedMsp(void); -bool handleMspFrame(uint8_t *frameStart, int frameLength); +bool handleMspFrame(uint8_t *frameStart, int payloadLength); bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 94e318e8a7a..355e550b67e 100755 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -27,7 +27,7 @@ #include "drivers/time.h" #include "fc/config.h" -#include "fc/controlrate_profile.h" +#include "fc/control_profile.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" @@ -162,11 +162,11 @@ static smartPortWriteFrameFn *smartPortWriteFrame; static bool smartPortMspReplyPending = false; #endif -static uint16_t frskyGetFlightMode(void) +static uint32_t frskyGetFlightMode(void) { - uint16_t tmpi = 0; + uint32_t tmpi = 0; - // ones column + // ones column (G) if (!isArmingDisabled()) tmpi += 1; else @@ -174,7 +174,7 @@ static uint16_t frskyGetFlightMode(void) if (ARMING_FLAG(ARMED)) tmpi += 4; - // tens column + // tens column (F) if (FLIGHT_MODE(ANGLE_MODE)) tmpi += 10; if (FLIGHT_MODE(HORIZON_MODE)) @@ -182,16 +182,16 @@ static uint16_t frskyGetFlightMode(void) if (FLIGHT_MODE(MANUAL_MODE)) tmpi += 40; - // hundreds column + // hundreds column (E) if (FLIGHT_MODE(HEADING_MODE)) tmpi += 100; if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) tmpi += 200; - if (FLIGHT_MODE(NAV_POSHOLD_MODE)) + if (FLIGHT_MODE(NAV_POSHOLD_MODE) && !STATE(AIRPLANE)) tmpi += 400; - // thousands column - if (FLIGHT_MODE(NAV_RTH_MODE)) + // thousands column (D) + if (FLIGHT_MODE(NAV_RTH_MODE) && !isWaypointMissionRTHActive()) tmpi += 1000; if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) // intentionally out of order and 'else-ifs' to prevent column overflow tmpi += 8000; @@ -200,7 +200,7 @@ static uint16_t frskyGetFlightMode(void) else if (FLIGHT_MODE(HEADFREE_MODE)) tmpi += 4000; - // ten thousands column + // ten thousands column (C) if (FLIGHT_MODE(FLAPERON)) tmpi += 10000; if (FLIGHT_MODE(FAILSAFE_MODE)) @@ -208,6 +208,22 @@ static uint16_t frskyGetFlightMode(void) else if (FLIGHT_MODE(AUTO_TUNE)) // intentionally reverse order and 'else-if' to prevent 16-bit overflow tmpi += 20000; + // hundred thousands column (B) + if (FLIGHT_MODE(NAV_FW_AUTOLAND)) + tmpi += 100000; + if (FLIGHT_MODE(TURTLE_MODE)) + tmpi += 200000; + else if (FLIGHT_MODE(NAV_POSHOLD_MODE) && STATE(AIRPLANE)) + tmpi += 800000; + if (FLIGHT_MODE(NAV_SEND_TO)) + tmpi += 400000; + + // million column (A) + if (FLIGHT_MODE(NAV_RTH_MODE) && isWaypointMissionRTHActive()) + tmpi += 1000000; + if (FLIGHT_MODE(ANGLEHOLD_MODE)) + tmpi += 2000000; + return tmpi; } @@ -401,10 +417,10 @@ void checkSmartPortTelemetryState(void) } #if defined(USE_MSP_OVER_TELEMETRY) -static void smartPortSendMspResponse(uint8_t *data) { +static void smartPortSendMspResponse(uint8_t *data, const uint8_t dataSize) { smartPortPayload_t payload; payload.frameId = FSSP_MSPS_FRAME; - memcpy(&payload.valueId, data, SMARTPORT_MSP_PAYLOAD_SIZE); + memcpy(&payload.valueId, data, MIN(dataSize,SMARTPORT_MSP_PAYLOAD_SIZE)); smartPortWriteFrame(&payload); } diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index e0820234e65..2c896945843 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -87,6 +87,7 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, #endif .mavlink = { + .autopilot_type = SETTING_MAVLINK_AUTOPILOT_TYPE_DEFAULT, .extended_status_rate = SETTING_MAVLINK_EXT_STATUS_RATE_DEFAULT, .rc_channels_rate = SETTING_MAVLINK_RC_CHAN_RATE_DEFAULT, .position_rate = SETTING_MAVLINK_POS_RATE_DEFAULT, @@ -95,7 +96,8 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT, .version = SETTING_MAVLINK_VERSION_DEFAULT, .min_txbuff = SETTING_MAVLINK_MIN_TXBUFFER_DEFAULT, - .radio_type = SETTING_MAVLINK_RADIO_TYPE_DEFAULT + .radio_type = SETTING_MAVLINK_RADIO_TYPE_DEFAULT, + .sysid = SETTING_MAVLINK_SYSID_DEFAULT } ); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index c8808959277..7fb26781c11 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -36,6 +36,11 @@ typedef enum { LTM_RATE_SLOW } ltmUpdateRate_e; +typedef enum { + MAVLINK_AUTOPILOT_GENERIC, + MAVLINK_AUTOPILOT_ARDUPILOT +} mavlinkAutopilotType_e; + typedef enum { MAVLINK_RADIO_GENERIC, MAVLINK_RADIO_ELRS, @@ -72,6 +77,7 @@ typedef struct telemetryConfig_s { uint16_t accEventThresholdNegX; #endif struct { + uint8_t autopilot_type; uint8_t extended_status_rate; uint8_t rc_channels_rate; uint8_t position_rate; @@ -81,6 +87,7 @@ typedef struct telemetryConfig_s { uint8_t version; uint8_t min_txbuff; uint8_t radio_type; + uint8_t sysid; } mavlink; } telemetryConfig_t; diff --git a/src/main/vcp_hal/usbd_desc.c b/src/main/vcp_hal/usbd_desc.c index 48806761364..62834e83382 100644 --- a/src/main/vcp_hal/usbd_desc.c +++ b/src/main/vcp_hal/usbd_desc.c @@ -91,6 +91,24 @@ USBD_DescriptorsTypeDef VCP_Desc = { USBD_VCP_InterfaceStrDescriptor, }; +#ifdef USE_USB_MSC +/* MSC-specific descriptor functions */ +static uint8_t *USBD_MSC_DeviceDescriptorCallback(USBD_SpeedTypeDef speed, uint16_t *length); +static uint8_t *USBD_MSC_ProductStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length); +static uint8_t *USBD_MSC_ConfigStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length); +static uint8_t *USBD_MSC_InterfaceStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length); + +USBD_DescriptorsTypeDef MSC_Desc = { + USBD_MSC_DeviceDescriptorCallback, + USBD_VCP_LangIDStrDescriptor, // Reuse VCP LangID + USBD_VCP_ManufacturerStrDescriptor, // Reuse VCP Manufacturer + USBD_MSC_ProductStrDescriptor, // MSC-specific + USBD_VCP_SerialStrDescriptor, // Reuse VCP Serial + USBD_MSC_ConfigStrDescriptor, // MSC-specific + USBD_MSC_InterfaceStrDescriptor, // MSC-specific +}; +#endif + /* USB Standard Device Descriptor */ #if defined ( __ICCARM__ ) /*!< IAR Compiler */ #pragma data_alignment=4 @@ -338,4 +356,77 @@ static void IntToUnicode (uint32_t value , uint8_t *pbuf , uint8_t len) pbuf[ 2* idx + 1] = 0; } } + +#ifdef USE_USB_MSC +/** + * @brief Returns the MSC device descriptor. + * @param speed: Current device speed + * @param length: Pointer to data length variable + * @retval Pointer to descriptor buffer + */ +static uint8_t *USBD_MSC_DeviceDescriptorCallback(USBD_SpeedTypeDef speed, uint16_t *length) +{ + (void)speed; + *length = sizeof(USBD_MSC_DeviceDesc); + return (uint8_t*)USBD_MSC_DeviceDesc; +} + +/** + * @brief Returns the MSC product string descriptor. + * @param speed: Current device speed + * @param length: Pointer to data length variable + * @retval Pointer to descriptor buffer + */ +static uint8_t *USBD_MSC_ProductStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) +{ + if (speed == USBD_SPEED_HIGH) + { + USBD_GetString((uint8_t *)"STM32 Mass Storage in HS Mode", USBD_StrDesc, length); + } + else + { + USBD_GetString((uint8_t *)"STM32 Mass Storage in FS Mode", USBD_StrDesc, length); + } + return USBD_StrDesc; +} + +/** + * @brief Returns the MSC configuration string descriptor. + * @param speed: Current device speed + * @param length: Pointer to data length variable + * @retval Pointer to descriptor buffer + */ +static uint8_t *USBD_MSC_ConfigStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) +{ + if (speed == USBD_SPEED_HIGH) + { + USBD_GetString((uint8_t *)"MSC Config", USBD_StrDesc, length); + } + else + { + USBD_GetString((uint8_t *)"MSC Config", USBD_StrDesc, length); + } + return USBD_StrDesc; +} + +/** + * @brief Returns the MSC interface string descriptor. + * @param speed: Current device speed + * @param length: Pointer to data length variable + * @retval Pointer to descriptor buffer + */ +static uint8_t *USBD_MSC_InterfaceStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) +{ + if (speed == USBD_SPEED_HIGH) + { + USBD_GetString((uint8_t *)"MSC Interface", USBD_StrDesc, length); + } + else + { + USBD_GetString((uint8_t *)"MSC Interface", USBD_StrDesc, length); + } + return USBD_StrDesc; +} +#endif /* USE_USB_MSC */ + /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/main/vcp_hal/usbd_desc.h b/src/main/vcp_hal/usbd_desc.h index 3baad094f40..57d50c4da30 100644 --- a/src/main/vcp_hal/usbd_desc.h +++ b/src/main/vcp_hal/usbd_desc.h @@ -60,6 +60,10 @@ /* Exported functions ------------------------------------------------------- */ extern USBD_DescriptorsTypeDef VCP_Desc; +#ifdef USE_USB_MSC +extern USBD_DescriptorsTypeDef MSC_Desc; +#endif + #endif /* __USBD_DESC_H */ /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/test/mock_msp_gps.py b/src/test/mock_msp_gps.py new file mode 100755 index 00000000000..6d36575ab73 --- /dev/null +++ b/src/test/mock_msp_gps.py @@ -0,0 +1,427 @@ +#!/usr/bin/env python3 +""" +Mock MSP GPS sender for testing GPS recovery after signal loss. + +This tool sends MSP_SET_RAW_GPS messages to a SITL or real flight controller +to simulate GPS fix, loss, and recovery scenarios. + +Uses the uNAVlib library for proper MSP communication. + +Usage: + python3 mock_msp_gps.py /dev/ttyUSB0 # For real FC + python3 mock_msp_gps.py 5761 # For SITL TCP (port number only) + +Test scenario: + 1. Sends GPS fix for 1 second + 2. Arms the FC (home position set on arm) + 3. Flies away at 30mph until >100m from home + 4. Simulates GPS loss for 5 seconds + 5. Simulates GPS recovery + 6. Checks if distance-to-home recovers properly + +Requirements: + pip3 install git+https://github.com/xznhj8129/uNAVlib +""" + +import struct +import time +import sys +import argparse +import math + +# MSP protocol constants +MSP_SET_RAW_GPS = 201 +MSP_RAW_GPS = 106 +MSP_COMP_GPS = 107 +MSP_SET_RAW_RC = 200 +MSP_RX_CONFIG = 44 +MSP_SET_RX_CONFIG = 45 +MSP_SET_MODE_RANGE = 35 +MSP_EEPROM_WRITE = 250 +MSP_REBOOT = 68 +MSP_SIMULATOR = 0x201F # Enable HITL mode +MSP2_INAV_STATUS = 0x2000 + +# Receiver types +RX_TYPE_MSP = 2 + +# Simulator flags +HITL_ENABLE = (1 << 0) +SIMULATOR_MSP_VERSION = 2 + +# 30 mph = 48.28 km/h = 13.41 m/s = 1341 cm/s +FLIGHT_SPEED_CMS = 1341 + +# RC channel values +RC_MID = 1500 +RC_LOW = 1000 +RC_HIGH = 2000 + +# Approximate meters per degree at mid-latitudes +# 1 degree latitude ~ 111,000 meters +# 1 unit in INAV (1e-7 degrees) ~ 0.0111 meters +METERS_PER_UNIT = 0.0000111 + + +def create_gps_payload(fix_type, num_sat, lat, lon, alt_m, ground_speed): + """ + Create MSP_SET_RAW_GPS payload. + + Args: + fix_type: 0=no fix, 1=2D, 2=3D + num_sat: number of satellites + lat: latitude in degrees * 10^7 (e.g., 51.5074 -> 515074000) + lon: longitude in degrees * 10^7 (e.g., -0.1278 -> -1278000) + alt_m: altitude in meters + ground_speed: ground speed in cm/s + """ + return list(struct.pack('> 8) & 0xFF]) + + board.send_RAW_msg(MSP_SET_RAW_RC, data=data) + # CRITICAL: Consume response to prevent socket buffer overflow + consume_response(board) + + +def send_gps(board, fix_type, num_sat, lat, lon, alt_m, ground_speed): + """Send GPS data via MSP and consume response.""" + payload = create_gps_payload(fix_type, num_sat, lat, lon, alt_m, ground_speed) + board.send_RAW_msg(MSP_SET_RAW_GPS, data=payload) + consume_response(board) + + +def query_distance_to_home(board): + """Query MSP_COMP_GPS to get distance to home.""" + board.send_RAW_msg(MSP_COMP_GPS, data=[]) + dataHandler = board.receive_msg() + if dataHandler: + board.process_recv_data(dataHandler) + return board.GPS_DATA.get('distanceToHome', None) + + +def setup_receiver_type(board): + """Set receiver type to MSP.""" + # Read current config + board.send_RAW_msg(MSP_RX_CONFIG, data=[]) + time.sleep(0.2) + dataHandler = board.receive_msg() + data = dataHandler.get('dataView', []) if dataHandler else [] + + if data and len(data) >= 24: + current_data = list(data) + else: + # Use defaults + current_data = [0] * 24 + current_data[1], current_data[2] = 0x6C, 0x07 # maxcheck = 1900 + current_data[3], current_data[4] = 0xDC, 0x05 # midrc = 1500 + current_data[5], current_data[6] = 0x4C, 0x04 # mincheck = 1100 + current_data[8], current_data[9] = 0x75, 0x03 # rx_min_usec = 885 + current_data[10], current_data[11] = 0x43, 0x08 # rx_max_usec = 2115 + + current_data[23] = RX_TYPE_MSP + board.send_RAW_msg(MSP_SET_RX_CONFIG, data=current_data[:24]) + consume_response(board) + + +def setup_arm_mode(board): + """Configure ARM mode on AUX1 for range 1700-2100.""" + # slot=0, boxId=0 (ARM), auxChannel=0 (AUX1), startStep=32 (1700), endStep=48 (2100) + payload = [0, 0, 0, 32, 48] + board.send_RAW_msg(MSP_SET_MODE_RANGE, data=payload) + consume_response(board) + + +def save_config(board): + """Save configuration to EEPROM.""" + board.send_RAW_msg(MSP_EEPROM_WRITE, data=[]) + time.sleep(0.5) + consume_response(board) + + +def enable_hitl_mode(board): + """Enable HITL mode to bypass sensor calibration.""" + payload = [SIMULATOR_MSP_VERSION, HITL_ENABLE] + board.send_RAW_msg(MSP_SIMULATOR, data=payload) + consume_response(board) + + +def run_gps_recovery_test(board, base_lat, base_lon, base_alt, log_file=None): + """ + Run the GPS recovery test scenario. + + Test sequence: + 1. Send GPS fix for 1 second (pre-arm) + 2. Arm the FC (home position is set on arm) + 3. Fly away at 30mph until >100m from home + 4. GPS loss for 5 seconds + 5. GPS recovery at new position + 6. Check if distance-to-home is correct (non-zero) + """ + + def log(msg): + print(msg) + if log_file: + log_file.write(msg + "\n") + log_file.flush() + + log("\n" + "="*60) + log("GPS Recovery Test - Issue #11049") + log("="*60) + log(f"Home position: lat={base_lat/1e7:.6f}, lon={base_lon/1e7:.6f}, alt={base_alt}m") + log(f"Flight speed: {FLIGHT_SPEED_CMS} cm/s (~30 mph)") + + # Phase 1: Send GPS fix for 2 seconds (pre-arm, establish GPS link) + log("\n[Phase 1] Sending GPS fix for 2 seconds (pre-arm)...") + start_time = time.time() + while time.time() - start_time < 2.0: + send_gps(board, fix_type=2, num_sat=12, lat=base_lat, lon=base_lon, + alt_m=base_alt, ground_speed=0) + # Also send RC to keep connection alive + send_rc(board, throttle=RC_LOW) + time.sleep(0.02) # 50 Hz for RC link + log(" -> GPS fix sent") + + # Phase 2: Arm the FC + log("\n[Phase 2] Arming FC (home position set on arm)...") + # Send arm command via AUX1 high (typical arm switch) + # Keep sending GPS and RC at 50Hz + start_time = time.time() + while time.time() - start_time < 2.0: + send_gps(board, fix_type=2, num_sat=12, lat=base_lat, lon=base_lon, + alt_m=base_alt, ground_speed=0) + # AUX1 high = arm (channel 5, value 2000) + send_rc(board, throttle=RC_LOW, aux1=RC_HIGH) + time.sleep(0.02) + + dist = query_distance_to_home(board) + log(f" -> Armed. Distance to home: {dist}m (should be ~0)") + + # Phase 3: Fly away at 30 mph until >100m from home + log("\n[Phase 3] Flying away at 30 mph until >100m from home...") + current_lat = base_lat + current_lon = base_lon + + # Calculate movement per update (at 50Hz) + # 30 mph = 13.41 m/s, at 50Hz = 0.27m per update + meters_per_update = FLIGHT_SPEED_CMS / 100.0 / 50.0 # m/s / Hz + units_per_update = int(meters_per_update / METERS_PER_UNIT) + + log(f" Movement: {meters_per_update:.2f}m per update, {units_per_update} lat/lon units") + + start_time = time.time() + max_fly_time = 30 # Max 30 seconds of flying + last_log_time = 0 + + while time.time() - start_time < max_fly_time: + # Move north + current_lat += units_per_update + + send_gps(board, fix_type=2, num_sat=12, lat=current_lat, lon=current_lon, + alt_m=base_alt + 50, ground_speed=FLIGHT_SPEED_CMS) + send_rc(board, throttle=1600, aux1=RC_HIGH) # Armed, some throttle + time.sleep(0.02) + + # Check distance every second + elapsed = time.time() - start_time + if elapsed - last_log_time >= 1.0: + dist = query_distance_to_home(board) + log(f" t={elapsed:.1f}s: Distance to home = {dist}m") + last_log_time = elapsed + if dist is not None and dist >= 100: + break + + dist_before_loss = query_distance_to_home(board) + log(f" -> Reached distance: {dist_before_loss}m from home") + + if dist_before_loss is None or dist_before_loss < 50: + log(" WARNING: Did not reach sufficient distance. Test may be invalid.") + + # Phase 4: GPS loss for 5 seconds + log("\n[Phase 4] Simulating GPS loss for 5 seconds...") + loss_lat = current_lat + loss_lon = current_lon + + start_time = time.time() + while time.time() - start_time < 5.0: + send_gps(board, fix_type=0, num_sat=0, lat=0, lon=0, alt_m=0, ground_speed=0) + send_rc(board, throttle=1600, aux1=RC_HIGH) # Stay armed + time.sleep(0.02) + + dist_during_loss = query_distance_to_home(board) + log(f" -> GPS lost. Distance to home: {dist_during_loss}m") + + # Phase 5: GPS recovery - simulate we moved a bit further during loss + log("\n[Phase 5] Simulating GPS recovery...") + # Assume we drifted another 20m north during loss + recovery_lat = loss_lat + int(20 / METERS_PER_UNIT) + recovery_lon = loss_lon + + start_time = time.time() + while time.time() - start_time < 5.0: + send_gps(board, fix_type=2, num_sat=10, lat=recovery_lat, lon=recovery_lon, + alt_m=base_alt + 50, ground_speed=500) + send_rc(board, throttle=1600, aux1=RC_HIGH) + time.sleep(0.02) + + dist_after_recovery = query_distance_to_home(board) + log(f" -> GPS recovered. Distance to home: {dist_after_recovery}m") + + # Results + log("\n" + "="*60) + log("TEST RESULTS") + log("="*60) + log(f"Distance before GPS loss: {dist_before_loss}m") + log(f"Distance during GPS loss: {dist_during_loss}m") + log(f"Distance after recovery: {dist_after_recovery}m") + + if dist_after_recovery is not None and dist_after_recovery > 50: + log("\nSUCCESS: Distance to home recovered correctly!") + log("The GPS recovery fix is working.") + return True + elif dist_after_recovery == 0: + log("\nBUG CONFIRMED: Distance to home stuck at 0!") + log("Issue #11049 is present - GPS recovery bug.") + return False + else: + log(f"\nINCONCLUSIVE: Unexpected distance value ({dist_after_recovery})") + return None + + +def main(): + parser = argparse.ArgumentParser(description='Mock MSP GPS for testing GPS recovery (uses uNAVlib)') + parser.add_argument('target', help='Serial port or TCP port number for SITL') + parser.add_argument('--lat', type=float, default=51.5074, + help='Base latitude in degrees (default: 51.5074 - London)') + parser.add_argument('--lon', type=float, default=-0.1278, + help='Base longitude in degrees (default: -0.1278 - London)') + parser.add_argument('--alt', type=int, default=100, + help='Base altitude in meters (default: 100)') + parser.add_argument('--log', type=str, default=None, + help='Log file path (optional)') + + args = parser.parse_args() + + # Convert lat/lon to INAV format (degrees * 10^7) + base_lat = int(args.lat * 1e7) + base_lon = int(args.lon * 1e7) + + # Import uNAVlib + try: + from unavlib.main import MSPy + except ImportError: + print("Error: uNAVlib not installed. Install with:") + print(" pip3 install git+https://github.com/xznhj8129/uNAVlib") + return 1 + + # Determine if TCP or serial + try: + port = int(args.target) + use_tcp = True + device = str(port) + except ValueError: + use_tcp = False + device = args.target + + print(f"Connecting to {'TCP port' if use_tcp else 'serial'} {device}...") + + log_file = None + if args.log: + log_file = open(args.log, 'w') + log_file.write(f"GPS Recovery Test Log\n") + log_file.write(f"Date: {time.strftime('%Y-%m-%d %H:%M:%S')}\n") + log_file.write(f"Target: {device}\n") + + try: + # Phase 0: Initial setup - configure receiver and arm mode + print("\n[Setup] Configuring FC for MSP receiver...") + with MSPy(device=device, use_tcp=use_tcp, loglevel='WARNING') as board: + if board == 1: + print(f"Error: Could not connect to {device}") + return 1 + + print(f"Connected! FC: {board.CONFIG.get('flightControllerIdentifier', 'Unknown')}") + print(f"API Version: {board.CONFIG.get('apiVersion', 'Unknown')}") + + if log_file: + log_file.write(f"FC: {board.CONFIG.get('flightControllerIdentifier', 'Unknown')}\n") + log_file.write(f"API: {board.CONFIG.get('apiVersion', 'Unknown')}\n") + + print(" Setting receiver type to MSP...") + setup_receiver_type(board) + print(" Setting up ARM mode on AUX1...") + setup_arm_mode(board) + print(" Saving config...") + save_config(board) + print(" Rebooting FC...") + board.send_RAW_msg(MSP_REBOOT, data=[]) + + # Wait for FC to restart + print(" Waiting 15 seconds for FC restart...") + time.sleep(15) + + # Reconnect and run test + print("\n[Test] Reconnecting for GPS recovery test...") + with MSPy(device=device, use_tcp=use_tcp, loglevel='WARNING') as board: + if board == 1: + print(f"Error: Could not reconnect to {device}") + return 1 + + print(f"Reconnected! FC: {board.CONFIG.get('flightControllerIdentifier', 'Unknown')}") + + # Enable HITL mode to bypass sensor calibration + print(" Enabling HITL mode...") + enable_hitl_mode(board) + + result = run_gps_recovery_test(board, base_lat, base_lon, args.alt, log_file) + + if log_file: + log_file.write(f"\nFinal result: {'PASS' if result else 'FAIL' if result is False else 'INCONCLUSIVE'}\n") + + return 0 if result else 1 + + except Exception as e: + print(f"Error: {e}") + import traceback + traceback.print_exc() + if log_file: + log_file.write(f"\nError: {e}\n") + return 1 + finally: + if log_file: + log_file.close() + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/src/test/unit/rc_controls_unittest.cc.txt b/src/test/unit/rc_controls_unittest.cc.txt index 8e744dba893..787126e2e70 100644 --- a/src/test/unit/rc_controls_unittest.cc.txt +++ b/src/test/unit/rc_controls_unittest.cc.txt @@ -177,7 +177,7 @@ TEST_F(RcControlsModesTest, updateActivatedModesUsingValidAuxConfigurationAndRXV enum { COUNTER_GENERATE_PITCH_ROLL_CURVE = 0, COUNTER_QUEUE_CONFIRMATION_BEEP, - COUNTER_CHANGE_CONTROL_RATE_PROFILE + COUNTER_CHANGE_CONTROL_PROFILE }; #define CALL_COUNT_ITEM_COUNT 3 @@ -199,7 +199,7 @@ void beeper(beeperMode_e mode) { } void changeControlRateProfile(uint8_t) { - callCounts[COUNTER_CHANGE_CONTROL_RATE_PROFILE]++; + callCounts[COUNTER_CHANGE_CONTROL_PROFILE]++; } } @@ -479,7 +479,7 @@ TEST_F(RcControlsAdjustmentsTest, processRcRateProfileAdjustments) // then EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 1); - EXPECT_EQ(CALL_COUNTER(COUNTER_CHANGE_CONTROL_RATE_PROFILE), 1); + EXPECT_EQ(CALL_COUNTER(COUNTER_CHANGE_CONTROL_PROFILE), 1); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); } diff --git a/src/utils/bf2inav.py b/src/utils/bf2inav.py index aaf6ad22157..5ccbb6b4ad6 100755 --- a/src/utils/bf2inav.py +++ b/src/utils/bf2inav.py @@ -464,7 +464,8 @@ def writeTargetH(folder, map): 'W25M', 'W25M02G', 'W25M512', - 'W25N01G' + 'W25N01G', + 'W25N02K', ] file.write("#define USE_FLASHFS\n") file.write("#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT\n") diff --git a/src/utils/mcus.json b/src/utils/mcus.json new file mode 100755 index 00000000000..a03b9ddffe3 --- /dev/null +++ b/src/utils/mcus.json @@ -0,0 +1,20493 @@ +{ + "Mcus": [ + { + "Name": "STM32F405", + "Pins": { + "PA0": { + "Name": "PA0", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH1_ETR" + ], + "AF2": [ + "TIM5_CH1" + ], + "AF3": [ + "TIM8_ETR" + ], + "AF7": [ + "USART2_CTS" + ], + "AF8": [ + "UART4_TX" + ], + "AF11": [ + "ETH_MII_CRS" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA1": { + "Name": "PA1", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH2" + ], + "AF2": [ + "TIM5_CH2" + ], + "AF7": [ + "USART2_RTS" + ], + "AF8": [ + "UART4_RX" + ], + "AF11": [ + "ETH_MII_RX_CLKETH_RMII__REF_CLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA2": { + "Name": "PA2", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH3" + ], + "AF2": [ + "TIM5_CH3" + ], + "AF3": [ + "TIM9_CH1" + ], + "AF7": [ + "USART2_TX" + ], + "AF11": [ + "ETH_MDIO" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA3": { + "Name": "PA3", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH4" + ], + "AF2": [ + "TIM5_CH4" + ], + "AF3": [ + "TIM9_CH2" + ], + "AF7": [ + "USART2_RX" + ], + "AF10": [ + "OTG_HS_ULPI_D0" + ], + "AF11": [ + "ETH_MII_COL" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA4": { + "Name": "PA4", + "AlternateFunctions": { + "AF5": [ + "SPI1_NSS" + ], + "AF6": [ + "SPI3_NSSI2S3_WS" + ], + "AF7": [ + "USART2_CK" + ], + "AF12": [ + "OTG_HS_SOF" + ], + "AF13": [ + "DCMI_HSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA5": { + "Name": "PA5", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH1_ETR" + ], + "AF3": [ + "TIM8_CH1N" + ], + "AF5": [ + "SPI1_SCK" + ], + "AF10": [ + "OTG_HS_ULPI_CK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA6": { + "Name": "PA6", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF2": [ + "TIM3_CH1" + ], + "AF3": [ + "TIM8_BKIN" + ], + "AF5": [ + "SPI1_MISO" + ], + "AF9": [ + "TIM13_CH1" + ], + "AF13": [ + "DCMI_PIXCLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA7": { + "Name": "PA7", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF2": [ + "TIM3_CH2" + ], + "AF3": [ + "TIM8_CH1N" + ], + "AF5": [ + "SPI1_MOSI" + ], + "AF9": [ + "TIM14_CH1" + ], + "AF11": [ + "ETH_MII_RX_DVETH_RMII_CRS_DV" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA8": { + "Name": "PA8", + "AlternateFunctions": { + "AF0": [ + "MCO1" + ], + "AF1": [ + "TIM1_CH1" + ], + "AF4": [ + "I2C3_SCL" + ], + "AF7": [ + "USART1_CK" + ], + "AF10": [ + "OTG_FS_SOF" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA9": { + "Name": "PA9", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2" + ], + "AF4": [ + "I2C3_SMBA" + ], + "AF7": [ + "USART1_TX" + ], + "AF13": [ + "DCMI_D0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA10": { + "Name": "PA10", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3" + ], + "AF7": [ + "USART1_RX" + ], + "AF10": [ + "OTG_FS_ID" + ], + "AF13": [ + "DCMI_D1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA11": { + "Name": "PA11", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH4" + ], + "AF7": [ + "USART1_CTS" + ], + "AF9": [ + "CAN1_RX" + ], + "AF10": [ + "OTG_FS_DM" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA12": { + "Name": "PA12", + "AlternateFunctions": { + "AF1": [ + "TIM1_ETR" + ], + "AF7": [ + "USART1_RTS" + ], + "AF9": [ + "CAN1_TX" + ], + "AF10": [ + "OTG_FS_DP" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA13": { + "Name": "PA13", + "AlternateFunctions": { + "AF0": [ + "JTMS-SWDIO" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA14": { + "Name": "PA14", + "AlternateFunctions": { + "AF0": [ + "JTCK-SWCLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA15": { + "Name": "PA15", + "AlternateFunctions": { + "AF0": [ + "JTDI" + ], + "AF1": [ + "TIM2_CH1", + "TIM2_ETR" + ], + "AF5": [ + "SPI1_NSS" + ], + "AF6": [ + "SPI3_NSS", + "I2S3_WS" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB0": { + "Name": "PB0", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF2": [ + "TIM3_CH3" + ], + "AF3": [ + "TIM8_CH2N" + ], + "AF10": [ + "OTG_HS_ULPI_D1" + ], + "AF11": [ + "ETH", + "_MII_RXD2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB1": { + "Name": "PB1", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3N" + ], + "AF2": [ + "TIM3_CH4" + ], + "AF3": [ + "TIM8_CH3N" + ], + "AF10": [ + "OTG_HS_ULPI_D2" + ], + "AF11": [ + "ETH", + "_MII_RXD3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB2": { + "Name": "PB2", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB3": { + "Name": "PB3", + "AlternateFunctions": { + "AF0": [ + "JTDO", + "TRACESWO" + ], + "AF1": [ + "TIM2_CH2" + ], + "AF5": [ + "SPI1_SCK" + ], + "AF6": [ + "SPI3_SCKI2S3_CK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB4": { + "Name": "PB4", + "AlternateFunctions": { + "AF0": [ + "NJTRST" + ], + "AF2": [ + "TIM3_CH1" + ], + "AF5": [ + "SPI1_MISO" + ], + "AF6": [ + "SPI3_MISO" + ], + "AF7": [ + "I2S3ext_SD" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB5": { + "Name": "PB5", + "AlternateFunctions": { + "AF2": [ + "TIM3_CH2" + ], + "AF4": [ + "I2C1_SMBA" + ], + "AF5": [ + "SPI1_MOSI" + ], + "AF6": [ + "SPI3_MOSII2S3_SD" + ], + "AF9": [ + "CAN2_RX" + ], + "AF10": [ + "OTG_HS_ULPI_D7" + ], + "AF11": [ + "ETH", + "_PPS_OUT" + ], + "AF13": [ + "DCMI_D10" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB6": { + "Name": "PB6", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH1" + ], + "AF4": [ + "I2C1_SCL" + ], + "AF7": [ + "USART1_TX" + ], + "AF9": [ + "CAN2_TX" + ], + "AF13": [ + "DCMI_D5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB7": { + "Name": "PB7", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH2" + ], + "AF4": [ + "I2C1_SDA" + ], + "AF7": [ + "USART1_RX" + ], + "AF12": [ + "FSMC_NL" + ], + "AF13": [ + "DCMI_VSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB8": { + "Name": "PB8", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH3" + ], + "AF3": [ + "TIM10_CH1" + ], + "AF4": [ + "I2C1_SCL" + ], + "AF9": [ + "CAN1_RX" + ], + "AF11": [ + "ETH", + "_MII_TXD3" + ], + "AF12": [ + "SDIO_D4" + ], + "AF13": [ + "DCMI_D6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB9": { + "Name": "PB9", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH4" + ], + "AF3": [ + "TIM11_CH1" + ], + "AF4": [ + "I2C1_SDA" + ], + "AF5": [ + "SPI2_NSSI2S2_WS" + ], + "AF9": [ + "CAN1_TX" + ], + "AF12": [ + "SDIO_D5" + ], + "AF13": [ + "DCMI_D7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB10": { + "Name": "PB10", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH3" + ], + "AF4": [ + "I2C2_SCL" + ], + "AF5": [ + "SPI2_SCKI2S2_CK" + ], + "AF7": [ + "USART3_TX" + ], + "AF10": [ + "OTG_HS_ULPI_D3" + ], + "AF11": [ + "ETH_", + "MII_RX_ER" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB11": { + "Name": "PB11", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH4" + ], + "AF4": [ + "I2C2_SDA" + ], + "AF7": [ + "USART3_RX" + ], + "AF10": [ + "OTG_HS_ULPI_D4" + ], + "AF11": [ + "ETH", + "_MII_TX_ENETH_RMII_TX_EN" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB12": { + "Name": "PB12", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF4": [ + "I2C2_SMBA" + ], + "AF5": [ + "SPI2_NSSI2S2_WS" + ], + "AF7": [ + "USART3_CK" + ], + "AF9": [ + "CAN2_RX" + ], + "AF10": [ + "OTG_HS_ULPI_D5" + ], + "AF11": [ + "ETH_MII_TXD0ETH_RMII_TXD0" + ], + "AF12": [ + "OTG_HS_ID" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB13": { + "Name": "PB13", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF5": [ + "SPI2_SCKI2S2_CK" + ], + "AF7": [ + "USART3_CTS" + ], + "AF9": [ + "CAN2_TX" + ], + "AF10": [ + "OTG_HS_ULPI_D6" + ], + "AF11": [ + "ETH_MII_TXD1ETH_RMII_TXD1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB14": { + "Name": "PB14", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF3": [ + "TIM8_CH2N" + ], + "AF5": [ + "SPI2_MISO" + ], + "AF6": [ + "I2S2ext_SD" + ], + "AF7": [ + "USART3_RTS" + ], + "AF9": [ + "TIM12_CH1" + ], + "AF12": [ + "OTG_HS_DM" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB15": { + "Name": "PB15", + "AlternateFunctions": { + "AF0": [ + "RTC_REFIN" + ], + "AF1": [ + "TIM1_CH3N" + ], + "AF3": [ + "TIM8_CH3N" + ], + "AF5": [ + "SPI2_MOSII2S2_SD" + ], + "AF9": [ + "TIM12_CH2" + ], + "AF12": [ + "OTG_HS_DP" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC0": { + "Name": "PC0", + "AlternateFunctions": { + "AF10": [ + "OTG_HS_ULPI_STP" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC1": { + "Name": "PC1", + "AlternateFunctions": { + "AF11": [ + "ETH_MDC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC2": { + "Name": "PC2", + "AlternateFunctions": { + "AF5": [ + "SPI2_MISO" + ], + "AF6": [ + "I2S2ext_SD" + ], + "AF10": [ + "OTG_HS_ULPI_DIR" + ], + "AF11": [ + "ETH_MII_TXD2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC3": { + "Name": "PC3", + "AlternateFunctions": { + "AF5": [ + "SPI2_MOSII2S2_SD" + ], + "AF10": [ + "OTG_HS_ULPI_NXT" + ], + "AF11": [ + "ETH_MII_TX_CLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC4": { + "Name": "PC4", + "AlternateFunctions": { + "AF11": [ + "ETH_MII_RXD0ETH_RMII_RXD0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC5": { + "Name": "PC5", + "AlternateFunctions": { + "AF11": [ + "ETH_MII_RXD1ETH_RMII_RXD1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC6": { + "Name": "PC6", + "AlternateFunctions": { + "AF2": [ + "TIM3_CH1" + ], + "AF3": [ + "TIM8_CH1" + ], + "AF5": [ + "I2S2_MCK" + ], + "AF8": [ + "USART6_TX" + ], + "AF12": [ + "SDIO_D6" + ], + "AF13": [ + "DCMI_D0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC7": { + "Name": "PC7", + "AlternateFunctions": { + "AF2": [ + "TIM3_CH2" + ], + "AF3": [ + "TIM8_CH2" + ], + "AF6": [ + "I2S3_MCK" + ], + "AF8": [ + "USART6_RX" + ], + "AF12": [ + "SDIO_D7" + ], + "AF13": [ + "DCMI_D1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC8": { + "Name": "PC8", + "AlternateFunctions": { + "AF2": [ + "TIM3_CH3" + ], + "AF3": [ + "TIM8_CH3" + ], + "AF8": [ + "USART6_CK" + ], + "AF12": [ + "SDIO_D0" + ], + "AF13": [ + "DCMI_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC9": { + "Name": "PC9", + "AlternateFunctions": { + "AF0": [ + "MCO2" + ], + "AF2": [ + "TIM3_CH4" + ], + "AF3": [ + "TIM8_CH4" + ], + "AF4": [ + "I2C3_SDA" + ], + "AF5": [ + "I2S_CKIN" + ], + "AF12": [ + "SDIO_D1" + ], + "AF13": [ + "DCMI_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC10": { + "Name": "PC10", + "AlternateFunctions": { + "AF6": [ + "SPI3_SCK", + "I2S3_CK" + ], + "AF7": [ + "USART3_TX" + ], + "AF8": [ + "UART4_TX" + ], + "AF12": [ + "SDIO_D2" + ], + "AF13": [ + "DCMI_D8" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC11": { + "Name": "PC11", + "AlternateFunctions": { + "AF5": [ + "I2S3ext_SD" + ], + "AF6": [ + "SPI3_MISO" + ], + "AF7": [ + "USART3_RX" + ], + "AF8": [ + "UART4_RX" + ], + "AF12": [ + "SDIO_D3" + ], + "AF13": [ + "DCMI_D4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC12": { + "Name": "PC12", + "AlternateFunctions": { + "AF6": [ + "SPI3_MOSII2S3_SD" + ], + "AF7": [ + "USART3_CK" + ], + "AF8": [ + "UART5_TX" + ], + "AF12": [ + "SDIO_CK" + ], + "AF13": [ + "DCMI_D9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC13": { + "Name": "PC13", + "AlternateFunctions": {} + }, + "PC14": { + "Name": "PC14", + "AlternateFunctions": {} + }, + "PC15": { + "Name": "PC15", + "AlternateFunctions": {} + }, + "PD0": { + "Name": "PD0", + "AlternateFunctions": { + "AF9": [ + "CAN1_RX" + ], + "AF12": [ + "FSMC_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD1": { + "Name": "PD1", + "AlternateFunctions": { + "AF9": [ + "CAN1_TX" + ], + "AF12": [ + "FSMC_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD2": { + "Name": "PD2", + "AlternateFunctions": { + "AF2": [ + "TIM3_ETR" + ], + "AF8": [ + "UART5_RX" + ], + "AF12": [ + "SDIO_CMD" + ], + "AF13": [ + "DCMI_D11" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD3": { + "Name": "PD3", + "AlternateFunctions": { + "AF7": [ + "USART2_CTS" + ], + "AF12": [ + "FSMC_CLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD4": { + "Name": "PD4", + "AlternateFunctions": { + "AF7": [ + "USART2_RTS" + ], + "AF12": [ + "FSMC_NOE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD5": { + "Name": "PD5", + "AlternateFunctions": { + "AF7": [ + "USART2_TX" + ], + "AF12": [ + "FSMC_NWE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD6": { + "Name": "PD6", + "AlternateFunctions": { + "AF7": [ + "USART2_RX" + ], + "AF12": [ + "FSMC_NWAIT" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD7": { + "Name": "PD7", + "AlternateFunctions": { + "AF7": [ + "USART2_CK" + ], + "AF12": [ + "FSMC_NE1", + "FSMC_NCE2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD8": { + "Name": "PD8", + "AlternateFunctions": { + "AF7": [ + "USART3_TX" + ], + "AF12": [ + "FSMC_D13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD9": { + "Name": "PD9", + "AlternateFunctions": { + "AF7": [ + "USART3_RX" + ], + "AF12": [ + "FSMC_D14" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD10": { + "Name": "PD10", + "AlternateFunctions": { + "AF7": [ + "USART3_CK" + ], + "AF12": [ + "FSMC_D15" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD11": { + "Name": "PD11", + "AlternateFunctions": { + "AF7": [ + "USART3_CTS" + ], + "AF12": [ + "FSMC_A16" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD12": { + "Name": "PD12", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH1" + ], + "AF7": [ + "USART3_RTS" + ], + "AF12": [ + "FSMC_A17" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD13": { + "Name": "PD13", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH2" + ], + "AF12": [ + "FSMC_A18" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD14": { + "Name": "PD14", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH3" + ], + "AF12": [ + "FSMC_D0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD15": { + "Name": "PD15", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH4" + ], + "AF12": [ + "FSMC_D1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE0": { + "Name": "PE0", + "AlternateFunctions": { + "AF2": [ + "TIM4_ETR" + ], + "AF12": [ + "FSMC_NBL0" + ], + "AF13": [ + "DCMI_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE1": { + "Name": "PE1", + "AlternateFunctions": { + "AF12": [ + "FSMC_NBL1" + ], + "AF13": [ + "DCMI_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE2": { + "Name": "PE2", + "AlternateFunctions": { + "AF0": [ + "TRACECLK" + ], + "AF11": [ + "ETH", + "_MII_TXD3" + ], + "AF12": [ + "FSMC_A23" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE3": { + "Name": "PE3", + "AlternateFunctions": { + "AF0": [ + "TRACED0" + ], + "AF12": [ + "FSMC_A19" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE4": { + "Name": "PE4", + "AlternateFunctions": { + "AF0": [ + "TRACED1" + ], + "AF12": [ + "FSMC_A20" + ], + "AF13": [ + "DCMI_D4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE5": { + "Name": "PE5", + "AlternateFunctions": { + "AF0": [ + "TRACED2" + ], + "AF3": [ + "TIM9_CH1" + ], + "AF12": [ + "FSMC_A21" + ], + "AF13": [ + "DCMI_D6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE6": { + "Name": "PE6", + "AlternateFunctions": { + "AF0": [ + "TRACED3" + ], + "AF3": [ + "TIM9_CH2" + ], + "AF12": [ + "FSMC_A22" + ], + "AF13": [ + "DCMI_D7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE7": { + "Name": "PE7", + "AlternateFunctions": { + "AF1": [ + "TIM1_ETR" + ], + "AF12": [ + "FSMC_D4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE8": { + "Name": "PE8", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF12": [ + "FSMC_D5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE9": { + "Name": "PE9", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1" + ], + "AF12": [ + "FSMC_D6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE10": { + "Name": "PE10", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF12": [ + "FSMC_D7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE11": { + "Name": "PE11", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2" + ], + "AF12": [ + "FSMC_D8" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE12": { + "Name": "PE12", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3N" + ], + "AF12": [ + "FSMC_D9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE13": { + "Name": "PE13", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3" + ], + "AF12": [ + "FSMC_D10" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE14": { + "Name": "PE14", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH4" + ], + "AF12": [ + "FSMC_D11" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE15": { + "Name": "PE15", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF12": [ + "FSMC_D12" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF0": { + "Name": "PF0", + "AlternateFunctions": { + "AF4": [ + "I2C2_SDA" + ], + "AF12": [ + "FSMC_A0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF1": { + "Name": "PF1", + "AlternateFunctions": { + "AF4": [ + "I2C2_SCL" + ], + "AF12": [ + "FSMC_A1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF2": { + "Name": "PF2", + "AlternateFunctions": { + "AF4": [ + "I2C2_SMBA" + ], + "AF12": [ + "FSMC_A2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF3": { + "Name": "PF3", + "AlternateFunctions": { + "AF12": [ + "FSMC_A3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF4": { + "Name": "PF4", + "AlternateFunctions": { + "AF12": [ + "FSMC_A4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF5": { + "Name": "PF5", + "AlternateFunctions": { + "AF12": [ + "FSMC_A5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF6": { + "Name": "PF6", + "AlternateFunctions": { + "AF3": [ + "TIM10_CH1" + ], + "AF12": [ + "FSMC_NIORD" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF7": { + "Name": "PF7", + "AlternateFunctions": { + "AF3": [ + "TIM11_CH1" + ], + "AF12": [ + "FSMC_NREG" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF8": { + "Name": "PF8", + "AlternateFunctions": { + "AF9": [ + "TIM13_CH1" + ], + "AF12": [ + "FSMC_NIOWR" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF9": { + "Name": "PF9", + "AlternateFunctions": { + "AF9": [ + "TIM14_CH1" + ], + "AF12": [ + "FSMC_CD" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF10": { + "Name": "PF10", + "AlternateFunctions": { + "AF12": [ + "FSMC_INTR" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF11": { + "Name": "PF11", + "AlternateFunctions": { + "AF13": [ + "DCMI_D12" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF12": { + "Name": "PF12", + "AlternateFunctions": { + "AF12": [ + "FSMC_A6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF13": { + "Name": "PF13", + "AlternateFunctions": { + "AF12": [ + "FSMC_A7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF14": { + "Name": "PF14", + "AlternateFunctions": { + "AF12": [ + "FSMC_A8" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF15": { + "Name": "PF15", + "AlternateFunctions": { + "AF12": [ + "FSMC_A9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG0": { + "Name": "PG0", + "AlternateFunctions": { + "AF12": [ + "FSMC_A10" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG1": { + "Name": "PG1", + "AlternateFunctions": { + "AF12": [ + "FSMC_A11" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG2": { + "Name": "PG2", + "AlternateFunctions": { + "AF12": [ + "FSMC_A12" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG3": { + "Name": "PG3", + "AlternateFunctions": { + "AF12": [ + "FSMC_A13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG4": { + "Name": "PG4", + "AlternateFunctions": { + "AF12": [ + "FSMC_A14" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG5": { + "Name": "PG5", + "AlternateFunctions": { + "AF12": [ + "FSMC_A15" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG6": { + "Name": "PG6", + "AlternateFunctions": { + "AF12": [ + "FSMC_INT2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG7": { + "Name": "PG7", + "AlternateFunctions": { + "AF8": [ + "USART6_CK" + ], + "AF12": [ + "FSMC_INT3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG8": { + "Name": "PG8", + "AlternateFunctions": { + "AF8": [ + "USART6_RTS" + ], + "AF11": [ + "ETH_PPS_OUT" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG9": { + "Name": "PG9", + "AlternateFunctions": { + "AF8": [ + "USART6_RX" + ], + "AF12": [ + "FSMC_NE2", + "FSMC_NCE3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG10": { + "Name": "PG10", + "AlternateFunctions": { + "AF12": [ + "FSMC_NCE4_1", + "FSMC_NE3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG11": { + "Name": "PG11", + "AlternateFunctions": { + "AF11": [ + "ETH_MII_TX_ENETH_RMII_TX_EN" + ], + "AF12": [ + "FSMC_NCE4_2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG12": { + "Name": "PG12", + "AlternateFunctions": { + "AF8": [ + "USART6_RTS" + ], + "AF12": [ + "FSMC_NE4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG13": { + "Name": "PG13", + "AlternateFunctions": { + "AF8": [ + "UART6_CTS" + ], + "AF11": [ + "ETH_MII_TXD0ETH_RMII_TXD0" + ], + "AF12": [ + "FSMC_A24" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG14": { + "Name": "PG14", + "AlternateFunctions": { + "AF8": [ + "USART6_TX" + ], + "AF11": [ + "ETH_MII_TXD1ETH_RMII_TXD1" + ], + "AF12": [ + "FSMC_A25" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG15": { + "Name": "PG15", + "AlternateFunctions": { + "AF8": [ + "USART6_CTS" + ], + "AF13": [ + "DCMI_D13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH0": { + "Name": "PH0", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH1": { + "Name": "PH1", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH2": { + "Name": "PH2", + "AlternateFunctions": { + "AF11": [ + "ETH_MII_CRS" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH3": { + "Name": "PH3", + "AlternateFunctions": { + "AF11": [ + "ETH_MII_COL" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH4": { + "Name": "PH4", + "AlternateFunctions": { + "AF4": [ + "I2C2_SCL" + ], + "AF10": [ + "OTG_HS_ULPI_NXT" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH5": { + "Name": "PH5", + "AlternateFunctions": { + "AF4": [ + "I2C2_SDA" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH6": { + "Name": "PH6", + "AlternateFunctions": { + "AF4": [ + "I2C2_SMBA" + ], + "AF9": [ + "TIM12_CH1" + ], + "AF11": [ + "ETH_MII_RXD2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH7": { + "Name": "PH7", + "AlternateFunctions": { + "AF4": [ + "I2C3_SCL" + ], + "AF11": [ + "ETH_MII_RXD3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH8": { + "Name": "PH8", + "AlternateFunctions": { + "AF4": [ + "I2C3_SDA" + ], + "AF13": [ + "DCMI_HSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH9": { + "Name": "PH9", + "AlternateFunctions": { + "AF4": [ + "I2C3_SMBA" + ], + "AF9": [ + "TIM12_CH2" + ], + "AF13": [ + "DCMI_D0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH10": { + "Name": "PH10", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH1" + ], + "AF13": [ + "DCMI_D1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH11": { + "Name": "PH11", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH2" + ], + "AF13": [ + "DCMI_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH12": { + "Name": "PH12", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH3" + ], + "AF13": [ + "DCMI_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH13": { + "Name": "PH13", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH1N" + ], + "AF9": [ + "CAN1_TX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH14": { + "Name": "PH14", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH2N" + ], + "AF13": [ + "DCMI_D4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH15": { + "Name": "PH15", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH3N" + ], + "AF13": [ + "DCMI_D11" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI0": { + "Name": "PI0", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH4" + ], + "AF5": [ + "SPI2_NSSI2S2_WS" + ], + "AF13": [ + "DCMI_D13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI1": { + "Name": "PI1", + "AlternateFunctions": { + "AF5": [ + "SPI2_SCKI2S2_CK" + ], + "AF13": [ + "DCMI_D8" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI2": { + "Name": "PI2", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH4" + ], + "AF5": [ + "SPI2_MISO" + ], + "AF6": [ + "I2S2ext_SD" + ], + "AF13": [ + "DCMI_D9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI3": { + "Name": "PI3", + "AlternateFunctions": { + "AF3": [ + "TIM8_ETR" + ], + "AF5": [ + "SPI2_MOSII2S2_SD" + ], + "AF13": [ + "DCMI_D10" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI4": { + "Name": "PI4", + "AlternateFunctions": { + "AF3": [ + "TIM8_BKIN" + ], + "AF13": [ + "DCMI_D5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI5": { + "Name": "PI5", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH1" + ], + "AF13": [ + "DCMI_VSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI6": { + "Name": "PI6", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH2" + ], + "AF13": [ + "DCMI_D6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI7": { + "Name": "PI7", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH3" + ], + "AF13": [ + "DCMI_D7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI8": { + "Name": "PI8", + "AlternateFunctions": {} + }, + "PI9": { + "Name": "PI9", + "AlternateFunctions": { + "AF9": [ + "CAN1_RX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI10": { + "Name": "PI10", + "AlternateFunctions": { + "AF11": [ + "ETH_MII_RX_ER" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI11": { + "Name": "PI11", + "AlternateFunctions": { + "AF10": [ + "OTG_HS_ULPI_DIR" + ], + "AF15": [ + "EVENTOUT" + ] + } + } + } + }, + { + "Name": "STM32F722", + "Pins": { + "PA0": { + "Name": "PA0", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH1", + "TIM2_ETR" + ], + "AF2": [ + "TIM5_CH1" + ], + "AF3": [ + "TIM8_ETR" + ], + "AF7": [ + "USART2_CTS" + ], + "AF8": [ + "UART4_TX" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA1": { + "Name": "PA1", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH2" + ], + "AF2": [ + "TIM5_CH2" + ], + "AF7": [ + "USART2_RTS" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "QUADSPI_BK1_IO3" + ], + "AF10": [ + "SAI2_MCK_B" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA2": { + "Name": "PA2", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH3" + ], + "AF2": [ + "TIM5_CH3" + ], + "AF3": [ + "TIM9_CH1" + ], + "AF7": [ + "USART2_TX" + ], + "AF8": [ + "SAI2_SCK_B" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA3": { + "Name": "PA3", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH4" + ], + "AF2": [ + "TIM5_CH4" + ], + "AF3": [ + "TIM9_CH2" + ], + "AF7": [ + "USART2_RX" + ], + "AF10": [ + "OTG_HS_ULPI_D0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA4": { + "Name": "PA4", + "AlternateFunctions": { + "AF5": [ + "SPI1_NSS", + "I2S1_WS" + ], + "AF6": [ + "SPI3_NSS", + "I2S3_WS" + ], + "AF7": [ + "USART2_CK" + ], + "AF12": [ + "OTG_HS_SOF" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA5": { + "Name": "PA5", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH1", + "TIM2_ETR" + ], + "AF3": [ + "TIM8_CH1N" + ], + "AF5": [ + "SPI1_SCK", + "I2S1_CK" + ], + "AF10": [ + "OTG_HS_ULPI_CK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA6": { + "Name": "PA6", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF2": [ + "TIM3_CH1" + ], + "AF3": [ + "TIM8_BKIN" + ], + "AF5": [ + "SPI1_MISO" + ], + "AF9": [ + "TIM13_CH1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA7": { + "Name": "PA7", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF2": [ + "TIM3_CH2" + ], + "AF3": [ + "TIM8_CH1N" + ], + "AF5": [ + "SPI1_MOSI", + "I2S1_SD" + ], + "AF9": [ + "TIM14_CH1" + ], + "AF12": [ + "FMC_SDNWE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA8": { + "Name": "PA8", + "AlternateFunctions": { + "AF0": [ + "MCO1" + ], + "AF1": [ + "TIM1_CH1" + ], + "AF3": [ + "TIM8_BKIN2" + ], + "AF4": [ + "I2C3_SCL" + ], + "AF7": [ + "USART1_CK" + ], + "AF10": [ + "OTG_FS_SOF" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA9": { + "Name": "PA9", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2" + ], + "AF4": [ + "I2C3_SMBA" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF7": [ + "USART1_TX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA10": { + "Name": "PA10", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3" + ], + "AF7": [ + "USART1_RX" + ], + "AF10": [ + "OTG_FS_ID" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA11": { + "Name": "PA11", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH4" + ], + "AF7": [ + "USART1_CTS" + ], + "AF9": [ + "CAN1_RX" + ], + "AF10": [ + "OTG_FS_DM" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA12": { + "Name": "PA12", + "AlternateFunctions": { + "AF1": [ + "TIM1_ETR" + ], + "AF7": [ + "USART1_RTS" + ], + "AF8": [ + "SAI2_FS_B" + ], + "AF9": [ + "CAN1_TX" + ], + "AF10": [ + "OTG_FS_DP" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA13": { + "Name": "PA13", + "AlternateFunctions": { + "AF0": [ + "JTMS-SWDIO" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA14": { + "Name": "PA14", + "AlternateFunctions": { + "AF0": [ + "JTCK-SWCLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA15": { + "Name": "PA15", + "AlternateFunctions": { + "AF0": [ + "JTDI" + ], + "AF1": [ + "TIM2_CH1", + "TIM2_ETR" + ], + "AF5": [ + "SPI1_NSS", + "I2S1_WS" + ], + "AF6": [ + "SPI3_NSS", + "I2S3_WS" + ], + "AF8": [ + "UART4_RTS" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB0": { + "Name": "PB0", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF2": [ + "TIM3_CH3" + ], + "AF3": [ + "TIM8_CH2N" + ], + "AF8": [ + "UART4_CTS" + ], + "AF10": [ + "OTG_HS_ULPI_D1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB1": { + "Name": "PB1", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3N" + ], + "AF2": [ + "TIM3_CH4" + ], + "AF3": [ + "TIM8_CH3N" + ], + "AF10": [ + "OTG_HS_ULPI_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB2": { + "Name": "PB2", + "AlternateFunctions": { + "AF6": [ + "SAI1_SD_A" + ], + "AF7": [ + "SPI3_MOSI", + "I2S3_SD" + ], + "AF9": [ + "QUADSPI_CLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB3": { + "Name": "PB3", + "AlternateFunctions": { + "AF0": [ + "JTDO", + "TRACESWO" + ], + "AF1": [ + "TIM2_CH2" + ], + "AF5": [ + "SPI1_SCK", + "I2S1_CK" + ], + "AF6": [ + "SPI3_SCK", + "I2S3_CK" + ], + "AF10": [ + "SDMMC2_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB4": { + "Name": "PB4", + "AlternateFunctions": { + "AF0": [ + "NJTRST" + ], + "AF2": [ + "TIM3_CH1" + ], + "AF5": [ + "SPI1_MISO" + ], + "AF6": [ + "SPI3_MISO" + ], + "AF7": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF10": [ + "SDMMC2_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB5": { + "Name": "PB5", + "AlternateFunctions": { + "AF2": [ + "TIM3_CH2" + ], + "AF4": [ + "I2C1_SMBA" + ], + "AF5": [ + "SPI1_MOSI", + "I2S1_SD" + ], + "AF6": [ + "SPI3_MOSI", + "I2S3_SD" + ], + "AF10": [ + "OTG_HS_ULPI_D7" + ], + "AF12": [ + "FMC_SDCKE1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB6": { + "Name": "PB6", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH1" + ], + "AF4": [ + "I2C1_SCL" + ], + "AF7": [ + "USART1_TX" + ], + "AF10": [ + "QUADSPI_BK1_NCS" + ], + "AF12": [ + "FMC_SDNE1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB7": { + "Name": "PB7", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH2" + ], + "AF4": [ + "I2C1_SDA" + ], + "AF7": [ + "USART1_RX" + ], + "AF12": [ + "FMC_NL" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB8": { + "Name": "PB8", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH3" + ], + "AF3": [ + "TIM10_CH1" + ], + "AF4": [ + "I2C1_SCL" + ], + "AF9": [ + "CAN1_RX" + ], + "AF10": [ + "SDMMC2_D4" + ], + "AF12": [ + "SDMMC1_D4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB9": { + "Name": "PB9", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH4" + ], + "AF3": [ + "TIM11_CH1" + ], + "AF4": [ + "I2C1_SDA" + ], + "AF5": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF9": [ + "CAN1_TX" + ], + "AF10": [ + "SDMMC2_D5" + ], + "AF12": [ + "SDMMC1_D5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB10": { + "Name": "PB10", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH3" + ], + "AF4": [ + "I2C2_SCL" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF7": [ + "USART3_TX" + ], + "AF10": [ + "OTG_HS_ULPI_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB11": { + "Name": "PB11", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH4" + ], + "AF4": [ + "I2C2_SDA" + ], + "AF7": [ + "USART3_RX" + ], + "AF10": [ + "OTG_HS_ULPI_D4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB12": { + "Name": "PB12", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF4": [ + "I2C2_SMBA" + ], + "AF5": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF7": [ + "USART3_CK" + ], + "AF10": [ + "OTG_HS_ULPI_D5" + ], + "AF12": [ + "OTG_HS_ID" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB13": { + "Name": "PB13", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF7": [ + "USART3_CTS" + ], + "AF10": [ + "OTG_HS_ULPI_D6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB14": { + "Name": "PB14", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF3": [ + "TIM8_CH2N" + ], + "AF5": [ + "SPI2_MISO" + ], + "AF7": [ + "USART3_RTS" + ], + "AF9": [ + "TIM12_CH1" + ], + "AF10": [ + "SDMMC2_D0" + ], + "AF12": [ + "OTG_HS_DM" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB15": { + "Name": "PB15", + "AlternateFunctions": { + "AF0": [ + "RTC_REFIN" + ], + "AF1": [ + "TIM1_CH3N" + ], + "AF3": [ + "TIM8_CH3N" + ], + "AF5": [ + "SPI2_MOSI", + "I2S2_SD" + ], + "AF9": [ + "TIM12_CH2" + ], + "AF10": [ + "SDMMC2_D1" + ], + "AF12": [ + "OTG_HS_DP" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC0": { + "Name": "PC0", + "AlternateFunctions": { + "AF8": [ + "SAI2_FS_B" + ], + "AF10": [ + "OTG_HS_ULPI_STP" + ], + "AF12": [ + "FMC_SDNWE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC1": { + "Name": "PC1", + "AlternateFunctions": { + "AF0": [ + "TRACED0" + ], + "AF5": [ + "SPI2_MOSI", + "I2S2_SD" + ], + "AF6": [ + "SAI1_SD_A" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC2": { + "Name": "PC2", + "AlternateFunctions": { + "AF5": [ + "SPI2_MISO" + ], + "AF10": [ + "OTG_HS_ULPI_DIR" + ], + "AF12": [ + "FMC_SDNE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC3": { + "Name": "PC3", + "AlternateFunctions": { + "AF5": [ + "SPI2_MOSI", + "I2S2_SD" + ], + "AF10": [ + "OTG_HS_ULPI_NXT" + ], + "AF12": [ + "FMC_SDCKE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC4": { + "Name": "PC4", + "AlternateFunctions": { + "AF5": [ + "I2S1_MCK" + ], + "AF12": [ + "FMC_SDNE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC5": { + "Name": "PC5", + "AlternateFunctions": { + "AF12": [ + "FMC_SDCKE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC6": { + "Name": "PC6", + "AlternateFunctions": { + "AF2": [ + "TIM3_CH1" + ], + "AF3": [ + "TIM8_CH1" + ], + "AF5": [ + "I2S2_MCK" + ], + "AF8": [ + "USART6_TX" + ], + "AF10": [ + "SDMMC2_D6" + ], + "AF12": [ + "SDMMC1_D6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC7": { + "Name": "PC7", + "AlternateFunctions": { + "AF2": [ + "TIM3_CH2" + ], + "AF3": [ + "TIM8_CH2" + ], + "AF6": [ + "I2S3_MCK" + ], + "AF8": [ + "USART6_RX" + ], + "AF10": [ + "SDMMC2_D7" + ], + "AF12": [ + "SDMMC1_D7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC8": { + "Name": "PC8", + "AlternateFunctions": { + "AF0": [ + "TRACED1" + ], + "AF2": [ + "TIM3_CH3" + ], + "AF3": [ + "TIM8_CH3" + ], + "AF7": [ + "UART5_RTS" + ], + "AF8": [ + "USART6_CK" + ], + "AF12": [ + "SDMMC1_D0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC9": { + "Name": "PC9", + "AlternateFunctions": { + "AF0": [ + "MCO2" + ], + "AF2": [ + "TIM3_CH4" + ], + "AF3": [ + "TIM8_CH4" + ], + "AF4": [ + "I2C3_SDA" + ], + "AF5": [ + "I2S_CKIN" + ], + "AF7": [ + "UART5_CTS" + ], + "AF9": [ + "QUADSPI_BK1_IO0" + ], + "AF12": [ + "SDMMC1_D1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC10": { + "Name": "PC10", + "AlternateFunctions": { + "AF6": [ + "SPI3_SCK", + "I2S3_CK" + ], + "AF7": [ + "USART3_TX" + ], + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "QUADSPI_BK1_IO1" + ], + "AF12": [ + "SDMMC1_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC11": { + "Name": "PC11", + "AlternateFunctions": { + "AF6": [ + "SPI3_MISO" + ], + "AF7": [ + "USART3_RX" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "QUADSPI_BK2_NCS" + ], + "AF12": [ + "SDMMC1_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC12": { + "Name": "PC12", + "AlternateFunctions": { + "AF0": [ + "TRACED3" + ], + "AF6": [ + "SPI3_MOSI", + "I2S3_SD" + ], + "AF7": [ + "USART3_CK" + ], + "AF8": [ + "UART5_TX" + ], + "AF12": [ + "SDMMC1_CK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC13": { + "Name": "PC13", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC14": { + "Name": "PC14", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC15": { + "Name": "PC15", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD2": { + "Name": "PD2", + "AlternateFunctions": { + "AF0": [ + "TRACED2" + ], + "AF2": [ + "TIM3_ETR" + ], + "AF8": [ + "UART5_RX" + ], + "AF12": [ + "SDMMC1_CMD" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD3": { + "Name": "PD3", + "AlternateFunctions": { + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF7": [ + "USART2_CTS" + ], + "AF12": [ + "FMC_CLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD4": { + "Name": "PD4", + "AlternateFunctions": { + "AF7": [ + "USART2_RTS" + ], + "AF12": [ + "FMC_NOE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD5": { + "Name": "PD5", + "AlternateFunctions": { + "AF7": [ + "USART2_TX" + ], + "AF12": [ + "FMC_NWE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD6": { + "Name": "PD6", + "AlternateFunctions": { + "AF5": [ + "SPI3_MOSI", + "I2S3_SD" + ], + "AF6": [ + "SAI1_SD_A" + ], + "AF7": [ + "USART2_RX" + ], + "AF11": [ + "SDMMC2_CK" + ], + "AF12": [ + "FMC_NWAIT" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD7": { + "Name": "PD7", + "AlternateFunctions": { + "AF7": [ + "USART2_CK" + ], + "AF11": [ + "SDMMC2_CMD" + ], + "AF12": [ + "FMC_NE1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD8": { + "Name": "PD8", + "AlternateFunctions": { + "AF7": [ + "USART3_TX" + ], + "AF12": [ + "FMC_D13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD9": { + "Name": "PD9", + "AlternateFunctions": { + "AF7": [ + "USART3_RX" + ], + "AF12": [ + "FMC_D14" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD10": { + "Name": "PD10", + "AlternateFunctions": { + "AF7": [ + "USART3_CK" + ], + "AF12": [ + "FMC_D15" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD14": { + "Name": "PD14", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH3" + ], + "AF8": [ + "UART8_CTS" + ], + "AF12": [ + "FMC_D0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD15": { + "Name": "PD15", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH4" + ], + "AF8": [ + "UART8_RTS" + ], + "AF12": [ + "FMC_D1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE0": { + "Name": "PE0", + "AlternateFunctions": { + "AF2": [ + "TIM4_ETR" + ], + "AF3": [ + "LPTIM1_ETR" + ], + "AF8": [ + "UART8_Rx" + ], + "AF10": [ + "SAI2_MCK_A" + ], + "AF12": [ + "FMC_NBL0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE1": { + "Name": "PE1", + "AlternateFunctions": { + "AF3": [ + "LPTIM1_IN2" + ], + "AF8": [ + "UART8_Tx" + ], + "AF12": [ + "FMC_NBL1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE3": { + "Name": "PE3", + "AlternateFunctions": { + "AF0": [ + "TRACED0" + ], + "AF6": [ + "SAI1_SD_B" + ], + "AF12": [ + "FMC_A19" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE4": { + "Name": "PE4", + "AlternateFunctions": { + "AF0": [ + "TRACED1" + ], + "AF5": [ + "SPI4_NSS" + ], + "AF6": [ + "SAI1_FS_A" + ], + "AF12": [ + "FMC_A20" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE5": { + "Name": "PE5", + "AlternateFunctions": { + "AF0": [ + "TRACED2" + ], + "AF3": [ + "TIM9_CH1" + ], + "AF5": [ + "SPI4_MISO" + ], + "AF6": [ + "SAI1_SCK_A" + ], + "AF12": [ + "FMC_A21" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE6": { + "Name": "PE6", + "AlternateFunctions": { + "AF0": [ + "TRACED3" + ], + "AF1": [ + "TIM1_BKIN2" + ], + "AF3": [ + "TIM9_CH2" + ], + "AF5": [ + "SPI4_MOSI" + ], + "AF6": [ + "SAI1_SD_A" + ], + "AF10": [ + "SAI2_MCK_B" + ], + "AF12": [ + "FMC_A22" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE7": { + "Name": "PE7", + "AlternateFunctions": { + "AF1": [ + "TIM1_ETR" + ], + "AF8": [ + "UART7_Rx" + ], + "AF10": [ + "QUADSPI_BK2_IO0" + ], + "AF12": [ + "FMC_D4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE8": { + "Name": "PE8", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF8": [ + "UART7_Tx" + ], + "AF10": [ + "QUADSPI_BK2_IO1" + ], + "AF12": [ + "FMC_D5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE9": { + "Name": "PE9", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1" + ], + "AF8": [ + "UART7_RTS" + ], + "AF10": [ + "QUADSPI_BK2_IO2" + ], + "AF12": [ + "FMC_D6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE10": { + "Name": "PE10", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF8": [ + "UART7_CTS" + ], + "AF10": [ + "QUADSPI_BK2_IO3" + ], + "AF12": [ + "FMC_D7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE11": { + "Name": "PE11", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2" + ], + "AF5": [ + "SPI4_NSS" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF12": [ + "FMC_D8" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE12": { + "Name": "PE12", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3N" + ], + "AF5": [ + "SPI4_SCK" + ], + "AF10": [ + "SAI2_SCK_B" + ], + "AF12": [ + "FMC_D9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE13": { + "Name": "PE13", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3" + ], + "AF5": [ + "SPI4_MISO" + ], + "AF10": [ + "SAI2_FS_B" + ], + "AF12": [ + "FMC_D10" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE14": { + "Name": "PE14", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH4" + ], + "AF5": [ + "SPI4_MOSI" + ], + "AF10": [ + "SAI2_MCK_B" + ], + "AF12": [ + "FMC_D11" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE15": { + "Name": "PE15", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF12": [ + "FMC_D12" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF0": { + "Name": "PF0", + "AlternateFunctions": { + "AF4": [ + "I2C2_SDA" + ], + "AF12": [ + "FMC_A0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF1": { + "Name": "PF1", + "AlternateFunctions": { + "AF4": [ + "I2C2_SCL" + ], + "AF12": [ + "FMC_A1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF2": { + "Name": "PF2", + "AlternateFunctions": { + "AF4": [ + "I2C2_SMBA" + ], + "AF12": [ + "FMC_A2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF3": { + "Name": "PF3", + "AlternateFunctions": { + "AF12": [ + "FMC_A3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF4": { + "Name": "PF4", + "AlternateFunctions": { + "AF12": [ + "FMC_A4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF5": { + "Name": "PF5", + "AlternateFunctions": { + "AF12": [ + "FMC_A5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF10": { + "Name": "PF10", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF11": { + "Name": "PF11", + "AlternateFunctions": { + "AF5": [ + "SPI5_MOSI" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF12": [ + "FMC_SDNRAS" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF12": { + "Name": "PF12", + "AlternateFunctions": { + "AF12": [ + "FMC_A6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF13": { + "Name": "PF13", + "AlternateFunctions": { + "AF12": [ + "FMC_A7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF14": { + "Name": "PF14", + "AlternateFunctions": { + "AF12": [ + "FMC_A8" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF15": { + "Name": "PF15", + "AlternateFunctions": { + "AF12": [ + "FMC_A9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG0": { + "Name": "PG0", + "AlternateFunctions": { + "AF12": [ + "FMC_A10" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG1": { + "Name": "PG1", + "AlternateFunctions": { + "AF12": [ + "FMC_A11" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG2": { + "Name": "PG2", + "AlternateFunctions": { + "AF12": [ + "FMC_A12" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG3": { + "Name": "PG3", + "AlternateFunctions": { + "AF12": [ + "FMC_A13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG4": { + "Name": "PG4", + "AlternateFunctions": { + "AF12": [ + "FMC_A14", + "FMC_BA0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG5": { + "Name": "PG5", + "AlternateFunctions": { + "AF12": [ + "FMC_A15", + "FMC_BA1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG6": { + "Name": "PG6", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG7": { + "Name": "PG7", + "AlternateFunctions": { + "AF8": [ + "USART6_CK" + ], + "AF12": [ + "FMC_INT" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG8": { + "Name": "PG8", + "AlternateFunctions": { + "AF8": [ + "USART6_RTS" + ], + "AF12": [ + "FMC_SDCLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG10": { + "Name": "PG10", + "AlternateFunctions": { + "AF10": [ + "SAI2_SD_B" + ], + "AF11": [ + "SDMMC2_D1" + ], + "AF12": [ + "FMC_NE3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG11": { + "Name": "PG11", + "AlternateFunctions": { + "AF10": [ + "SDMMC2_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG12": { + "Name": "PG12", + "AlternateFunctions": { + "AF3": [ + "LPTIM1_IN1" + ], + "AF8": [ + "USART6_RTS" + ], + "AF11": [ + "SDMMC2_D3" + ], + "AF12": [ + "FMC_NE4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG13": { + "Name": "PG13", + "AlternateFunctions": { + "AF0": [ + "TRACED0" + ], + "AF3": [ + "LPTIM1_OUT" + ], + "AF8": [ + "USART6_CTS" + ], + "AF12": [ + "FMC_A24" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG15": { + "Name": "PG15", + "AlternateFunctions": { + "AF8": [ + "USART6_CTS" + ], + "AF12": [ + "FMC_SDNCAS" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH0": { + "Name": "PH0", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH1": { + "Name": "PH1", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH4": { + "Name": "PH4", + "AlternateFunctions": { + "AF4": [ + "I2C2_SCL" + ], + "AF10": [ + "OTG_HS_ULPI_NXT" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH5": { + "Name": "PH5", + "AlternateFunctions": { + "AF4": [ + "I2C2_SDA" + ], + "AF5": [ + "SPI5_NSS" + ], + "AF12": [ + "FMC_SDNWE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH7": { + "Name": "PH7", + "AlternateFunctions": { + "AF4": [ + "I2C3_SCL" + ], + "AF5": [ + "SPI5_MISO" + ], + "AF12": [ + "FMC_SDCKE1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH8": { + "Name": "PH8", + "AlternateFunctions": { + "AF4": [ + "I2C3_SDA" + ], + "AF12": [ + "FMC_D16" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH10": { + "Name": "PH10", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH1" + ], + "AF12": [ + "FMC_D18" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH11": { + "Name": "PH11", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH2" + ], + "AF12": [ + "FMC_D19" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH12": { + "Name": "PH12", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH3" + ], + "AF12": [ + "FMC_D20" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH15": { + "Name": "PH15", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH3N" + ], + "AF12": [ + "FMC_D23" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI0": { + "Name": "PI0", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH4" + ], + "AF5": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF12": [ + "FMC_D24" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI1": { + "Name": "PI1", + "AlternateFunctions": { + "AF3": [ + "TIM8_BKIN2" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF12": [ + "FMC_D25" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI2": { + "Name": "PI2", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH4" + ], + "AF5": [ + "SPI2_MISO" + ], + "AF12": [ + "FMC_D26" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI3": { + "Name": "PI3", + "AlternateFunctions": { + "AF3": [ + "TIM8_ETR" + ], + "AF5": [ + "SPI2_MOSI", + "I2S2_SD" + ], + "AF12": [ + "FMC_D27" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI4": { + "Name": "PI4", + "AlternateFunctions": { + "AF3": [ + "TIM8_BKIN" + ], + "AF10": [ + "SAI2_MCK_A" + ], + "AF12": [ + "FMC_NBL2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI5": { + "Name": "PI5", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH1" + ], + "AF10": [ + "SAI2_SCK_A" + ], + "AF12": [ + "FMC_NBL3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI6": { + "Name": "PI6", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH2" + ], + "AF10": [ + "SAI2_SD_A" + ], + "AF12": [ + "FMC_D28" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI7": { + "Name": "PI7", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH3" + ], + "AF10": [ + "SAI2_FS_A" + ], + "AF12": [ + "FMC_D29" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI8": { + "Name": "PI8", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI10": { + "Name": "PI10", + "AlternateFunctions": { + "AF12": [ + "FMC_D31" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD0": { + "Name": "PD0", + "AlternateFunctions": { + "AF9": [ + "CAN1_RX" + ], + "AF12": [ + "FMC_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD1": { + "Name": "PD1", + "AlternateFunctions": { + "AF9": [ + "CAN1_TX" + ], + "AF12": [ + "FMC_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD11": { + "Name": "PD11", + "AlternateFunctions": { + "AF7": [ + "USART3_CTS" + ], + "AF9": [ + "QUADSPI_BK1_IO0" + ], + "AF10": [ + "SAI2_SD_A" + ], + "AF12": [ + "FMC_A16", + "FMC_CLE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD12": { + "Name": "PD12", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH1" + ], + "AF3": [ + "LPTIM1_IN1" + ], + "AF7": [ + "USART3_RTS" + ], + "AF9": [ + "QUADSPI_BK1_IO1" + ], + "AF10": [ + "SAI2_FS_A" + ], + "AF12": [ + "FMC_A17", + "FMC_ALE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD13": { + "Name": "PD13", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH2" + ], + "AF3": [ + "LPTIM1_OUT" + ], + "AF9": [ + "QUADSPI_BK1_IO3" + ], + "AF10": [ + "SAI2_SCK_A" + ], + "AF12": [ + "FMC_A18" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE2": { + "Name": "PE2", + "AlternateFunctions": { + "AF0": [ + "TRACECLK" + ], + "AF5": [ + "SPI4_SCK" + ], + "AF6": [ + "SAI1_MCLK_A" + ], + "AF9": [ + "QUADSPI_BK1_IO2" + ], + "AF12": [ + "FMC_A23" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF6": { + "Name": "PF6", + "AlternateFunctions": { + "AF3": [ + "TIM10_CH1" + ], + "AF5": [ + "SPI5_NSS" + ], + "AF6": [ + "SAI1_SD_B" + ], + "AF8": [ + "UART7_Rx" + ], + "AF9": [ + "QUADSPI_BK1_IO3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF7": { + "Name": "PF7", + "AlternateFunctions": { + "AF3": [ + "TIM11_CH1" + ], + "AF5": [ + "SPI5_SCK" + ], + "AF6": [ + "SAI1_MCLK_B" + ], + "AF8": [ + "UART7_Tx" + ], + "AF9": [ + "QUADSPI_BK1_IO2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF8": { + "Name": "PF8", + "AlternateFunctions": { + "AF5": [ + "SPI5_MISO" + ], + "AF6": [ + "SAI1_SCK_B" + ], + "AF8": [ + "UART7_RTS" + ], + "AF9": [ + "TIM13_CH1" + ], + "AF10": [ + "QUADSPI_BK1_IO0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF9": { + "Name": "PF9", + "AlternateFunctions": { + "AF5": [ + "SPI5_MOSI" + ], + "AF6": [ + "SAI1_FS_B" + ], + "AF8": [ + "UART7_CTS" + ], + "AF9": [ + "TIM14_CH1" + ], + "AF10": [ + "QUADSPI_BK1_IO1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG9": { + "Name": "PG9", + "AlternateFunctions": { + "AF8": [ + "USART6_RX" + ], + "AF9": [ + "QUADSPI_BK2_IO2" + ], + "AF10": [ + "SAI2_FS_B" + ], + "AF11": [ + "SDMMC2_D0" + ], + "AF12": [ + "FMC_NE2", + "FMC_NCE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG14": { + "Name": "PG14", + "AlternateFunctions": { + "AF0": [ + "TRACED1" + ], + "AF3": [ + "LPTIM1_ETR" + ], + "AF8": [ + "USART6_TX" + ], + "AF9": [ + "QUADSPI_BK2_IO3" + ], + "AF12": [ + "FMC_A25" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH2": { + "Name": "PH2", + "AlternateFunctions": { + "AF3": [ + "LPTIM1_IN2" + ], + "AF9": [ + "QUADSPI_BK2_IO0" + ], + "AF10": [ + "SAI2_SCK_B" + ], + "AF12": [ + "FMC_SDCKE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH3": { + "Name": "PH3", + "AlternateFunctions": { + "AF9": [ + "QUADSPI_BK2_IO1" + ], + "AF10": [ + "SAI2_MCK_B" + ], + "AF12": [ + "FMC_SDNE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH6": { + "Name": "PH6", + "AlternateFunctions": { + "AF4": [ + "I2C2_SMBA" + ], + "AF5": [ + "SPI5_SCK" + ], + "AF9": [ + "TIM12_CH1" + ], + "AF12": [ + "FMC_SDNE1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH9": { + "Name": "PH9", + "AlternateFunctions": { + "AF4": [ + "I2C3_SMBA" + ], + "AF9": [ + "TIM12_CH2" + ], + "AF12": [ + "FMC_D17" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH13": { + "Name": "PH13", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH1N" + ], + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "CAN1_TX" + ], + "AF12": [ + "FMC_D21" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH14": { + "Name": "PH14", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH2N" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "CAN1_RX" + ], + "AF12": [ + "FMC_D22" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI9": { + "Name": "PI9", + "AlternateFunctions": { + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "CAN1_RX" + ], + "AF12": [ + "FMC_D30" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI11": { + "Name": "PI11", + "AlternateFunctions": { + "AF10": [ + "OTG_HS_ULPI_DIR" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI12": { + "Name": "PI12", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI13": { + "Name": "PI13", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI14": { + "Name": "PI14", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI15": { + "Name": "PI15", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + } + } + }, + { + "Name": "STM32F745", + "Pins": { + "PA0": { + "Name": "PA0", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH1", + "TIM2_ETR" + ], + "AF2": [ + "TIM5_CH1" + ], + "AF3": [ + "TIM8_ETR" + ], + "AF7": [ + "USART2_CTS" + ], + "AF8": [ + "UART4_TX" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF11": [ + "ETH_MII_CRS" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA1": { + "Name": "PA1", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH2" + ], + "AF2": [ + "TIM5_CH2" + ], + "AF7": [ + "USART2_RTS" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "QUADSPI_BK1_IO3" + ], + "AF10": [ + "SAI2_MCK_B" + ], + "AF11": [ + "ETH_MII_RX_CLK", + "ETH_RMII_REF_CLK" + ], + "AF14": [ + "LCD_R2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA2": { + "Name": "PA2", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH3" + ], + "AF2": [ + "TIM5_CH3" + ], + "AF3": [ + "TIM9_CH1" + ], + "AF7": [ + "USART2_TX" + ], + "AF8": [ + "SAI2_SCK_B" + ], + "AF11": [ + "ETH_MDIO" + ], + "AF12": [ + "MDIOS_MDIO" + ], + "AF14": [ + "LCD_R1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA3": { + "Name": "PA3", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH4" + ], + "AF2": [ + "TIM5_CH4" + ], + "AF3": [ + "TIM9_CH2" + ], + "AF7": [ + "USART2_RX" + ], + "AF9": [ + "LCD_B2" + ], + "AF10": [ + "OTG_HS_ULPI_D0" + ], + "AF11": [ + "ETH_MII_COL" + ], + "AF14": [ + "LCD_B5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA4": { + "Name": "PA4", + "AlternateFunctions": { + "AF5": [ + "SPI1_NSS", + "I2S1_WS" + ], + "AF6": [ + "SPI3_NSS", + "I2S3_WS" + ], + "AF7": [ + "USART2_CK" + ], + "AF8": [ + "SPI6_NSS" + ], + "AF12": [ + "OTG_HS_SOF" + ], + "AF13": [ + "DCMI_HSYNC" + ], + "AF14": [ + "LCD_VSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA5": { + "Name": "PA5", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH1", + "TIM2_ETR" + ], + "AF3": [ + "TIM8_CH1N" + ], + "AF5": [ + "SPI1_SCK", + "I2S1_CK" + ], + "AF8": [ + "SPI6_SCK" + ], + "AF10": [ + "OTG_HS_ULPI_CK" + ], + "AF14": [ + "LCD_R4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA6": { + "Name": "PA6", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF2": [ + "TIM3_CH1" + ], + "AF3": [ + "TIM8_BKIN" + ], + "AF5": [ + "SPI1_MISO" + ], + "AF8": [ + "SPI6_MISO" + ], + "AF9": [ + "TIM13_CH1" + ], + "AF12": [ + "MDIOS_MDC" + ], + "AF13": [ + "DCMI_PIXCLK" + ], + "AF14": [ + "LCD_G2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA7": { + "Name": "PA7", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF2": [ + "TIM3_CH2" + ], + "AF3": [ + "TIM8_CH1N" + ], + "AF5": [ + "SPI1_MOSI", + "I2S1_SD" + ], + "AF8": [ + "SPI6_MOSI" + ], + "AF9": [ + "TIM14_CH1" + ], + "AF11": [ + "ETH_MII_RX_DV", + "ETH_RMII_CRS_DV" + ], + "AF12": [ + "FMC_SDNWE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA8": { + "Name": "PA8", + "AlternateFunctions": { + "AF0": [ + "MCO1" + ], + "AF1": [ + "TIM1_CH1" + ], + "AF3": [ + "TIM8_BKIN2" + ], + "AF4": [ + "I2C3_SCL" + ], + "AF7": [ + "USART1_CK" + ], + "AF10": [ + "OTG_FS_SOF" + ], + "AF11": [ + "CAN3_RX" + ], + "AF12": [ + "UART7_RX" + ], + "AF13": [ + "LCD_B3" + ], + "AF14": [ + "LCD_R6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA9": { + "Name": "PA9", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2" + ], + "AF4": [ + "I2C3_SMBA" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF7": [ + "USART1_TX" + ], + "AF13": [ + "DCMI_D0" + ], + "AF14": [ + "LCD_R5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA10": { + "Name": "PA10", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3" + ], + "AF7": [ + "USART1_RX" + ], + "AF9": [ + "LCD_B4" + ], + "AF10": [ + "OTG_FS_ID" + ], + "AF12": [ + "MDIOS_MDIO" + ], + "AF13": [ + "DCMI_D1" + ], + "AF14": [ + "LCD_B1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA11": { + "Name": "PA11", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH4" + ], + "AF5": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF6": [ + "UART4_RX" + ], + "AF7": [ + "USART1_CTS" + ], + "AF9": [ + "CAN1_RX" + ], + "AF10": [ + "OTG_FS_DM" + ], + "AF14": [ + "LCD_R4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA12": { + "Name": "PA12", + "AlternateFunctions": { + "AF1": [ + "TIM1_ETR" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF6": [ + "UART4_TX" + ], + "AF7": [ + "USART1_RTS" + ], + "AF8": [ + "SAI2_FS_B" + ], + "AF9": [ + "CAN1_TX" + ], + "AF10": [ + "OTG_FS_DP" + ], + "AF14": [ + "LCD_R5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA13": { + "Name": "PA13", + "AlternateFunctions": { + "AF0": [ + "JTMS-SWDIO" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA14": { + "Name": "PA14", + "AlternateFunctions": { + "AF0": [ + "JTCK-SWCLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA15": { + "Name": "PA15", + "AlternateFunctions": { + "AF0": [ + "JTDI" + ], + "AF1": [ + "TIM2_CH1", + "TIM2_ETR" + ], + "AF4": [ + "HDMI-CEC" + ], + "AF5": [ + "SPI1_NSS", + "I2S1_WS" + ], + "AF6": [ + "SPI3_NSS", + "I2S3_WS" + ], + "AF7": [ + "SPI6_NSS" + ], + "AF8": [ + "UART4_RTS" + ], + "AF11": [ + "CAN3_TX" + ], + "AF12": [ + "UART7_TX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB0": { + "Name": "PB0", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF2": [ + "TIM3_CH3" + ], + "AF3": [ + "TIM8_CH2N" + ], + "AF6": [ + "DFSDM1_CKOUT" + ], + "AF8": [ + "UART4_CTS" + ], + "AF9": [ + "LCD_R3" + ], + "AF10": [ + "OTG_HS_ULPI_D1" + ], + "AF11": [ + "ETH_MII_RXD2" + ], + "AF14": [ + "LCD_G1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB1": { + "Name": "PB1", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3N" + ], + "AF2": [ + "TIM3_CH4" + ], + "AF3": [ + "TIM8_CH3N" + ], + "AF6": [ + "DFSDM1_DATIN1" + ], + "AF9": [ + "LCD_R6" + ], + "AF10": [ + "OTG_HS_ULPI_D2" + ], + "AF11": [ + "ETH_MII_RXD3" + ], + "AF14": [ + "LCD_G0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB2": { + "Name": "PB2", + "AlternateFunctions": { + "AF6": [ + "SAI1_SD_A" + ], + "AF7": [ + "SPI3_MOSI", + "I2S3_SD" + ], + "AF9": [ + "QUADSPI_CLK" + ], + "AF10": [ + "DFSDM1_CKIN1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB3": { + "Name": "PB3", + "AlternateFunctions": { + "AF0": [ + "JTDO", + "TRACESWO" + ], + "AF1": [ + "TIM2_CH2" + ], + "AF5": [ + "SPI1_SCK", + "I2S1_CK" + ], + "AF6": [ + "SPI3_SCK", + "I2S3_CK" + ], + "AF8": [ + "SPI6_SCK" + ], + "AF10": [ + "SDMMC2_D2" + ], + "AF11": [ + "CAN3_RX" + ], + "AF12": [ + "UART7_RX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB4": { + "Name": "PB4", + "AlternateFunctions": { + "AF0": [ + "NJTRST" + ], + "AF2": [ + "TIM3_CH1" + ], + "AF5": [ + "SPI1_MISO" + ], + "AF6": [ + "SPI3_MISO" + ], + "AF7": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF8": [ + "SPI6_MISO" + ], + "AF10": [ + "SDMMC2_D3" + ], + "AF11": [ + "CAN3_TX" + ], + "AF12": [ + "UART7_TX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB5": { + "Name": "PB5", + "AlternateFunctions": { + "AF1": [ + "UART5_RX" + ], + "AF2": [ + "TIM3_CH2" + ], + "AF4": [ + "I2C1_SMBA" + ], + "AF5": [ + "SPI1_MOSI", + "I2S1_SD" + ], + "AF6": [ + "SPI3_MOSI", + "I2S3_SD" + ], + "AF8": [ + "SPI6_MOSI" + ], + "AF9": [ + "CAN2_RX" + ], + "AF10": [ + "OTG_HS_ULPI_D7" + ], + "AF11": [ + "ETH_PPS_OUT" + ], + "AF12": [ + "FMC_SDCKE1" + ], + "AF13": [ + "DCMI_D10" + ], + "AF14": [ + "LCD_G7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB6": { + "Name": "PB6", + "AlternateFunctions": { + "AF1": [ + "UART5_TX" + ], + "AF2": [ + "TIM4_CH1" + ], + "AF3": [ + "HDMI-CEC" + ], + "AF4": [ + "I2C1_SCL" + ], + "AF6": [ + "DFSDM1_DATIN5" + ], + "AF7": [ + "USART1_TX" + ], + "AF9": [ + "CAN2_TX" + ], + "AF10": [ + "QUADSPI_BK1_NCS" + ], + "AF11": [ + "I2C4_SCL" + ], + "AF12": [ + "FMC_SDNE1" + ], + "AF13": [ + "DCMI_D5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB7": { + "Name": "PB7", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH2" + ], + "AF4": [ + "I2C1_SDA" + ], + "AF6": [ + "DFSDM1_CKIN5" + ], + "AF7": [ + "USART1_RX" + ], + "AF11": [ + "I2S4_SDA" + ], + "AF12": [ + "FMC_NL" + ], + "AF13": [ + "DCMI_VSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB8": { + "Name": "PB8", + "AlternateFunctions": { + "AF1": [ + "I2C4_SCL" + ], + "AF2": [ + "TIM4_CH3" + ], + "AF3": [ + "TIM10_CH1" + ], + "AF4": [ + "I2C1_SCL" + ], + "AF6": [ + "DFSDM1_CKIN7" + ], + "AF7": [ + "UART5_RX" + ], + "AF9": [ + "CAN1_RX" + ], + "AF10": [ + "SDMMC2_D4" + ], + "AF11": [ + "ETH_MII_TXD3" + ], + "AF12": [ + "SDMMC_D4" + ], + "AF13": [ + "DCMI_D6" + ], + "AF14": [ + "LCD_B6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB9": { + "Name": "PB9", + "AlternateFunctions": { + "AF1": [ + "I2C4_SDA" + ], + "AF2": [ + "TIM4_CH4" + ], + "AF3": [ + "TIM11_CH1" + ], + "AF4": [ + "I2C1_SDA" + ], + "AF5": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF6": [ + "DFSDM1_DATIN7" + ], + "AF7": [ + "UART5_TX" + ], + "AF9": [ + "CAN1_TX" + ], + "AF10": [ + "SDMMC2_D5" + ], + "AF11": [ + "I2C4_SMBA" + ], + "AF12": [ + "SDMMC_D5" + ], + "AF13": [ + "DCMI_D7" + ], + "AF14": [ + "LCD_B7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB10": { + "Name": "PB10", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH3" + ], + "AF4": [ + "I2C2_SCL" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF6": [ + "DFSDM1_DATIN7" + ], + "AF7": [ + "USART3_TX" + ], + "AF9": [ + "-QUADSPI_BK1_NCS" + ], + "AF10": [ + "OTG_HS_ULPI_D3" + ], + "AF11": [ + "ETH_MII_RX_ER" + ], + "AF14": [ + "LCD_G4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB11": { + "Name": "PB11", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH4" + ], + "AF4": [ + "I2C2_SDA" + ], + "AF6": [ + "DFSDM1_CKIN7" + ], + "AF7": [ + "USART3_RX" + ], + "AF10": [ + "OTG_HS_ULPI_D4" + ], + "AF11": [ + "ETH_MII_TX_EN", + "ETH_RMII_TX_EN" + ], + "AF13": [ + "DSI_TE" + ], + "AF14": [ + "LCD_G5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB12": { + "Name": "PB12", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF4": [ + "I2C2_SMBA" + ], + "AF5": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF6": [ + "DFSDM1_DATIN1" + ], + "AF7": [ + "USART3_CK" + ], + "AF8": [ + "UART5_RX" + ], + "AF9": [ + "CAN2_RX" + ], + "AF10": [ + "OTG_HS_ULPI_D5" + ], + "AF11": [ + "ETH_MII_TXD0", + "ETH_RMII_TXD0" + ], + "AF12": [ + "OTG_HS_ID" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB13": { + "Name": "PB13", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF6": [ + "DFSDM1_CKIN1" + ], + "AF7": [ + "USART3_CTS" + ], + "AF8": [ + "UART5_TX" + ], + "AF9": [ + "CAN2_TX" + ], + "AF10": [ + "OTG_HS_ULPI_D6" + ], + "AF11": [ + "ETH_MII_TXD1", + "ETH_RMII_TXD1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB14": { + "Name": "PB14", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF3": [ + "TIM8_CH2N" + ], + "AF4": [ + "USART1_TX" + ], + "AF5": [ + "SPI2_MISO" + ], + "AF6": [ + "DFSDM1_DATIN2" + ], + "AF7": [ + "USART3_RTS" + ], + "AF8": [ + "UART4_RTS" + ], + "AF9": [ + "TIM12_CH1" + ], + "AF10": [ + "SDMMC2_D0" + ], + "AF12": [ + "OTG_HS_DM" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB15": { + "Name": "PB15", + "AlternateFunctions": { + "AF0": [ + "RTC_REFIN" + ], + "AF1": [ + "TIM1_CH3N" + ], + "AF3": [ + "TIM8_CH3N" + ], + "AF4": [ + "USART1_RX" + ], + "AF5": [ + "SPI2_MOSI", + "I2S2_SD" + ], + "AF6": [ + "DFSDM1_CKIN2" + ], + "AF8": [ + "UART4_CTS" + ], + "AF9": [ + "TIM12_CH2" + ], + "AF10": [ + "SDMMC2_D1" + ], + "AF12": [ + "OTG_HS_DP" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC0": { + "Name": "PC0", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN0" + ], + "AF6": [ + "DFSDM1_DATIN4" + ], + "AF8": [ + "SAI2_FS_B" + ], + "AF10": [ + "OTG_HS_ULPI_STP" + ], + "AF12": [ + "FMC_SDNWE" + ], + "AF14": [ + "LCD_R5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC1": { + "Name": "PC1", + "AlternateFunctions": { + "AF0": [ + "TRACED0" + ], + "AF3": [ + "DFSDM1_DATAIN0" + ], + "AF5": [ + "SPI2_MOSI", + "I2S2_SD" + ], + "AF6": [ + "SAI1_SD_A" + ], + "AF10": [ + "DFSDM1_CKIN4" + ], + "AF11": [ + "ETH_MDC" + ], + "AF12": [ + "MDIOS_MDC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC2": { + "Name": "PC2", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN1" + ], + "AF5": [ + "SPI2_MISO" + ], + "AF6": [ + "DFSDM1_CKOUT" + ], + "AF10": [ + "OTG_HS_ULPI_DIR" + ], + "AF11": [ + "ETH_MII_TXD2" + ], + "AF12": [ + "FMC_SDNE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC3": { + "Name": "PC3", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_DATAIN1" + ], + "AF5": [ + "SPI2_MOSI", + "I2S2_SD" + ], + "AF10": [ + "OTG_HS_ULPI_NXT" + ], + "AF11": [ + "ETH_MII_TX_CLK" + ], + "AF12": [ + "FMC_SDCKE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC4": { + "Name": "PC4", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN2" + ], + "AF5": [ + "I2S1_MCK" + ], + "AF8": [ + "SPDIF_RX2" + ], + "AF11": [ + "ETH_MII_RXD0", + "ETH_RMII_RXD0" + ], + "AF12": [ + "FMC_SDNE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC5": { + "Name": "PC5", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_DATAIN2" + ], + "AF8": [ + "SPDIF_RX3" + ], + "AF11": [ + "ETH_MII_RXD1", + "ETH_RMII_RXD1" + ], + "AF12": [ + "FMC_SDCKE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC6": { + "Name": "PC6", + "AlternateFunctions": { + "AF2": [ + "TIM3_CH1" + ], + "AF3": [ + "TIM8_CH1" + ], + "AF5": [ + "I2S2_MCK" + ], + "AF7": [ + "DFSDM1_CKIN3" + ], + "AF8": [ + "USART6_TX" + ], + "AF9": [ + "FMC_NWAIT" + ], + "AF10": [ + "SDMMC2_D6" + ], + "AF12": [ + "SDMMC_D6" + ], + "AF13": [ + "DCMI_D0" + ], + "AF14": [ + "LCD_HSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC7": { + "Name": "PC7", + "AlternateFunctions": { + "AF2": [ + "TIM3_CH2" + ], + "AF3": [ + "TIM8_CH2" + ], + "AF6": [ + "I2S3_MCK" + ], + "AF7": [ + "DFSDM1_DATAIN3" + ], + "AF8": [ + "USART6_RX" + ], + "AF9": [ + "FMC_NE1" + ], + "AF10": [ + "SDMMC2_D7" + ], + "AF12": [ + "SDMMC_D7" + ], + "AF13": [ + "DCMI_D1" + ], + "AF14": [ + "LCD_G6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC8": { + "Name": "PC8", + "AlternateFunctions": { + "AF0": [ + "TRACED1" + ], + "AF2": [ + "TIM3_CH3" + ], + "AF3": [ + "TIM8_CH3" + ], + "AF7": [ + "UART5_RTS" + ], + "AF8": [ + "USART6_CK" + ], + "AF9": [ + "FMC_NE2", + "FMC_NCE" + ], + "AF12": [ + "SDMMC_D0" + ], + "AF13": [ + "DCMI_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC9": { + "Name": "PC9", + "AlternateFunctions": { + "AF0": [ + "MCO2" + ], + "AF2": [ + "TIM3_CH4" + ], + "AF3": [ + "TIM8_CH4" + ], + "AF4": [ + "I2C3_SDA" + ], + "AF5": [ + "I2S_CKIN" + ], + "AF7": [ + "UART5_CTS" + ], + "AF9": [ + "QUADSPI_BK1_IO0" + ], + "AF10": [ + "LCD_G3" + ], + "AF12": [ + "SDMMC_D1" + ], + "AF13": [ + "DCMI_D3" + ], + "AF14": [ + "LCD_B2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC10": { + "Name": "PC10", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN5" + ], + "AF6": [ + "SPI3_SCK", + "I2S3_CK" + ], + "AF7": [ + "USART3_TX" + ], + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "QUADSPI_BK1_IO1" + ], + "AF12": [ + "SDMMC_D2" + ], + "AF13": [ + "DCMI_D8" + ], + "AF14": [ + "LCD_R2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC11": { + "Name": "PC11", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_DATAIN5" + ], + "AF6": [ + "SPI3_MISO" + ], + "AF7": [ + "USART3_RX" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "QUADSPI_BK2_NCS" + ], + "AF12": [ + "SDMMC_D3" + ], + "AF13": [ + "DCMI_D4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC12": { + "Name": "PC12", + "AlternateFunctions": { + "AF0": [ + "TRACED3" + ], + "AF6": [ + "SPI3_MOSI", + "I2S3_SD" + ], + "AF7": [ + "USART3_CK" + ], + "AF8": [ + "UART5_TX" + ], + "AF12": [ + "SDMMC_CK" + ], + "AF13": [ + "DCMI_D9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC13": { + "Name": "PC13", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC14": { + "Name": "PC14", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC15": { + "Name": "PC15", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD0": { + "Name": "PD0", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN6" + ], + "AF6": [ + "DFSDM1_DATAIN7" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "CAN1_RX" + ], + "AF12": [ + "FMC_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD1": { + "Name": "PD1", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_DATAIN6" + ], + "AF6": [ + "DFSDM1_CKIN7" + ], + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "CAN1_TX" + ], + "AF12": [ + "FMC_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD2": { + "Name": "PD2", + "AlternateFunctions": { + "AF0": [ + "TRACED2" + ], + "AF2": [ + "TIM3_ETR" + ], + "AF8": [ + "UART5_RX" + ], + "AF12": [ + "SDMMC_CMD" + ], + "AF13": [ + "DCMI_D11" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD3": { + "Name": "PD3", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKOUT" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF6": [ + "DFSDM1_DATAIN0" + ], + "AF7": [ + "USART2_CTS" + ], + "AF12": [ + "FMC_CLK" + ], + "AF13": [ + "DCMI_D5" + ], + "AF14": [ + "LCD_G7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD4": { + "Name": "PD4", + "AlternateFunctions": { + "AF6": [ + "DFSDM1_CKIN0" + ], + "AF7": [ + "USART2_RTS" + ], + "AF12": [ + "FMC_NOE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD5": { + "Name": "PD5", + "AlternateFunctions": { + "AF7": [ + "USART2_TX" + ], + "AF12": [ + "FMC_NWE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD6": { + "Name": "PD6", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN4" + ], + "AF5": [ + "SPI3_MOSI", + "I2S3_SD" + ], + "AF6": [ + "SAI1_SD_A" + ], + "AF7": [ + "USART2_RX" + ], + "AF10": [ + "DFSDM1_DATAIN1" + ], + "AF11": [ + "SDMMC2_CK" + ], + "AF12": [ + "FMC_NWAIT" + ], + "AF13": [ + "DCMI_D10" + ], + "AF14": [ + "LCD_B2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD7": { + "Name": "PD7", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_DATAIN4" + ], + "AF5": [ + "SPI1_MOSI", + "I2S1_SD" + ], + "AF6": [ + "DFSDM1_CKIN1" + ], + "AF7": [ + "USART2_CK" + ], + "AF8": [ + "SPDIF_RX0" + ], + "AF11": [ + "SDMMC2_CMD" + ], + "AF12": [ + "FMC_NE1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD8": { + "Name": "PD8", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN3" + ], + "AF7": [ + "USART3_TX" + ], + "AF8": [ + "SPDIF_RX1" + ], + "AF12": [ + "FMC_D13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD9": { + "Name": "PD9", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_DATAIN3" + ], + "AF7": [ + "USART3_RX" + ], + "AF12": [ + "FMC_D14" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD10": { + "Name": "PD10", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKOUT" + ], + "AF7": [ + "USART3_CK" + ], + "AF12": [ + "FMC_D15" + ], + "AF14": [ + "LCD_B3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD11": { + "Name": "PD11", + "AlternateFunctions": { + "AF4": [ + "I2C4_SMBA" + ], + "AF7": [ + "USART3_CTS" + ], + "AF9": [ + "QUADSPI_BK1_IO0" + ], + "AF10": [ + "SAI2_SD_A" + ], + "AF12": [ + "FMC_A16", + "FMC_CLE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD12": { + "Name": "PD12", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH1" + ], + "AF3": [ + "LPTIM1_IN1" + ], + "AF4": [ + "I2C4_SCL" + ], + "AF7": [ + "USART3_RTS" + ], + "AF9": [ + "QUADSPI_BK1_IO1" + ], + "AF10": [ + "SAI2_FS_A" + ], + "AF12": [ + "FMC_A17", + "FMC_ALE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD13": { + "Name": "PD13", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH2" + ], + "AF3": [ + "LPTIM1_OUT" + ], + "AF4": [ + "I2C4_SDA" + ], + "AF9": [ + "QUADSPI_BK1_IO3" + ], + "AF10": [ + "SAI2_SCK_A" + ], + "AF12": [ + "FMC_A18" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD14": { + "Name": "PD14", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH3" + ], + "AF8": [ + "UART8_CTS" + ], + "AF12": [ + "FMC_D0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD15": { + "Name": "PD15", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH4" + ], + "AF8": [ + "UART8_RTS" + ], + "AF12": [ + "FMC_D1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE0": { + "Name": "PE0", + "AlternateFunctions": { + "AF2": [ + "TIM4_ETR" + ], + "AF3": [ + "LPTIM1_ETR" + ], + "AF8": [ + "UART8_Rx" + ], + "AF10": [ + "SAI2_MCK_A" + ], + "AF12": [ + "FMC_NBL0" + ], + "AF13": [ + "DCMI_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE1": { + "Name": "PE1", + "AlternateFunctions": { + "AF3": [ + "LPTIM1_IN2" + ], + "AF8": [ + "UART8_Tx" + ], + "AF12": [ + "FMC_NBL1" + ], + "AF13": [ + "DCMI_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE2": { + "Name": "PE2", + "AlternateFunctions": { + "AF0": [ + "TRACECLK" + ], + "AF5": [ + "SPI4_SCK" + ], + "AF6": [ + "SAI1_MCLK_A" + ], + "AF9": [ + "QUADSPI_BK1_IO2" + ], + "AF11": [ + "ETH_MII_TXD3" + ], + "AF12": [ + "FMC_A23" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE3": { + "Name": "PE3", + "AlternateFunctions": { + "AF0": [ + "TRACED0" + ], + "AF6": [ + "SAI1_SD_B" + ], + "AF12": [ + "FMC_A19" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE4": { + "Name": "PE4", + "AlternateFunctions": { + "AF0": [ + "TRACED1" + ], + "AF5": [ + "SPI4_NSS" + ], + "AF6": [ + "SAI1_FS_A" + ], + "AF10": [ + "DFSDM1_DATAIN3" + ], + "AF12": [ + "FMC_A20" + ], + "AF13": [ + "DCMI_D4" + ], + "AF14": [ + "LCD_B0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE5": { + "Name": "PE5", + "AlternateFunctions": { + "AF0": [ + "TRACED2" + ], + "AF3": [ + "TIM9_CH1" + ], + "AF5": [ + "SPI4_MISO" + ], + "AF6": [ + "SAI1_SCK_A" + ], + "AF10": [ + "DFSDM1_CKIN3" + ], + "AF12": [ + "FMC_A21" + ], + "AF13": [ + "DCMI_D6" + ], + "AF14": [ + "LCD_G0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE6": { + "Name": "PE6", + "AlternateFunctions": { + "AF0": [ + "TRACED3" + ], + "AF1": [ + "TIM1_BKIN2" + ], + "AF3": [ + "TIM9_CH2" + ], + "AF5": [ + "SPI4_MOSI" + ], + "AF6": [ + "SAI1_SD_A" + ], + "AF10": [ + "SAI2_MCK_B" + ], + "AF12": [ + "FMC_A22" + ], + "AF13": [ + "DCMI_D7" + ], + "AF14": [ + "LCD_G1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE7": { + "Name": "PE7", + "AlternateFunctions": { + "AF1": [ + "TIM1_ETR" + ], + "AF6": [ + "DFSDM1_DATAIN2" + ], + "AF8": [ + "UART7_Rx" + ], + "AF10": [ + "QUADSPI_BK2_IO0" + ], + "AF12": [ + "FMC_D4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE8": { + "Name": "PE8", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF6": [ + "DFSDM1_CKIN2" + ], + "AF8": [ + "UART7_Tx" + ], + "AF10": [ + "QUADSPI_BK2_IO1" + ], + "AF12": [ + "FMC_D5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE9": { + "Name": "PE9", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1" + ], + "AF6": [ + "DFSDM1_CKOUT" + ], + "AF8": [ + "UART7_RTS" + ], + "AF10": [ + "QUADSPI_BK2_IO2" + ], + "AF12": [ + "FMC_D6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE10": { + "Name": "PE10", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF6": [ + "DFSDM1_DATAIN4" + ], + "AF8": [ + "UART7_CTS" + ], + "AF10": [ + "QUADSPI_BK2_IO3" + ], + "AF12": [ + "FMC_D7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE11": { + "Name": "PE11", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2" + ], + "AF5": [ + "SPI4_NSS" + ], + "AF6": [ + "DFSDM1_CKIN4" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF12": [ + "FMC_D8" + ], + "AF14": [ + "LCD_G3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE12": { + "Name": "PE12", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3N" + ], + "AF5": [ + "SPI4_SCK" + ], + "AF6": [ + "DFSDM1_DATAIN5" + ], + "AF10": [ + "SAI2_SCK_B" + ], + "AF12": [ + "FMC_D9" + ], + "AF14": [ + "LCD_B4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE13": { + "Name": "PE13", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3" + ], + "AF5": [ + "SPI4_MISO" + ], + "AF6": [ + "DFSDM1_CKIN5" + ], + "AF10": [ + "SAI2_FS_B" + ], + "AF12": [ + "FMC_D10" + ], + "AF14": [ + "LCD_DE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE14": { + "Name": "PE14", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH4" + ], + "AF5": [ + "SPI4_MOSI" + ], + "AF10": [ + "SAI2_MCK_B" + ], + "AF12": [ + "FMC_D11" + ], + "AF14": [ + "LCD_CLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE15": { + "Name": "PE15", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF12": [ + "FMC_D12" + ], + "AF14": [ + "LCD_R7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF0": { + "Name": "PF0", + "AlternateFunctions": { + "AF4": [ + "I2C2_SDA" + ], + "AF12": [ + "FMC_A0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF1": { + "Name": "PF1", + "AlternateFunctions": { + "AF4": [ + "I2C2_SCL" + ], + "AF12": [ + "FMC_A1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF2": { + "Name": "PF2", + "AlternateFunctions": { + "AF4": [ + "I2C2_SMBA" + ], + "AF12": [ + "FMC_A2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF3": { + "Name": "PF3", + "AlternateFunctions": { + "AF12": [ + "FMC_A3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF4": { + "Name": "PF4", + "AlternateFunctions": { + "AF12": [ + "FMC_A4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF5": { + "Name": "PF5", + "AlternateFunctions": { + "AF12": [ + "FMC_A5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF6": { + "Name": "PF6", + "AlternateFunctions": { + "AF3": [ + "TIM10_CH1" + ], + "AF5": [ + "SPI5_NSS" + ], + "AF6": [ + "SAI1_SD_B" + ], + "AF8": [ + "UART7_Rx" + ], + "AF9": [ + "QUADSPI_BK1_IO3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF7": { + "Name": "PF7", + "AlternateFunctions": { + "AF3": [ + "TIM11_CH1" + ], + "AF5": [ + "SPI5_SCK" + ], + "AF6": [ + "SAI1_MCLK_B" + ], + "AF8": [ + "UART7_Tx" + ], + "AF9": [ + "QUADSPI_BK1_IO2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF8": { + "Name": "PF8", + "AlternateFunctions": { + "AF5": [ + "SPI5_MISO" + ], + "AF6": [ + "SAI1_SCK_B" + ], + "AF8": [ + "UART7_RTS" + ], + "AF9": [ + "TIM13_CH1" + ], + "AF10": [ + "QUADSPI_BK1_IO0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF9": { + "Name": "PF9", + "AlternateFunctions": { + "AF5": [ + "SPI5_MOSI" + ], + "AF6": [ + "SAI1_FS_B" + ], + "AF8": [ + "UART7_CTS" + ], + "AF9": [ + "TIM14_CH1" + ], + "AF10": [ + "QUADSPI_BK1_IO1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF10": { + "Name": "PF10", + "AlternateFunctions": { + "AF9": [ + "QUADSPI_CLK" + ], + "AF13": [ + "DCMI_D11" + ], + "AF14": [ + "LCD_DE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF11": { + "Name": "PF11", + "AlternateFunctions": { + "AF5": [ + "SPI5_MOSI" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF12": [ + "FMC_SDNRAS" + ], + "AF13": [ + "DCMI_D12" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF12": { + "Name": "PF12", + "AlternateFunctions": { + "AF12": [ + "FMC_A6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF13": { + "Name": "PF13", + "AlternateFunctions": { + "AF4": [ + "I2C4_SMBA" + ], + "AF6": [ + "DFSDM1_DATAIN6" + ], + "AF12": [ + "FMC_A7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF14": { + "Name": "PF14", + "AlternateFunctions": { + "AF4": [ + "I2C4_SCL" + ], + "AF6": [ + "DFSDM1_CKIN6" + ], + "AF12": [ + "FMC_A8" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF15": { + "Name": "PF15", + "AlternateFunctions": { + "AF4": [ + "I2C4_SDA" + ], + "AF12": [ + "FMC_A9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG0": { + "Name": "PG0", + "AlternateFunctions": { + "AF12": [ + "FMC_A10" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG1": { + "Name": "PG1", + "AlternateFunctions": { + "AF12": [ + "FMC_A11" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG2": { + "Name": "PG2", + "AlternateFunctions": { + "AF12": [ + "FMC_A12" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG3": { + "Name": "PG3", + "AlternateFunctions": { + "AF12": [ + "FMC_A13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG4": { + "Name": "PG4", + "AlternateFunctions": { + "AF12": [ + "FMC_A14", + "FMC_BA0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG5": { + "Name": "PG5", + "AlternateFunctions": { + "AF12": [ + "FMC_A15", + "FMC_BA1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG6": { + "Name": "PG6", + "AlternateFunctions": { + "AF12": [ + "FMC_NE3" + ], + "AF13": [ + "DCMI_D12" + ], + "AF14": [ + "LCD_R7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG7": { + "Name": "PG7", + "AlternateFunctions": { + "AF6": [ + "SAI1_MCLK_A" + ], + "AF8": [ + "USART6_CK" + ], + "AF12": [ + "FMC_INT" + ], + "AF13": [ + "DCMI_D13" + ], + "AF14": [ + "LCD_CLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG8": { + "Name": "PG8", + "AlternateFunctions": { + "AF5": [ + "SPI6_NSS" + ], + "AF7": [ + "SPDIF_RX2" + ], + "AF8": [ + "USART6_RTS" + ], + "AF11": [ + "ETH_PPS_OUT" + ], + "AF12": [ + "FMC_SDCLK" + ], + "AF14": [ + "LCD_G7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG9": { + "Name": "PG9", + "AlternateFunctions": { + "AF5": [ + "SPI1_MISO" + ], + "AF7": [ + "SPDIF_RX3" + ], + "AF8": [ + "USART6_RX" + ], + "AF9": [ + "QUADSPI_BK2_IO2" + ], + "AF10": [ + "SAI2_FS_B" + ], + "AF11": [ + "SDMMC2_D0" + ], + "AF12": [ + "FMC_NE2", + "FMC_NCE" + ], + "AF13": [ + "DCMI_VSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG10": { + "Name": "PG10", + "AlternateFunctions": { + "AF5": [ + "SPI1_NSS", + "I2S1_WS" + ], + "AF9": [ + "LCD_G3" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF11": [ + "SDMMC2_D1" + ], + "AF12": [ + "FMC_NE3" + ], + "AF13": [ + "DCMI_D2" + ], + "AF14": [ + "LCD_B2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG11": { + "Name": "PG11", + "AlternateFunctions": { + "AF5": [ + "SPI1_SCK", + "I2S1_CK" + ], + "AF7": [ + "SPDIF_RX0" + ], + "AF10": [ + "SDMMC2_D2" + ], + "AF11": [ + "ETH_MII_TX_EN", + "ETH_RMII_TX_EN" + ], + "AF13": [ + "DCMI_D3" + ], + "AF14": [ + "LCD_B3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG12": { + "Name": "PG12", + "AlternateFunctions": { + "AF3": [ + "LPTIM1_IN1" + ], + "AF5": [ + "SPI6_MISO" + ], + "AF7": [ + "SPDIF_RX1" + ], + "AF8": [ + "USART6_RTS" + ], + "AF9": [ + "LCD_B4" + ], + "AF11": [ + "SDMMC2_D3" + ], + "AF12": [ + "FMC_NE4" + ], + "AF14": [ + "LCD_B1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG13": { + "Name": "PG13", + "AlternateFunctions": { + "AF0": [ + "TRACED0" + ], + "AF3": [ + "LPTIM1_OUT" + ], + "AF5": [ + "SPI6_SCK" + ], + "AF8": [ + "USART6_CTS" + ], + "AF11": [ + "ETH_MII_TXD0", + "ETH_RMII_TXD0" + ], + "AF12": [ + "FMC_A24" + ], + "AF14": [ + "LCD_R0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG14": { + "Name": "PG14", + "AlternateFunctions": { + "AF0": [ + "TRACED1" + ], + "AF3": [ + "LPTIM1_ETR" + ], + "AF5": [ + "SPI6_MOSI" + ], + "AF8": [ + "USART6_TX" + ], + "AF9": [ + "QUADSPI_BK2_IO3" + ], + "AF11": [ + "ETH_MII_TXD1", + "ETH_RMII_TXD1" + ], + "AF12": [ + "FMC_A25" + ], + "AF14": [ + "LCD_B0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG15": { + "Name": "PG15", + "AlternateFunctions": { + "AF8": [ + "USART6_CTS" + ], + "AF12": [ + "FMC_SDNCAS" + ], + "AF13": [ + "DCMI_D13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH0": { + "Name": "PH0", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH1": { + "Name": "PH1", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH2": { + "Name": "PH2", + "AlternateFunctions": { + "AF3": [ + "LPTIM1_IN2" + ], + "AF9": [ + "QUADSPI_BK2_IO0" + ], + "AF10": [ + "SAI2_SCK_B" + ], + "AF11": [ + "ETH_MII_CRS" + ], + "AF12": [ + "FMC_SDCKE0" + ], + "AF14": [ + "LCD_R0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH3": { + "Name": "PH3", + "AlternateFunctions": { + "AF9": [ + "QUADSPI_BK2_IO1" + ], + "AF10": [ + "SAI2_MCK_B" + ], + "AF11": [ + "ETH_MII_COL" + ], + "AF12": [ + "FMC_SDNE0" + ], + "AF14": [ + "LCD_R1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH4": { + "Name": "PH4", + "AlternateFunctions": { + "AF4": [ + "I2C2_SCL" + ], + "AF9": [ + "LCD_G5" + ], + "AF10": [ + "OTG_HS_ULPI_NXT" + ], + "AF14": [ + "LCD_G4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH5": { + "Name": "PH5", + "AlternateFunctions": { + "AF4": [ + "I2C2_SDA" + ], + "AF5": [ + "SPI5_NSS" + ], + "AF12": [ + "FMC_SDNWE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH6": { + "Name": "PH6", + "AlternateFunctions": { + "AF4": [ + "I2C2_SMBA" + ], + "AF5": [ + "SPI5_SCK" + ], + "AF9": [ + "TIM12_CH1" + ], + "AF11": [ + "ETH_MII_RXD2" + ], + "AF12": [ + "FMC_SDNE1" + ], + "AF13": [ + "DCMI_D8" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH7": { + "Name": "PH7", + "AlternateFunctions": { + "AF4": [ + "I2C3_SCL" + ], + "AF5": [ + "SPI5_MISO" + ], + "AF11": [ + "ETH_MII_RXD3" + ], + "AF12": [ + "FMC_SDCKE1" + ], + "AF13": [ + "DCMI_D9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH8": { + "Name": "PH8", + "AlternateFunctions": { + "AF4": [ + "I2C3_SDA" + ], + "AF12": [ + "FMC_D16" + ], + "AF13": [ + "DCMI_HSYNC" + ], + "AF14": [ + "LCD_R2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH9": { + "Name": "PH9", + "AlternateFunctions": { + "AF4": [ + "I2C3_SMBA" + ], + "AF9": [ + "TIM12_CH2" + ], + "AF12": [ + "FMC_D17" + ], + "AF13": [ + "DCMI_D0" + ], + "AF14": [ + "LCD_R3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH10": { + "Name": "PH10", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH1" + ], + "AF4": [ + "I2C4_SMBA" + ], + "AF12": [ + "FMC_D18" + ], + "AF13": [ + "DCMI_D1" + ], + "AF14": [ + "LCD_R4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH11": { + "Name": "PH11", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH2" + ], + "AF4": [ + "I2C4_SCL" + ], + "AF12": [ + "FMC_D19" + ], + "AF13": [ + "DCMI_D2" + ], + "AF14": [ + "LCD_R5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH12": { + "Name": "PH12", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH3" + ], + "AF4": [ + "I2C4_SDA" + ], + "AF12": [ + "FMC_D20" + ], + "AF13": [ + "DCMI_D3" + ], + "AF14": [ + "LCD_R6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH13": { + "Name": "PH13", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH1N" + ], + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "CAN1_TX" + ], + "AF12": [ + "FMC_D21" + ], + "AF14": [ + "LCD_G2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH14": { + "Name": "PH14", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH2N" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "CAN1_RX" + ], + "AF12": [ + "FMC_D22" + ], + "AF13": [ + "DCMI_D4" + ], + "AF14": [ + "LCD_G3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH15": { + "Name": "PH15", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH3N" + ], + "AF12": [ + "FMC_D23" + ], + "AF13": [ + "DCMI_D11" + ], + "AF14": [ + "LCD_G4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI0": { + "Name": "PI0", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH4" + ], + "AF5": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF12": [ + "FMC_D24" + ], + "AF13": [ + "DCMI_D13" + ], + "AF14": [ + "LCD_G5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI1": { + "Name": "PI1", + "AlternateFunctions": { + "AF3": [ + "TIM8_BKIN2" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF12": [ + "FMC_D25" + ], + "AF13": [ + "DCMI_D8" + ], + "AF14": [ + "LCD_G6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI2": { + "Name": "PI2", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH4" + ], + "AF5": [ + "SPI2_MISO" + ], + "AF12": [ + "FMC_D26" + ], + "AF13": [ + "DCMI_D9" + ], + "AF14": [ + "LCD_G7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI3": { + "Name": "PI3", + "AlternateFunctions": { + "AF3": [ + "TIM8_ETR" + ], + "AF5": [ + "SPI2_MOSI", + "I2S2_SD" + ], + "AF12": [ + "FMC_D27" + ], + "AF13": [ + "DCMI_D10" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI4": { + "Name": "PI4", + "AlternateFunctions": { + "AF3": [ + "TIM8_BKIN" + ], + "AF10": [ + "SAI2_MCK_A" + ], + "AF12": [ + "FMC_NBL2" + ], + "AF13": [ + "DCMI_D5" + ], + "AF14": [ + "LCD_B4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI5": { + "Name": "PI5", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH1" + ], + "AF10": [ + "SAI2_SCK_A" + ], + "AF12": [ + "FMC_NBL3" + ], + "AF13": [ + "DCMI_VSYNC" + ], + "AF14": [ + "LCD_B5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI6": { + "Name": "PI6", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH2" + ], + "AF10": [ + "SAI2_SD_A" + ], + "AF12": [ + "FMC_D28" + ], + "AF13": [ + "DCMI_D6" + ], + "AF14": [ + "LCD_B6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI7": { + "Name": "PI7", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH3" + ], + "AF10": [ + "SAI2_FS_A" + ], + "AF12": [ + "FMC_D29" + ], + "AF13": [ + "DCMI_D7" + ], + "AF14": [ + "LCD_B7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI8": { + "Name": "PI8", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI9": { + "Name": "PI9", + "AlternateFunctions": { + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "CAN1_RX" + ], + "AF12": [ + "FMC_D30" + ], + "AF14": [ + "LCD_VSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI10": { + "Name": "PI10", + "AlternateFunctions": { + "AF11": [ + "ETH_MII_RX_ER" + ], + "AF12": [ + "FMC_D31" + ], + "AF14": [ + "LCD_HSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI11": { + "Name": "PI11", + "AlternateFunctions": { + "AF9": [ + "LCD_G6" + ], + "AF10": [ + "OTG_HS_ULPI_DIR" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI12": { + "Name": "PI12", + "AlternateFunctions": { + "AF14": [ + "LCD_HSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI13": { + "Name": "PI13", + "AlternateFunctions": { + "AF14": [ + "LCD_VSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI14": { + "Name": "PI14", + "AlternateFunctions": { + "AF14": [ + "LCD_CLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI15": { + "Name": "PI15", + "AlternateFunctions": { + "AF9": [ + "LCD_G2" + ], + "AF14": [ + "LCD_R0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ0": { + "Name": "PJ0", + "AlternateFunctions": { + "AF9": [ + "LCD_R7" + ], + "AF14": [ + "LCD_R1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ1": { + "Name": "PJ1", + "AlternateFunctions": { + "AF14": [ + "LCD_R2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ2": { + "Name": "PJ2", + "AlternateFunctions": { + "AF13": [ + "DSI_TE" + ], + "AF14": [ + "LCD_R3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ3": { + "Name": "PJ3", + "AlternateFunctions": { + "AF14": [ + "LCD_R4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ4": { + "Name": "PJ4", + "AlternateFunctions": { + "AF14": [ + "LCD_R5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ5": { + "Name": "PJ5", + "AlternateFunctions": { + "AF14": [ + "LCD_R6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ6": { + "Name": "PJ6", + "AlternateFunctions": { + "AF14": [ + "LCD_R7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ7": { + "Name": "PJ7", + "AlternateFunctions": { + "AF14": [ + "LCD_G0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ8": { + "Name": "PJ8", + "AlternateFunctions": { + "AF14": [ + "LCD_G1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ9": { + "Name": "PJ9", + "AlternateFunctions": { + "AF14": [ + "LCD_G2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ10": { + "Name": "PJ10", + "AlternateFunctions": { + "AF14": [ + "LCD_G3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ11": { + "Name": "PJ11", + "AlternateFunctions": { + "AF14": [ + "LCD_G4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ12": { + "Name": "PJ12", + "AlternateFunctions": { + "AF9": [ + "LCD_G3" + ], + "AF14": [ + "LCD_B0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ13": { + "Name": "PJ13", + "AlternateFunctions": { + "AF9": [ + "LCD_G4" + ], + "AF14": [ + "LCD_B1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ14": { + "Name": "PJ14", + "AlternateFunctions": { + "AF14": [ + "LCD_B2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ15": { + "Name": "PJ15", + "AlternateFunctions": { + "AF14": [ + "LCD_B3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK0": { + "Name": "PK0", + "AlternateFunctions": { + "AF14": [ + "LCD_G5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK1": { + "Name": "PK1", + "AlternateFunctions": { + "AF14": [ + "LCD_G6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK2": { + "Name": "PK2", + "AlternateFunctions": { + "AF14": [ + "LCD_G7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK3": { + "Name": "PK3", + "AlternateFunctions": { + "AF14": [ + "LCD_B4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK4": { + "Name": "PK4", + "AlternateFunctions": { + "AF14": [ + "LCD_B5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK5": { + "Name": "PK5", + "AlternateFunctions": { + "AF14": [ + "LCD_B6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK6": { + "Name": "PK6", + "AlternateFunctions": { + "AF14": [ + "LCD_B7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK7": { + "Name": "PK7", + "AlternateFunctions": { + "AF14": [ + "LCD_DE" + ], + "AF15": [ + "EVENTOUT" + ] + } + } + } + }, + { + "Name": "STM32F765", + "Pins": { + "PA0": { + "Name": "PA0", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH1", + "TIM2_ETR" + ], + "AF2": [ + "TIM5_CH1" + ], + "AF3": [ + "TIM8_ETR" + ], + "AF7": [ + "USART2_CTS" + ], + "AF8": [ + "UART4_TX" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF11": [ + "ETH_MII_CRS" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA1": { + "Name": "PA1", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH2" + ], + "AF2": [ + "TIM5_CH2" + ], + "AF7": [ + "USART2_RTS" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "QUADSPI_BK1_IO3" + ], + "AF10": [ + "SAI2_MCK_B" + ], + "AF11": [ + "ETH_MII_RX_CLK", + "ETH_RMII_REF_CLK" + ], + "AF14": [ + "LCD_R2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA2": { + "Name": "PA2", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH3" + ], + "AF2": [ + "TIM5_CH3" + ], + "AF3": [ + "TIM9_CH1" + ], + "AF7": [ + "USART2_TX" + ], + "AF8": [ + "SAI2_SCK_B" + ], + "AF11": [ + "ETH_MDIO" + ], + "AF12": [ + "MDIOS_MDIO" + ], + "AF14": [ + "LCD_R1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA3": { + "Name": "PA3", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH4" + ], + "AF2": [ + "TIM5_CH4" + ], + "AF3": [ + "TIM9_CH2" + ], + "AF7": [ + "USART2_RX" + ], + "AF9": [ + "LCD_B2" + ], + "AF10": [ + "OTG_HS_ULPI_D0" + ], + "AF11": [ + "ETH_MII_COL" + ], + "AF14": [ + "LCD_B5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA4": { + "Name": "PA4", + "AlternateFunctions": { + "AF5": [ + "SPI1_NSS", + "I2S1_WS" + ], + "AF6": [ + "SPI3_NSS", + "I2S3_WS" + ], + "AF7": [ + "USART2_CK" + ], + "AF8": [ + "SPI6_NSS" + ], + "AF12": [ + "OTG_HS_SOF" + ], + "AF13": [ + "DCMI_HSYNC" + ], + "AF14": [ + "LCD_VSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA5": { + "Name": "PA5", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH1", + "TIM2_ETR" + ], + "AF3": [ + "TIM8_CH1N" + ], + "AF5": [ + "SPI1_SCK", + "I2S1_CK" + ], + "AF8": [ + "SPI6_SCK" + ], + "AF10": [ + "OTG_HS_ULPI_CK" + ], + "AF14": [ + "LCD_R4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA6": { + "Name": "PA6", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF2": [ + "TIM3_CH1" + ], + "AF3": [ + "TIM8_BKIN" + ], + "AF5": [ + "SPI1_MISO" + ], + "AF8": [ + "SPI6_MISO" + ], + "AF9": [ + "TIM13_CH1" + ], + "AF12": [ + "MDIOS_MDC" + ], + "AF13": [ + "DCMI_PIXCLK" + ], + "AF14": [ + "LCD_G2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA7": { + "Name": "PA7", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF2": [ + "TIM3_CH2" + ], + "AF3": [ + "TIM8_CH1N" + ], + "AF5": [ + "SPI1_MOSI", + "I2S1_SD" + ], + "AF8": [ + "SPI6_MOSI" + ], + "AF9": [ + "TIM14_CH1" + ], + "AF11": [ + "ETH_MII_RX_DV", + "ETH_RMII_CRS_DV" + ], + "AF12": [ + "FMC_SDNWE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA8": { + "Name": "PA8", + "AlternateFunctions": { + "AF0": [ + "MCO1" + ], + "AF1": [ + "TIM1_CH1" + ], + "AF3": [ + "TIM8_BKIN2" + ], + "AF4": [ + "I2C3_SCL" + ], + "AF7": [ + "USART1_CK" + ], + "AF10": [ + "OTG_FS_SOF" + ], + "AF11": [ + "CAN3_RX" + ], + "AF12": [ + "UART7_RX" + ], + "AF13": [ + "LCD_B3" + ], + "AF14": [ + "LCD_R6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA9": { + "Name": "PA9", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2" + ], + "AF4": [ + "I2C3_SMBA" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF7": [ + "USART1_TX" + ], + "AF13": [ + "DCMI_D0" + ], + "AF14": [ + "LCD_R5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA10": { + "Name": "PA10", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3" + ], + "AF7": [ + "USART1_RX" + ], + "AF9": [ + "LCD_B4" + ], + "AF10": [ + "OTG_FS_ID" + ], + "AF12": [ + "MDIOS_MDIO" + ], + "AF13": [ + "DCMI_D1" + ], + "AF14": [ + "LCD_B1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA11": { + "Name": "PA11", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH4" + ], + "AF5": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF6": [ + "UART4_RX" + ], + "AF7": [ + "USART1_CTS" + ], + "AF9": [ + "CAN1_RX" + ], + "AF10": [ + "OTG_FS_DM" + ], + "AF14": [ + "LCD_R4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA12": { + "Name": "PA12", + "AlternateFunctions": { + "AF1": [ + "TIM1_ETR" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF6": [ + "UART4_TX" + ], + "AF7": [ + "USART1_RTS" + ], + "AF8": [ + "SAI2_FS_B" + ], + "AF9": [ + "CAN1_TX" + ], + "AF10": [ + "OTG_FS_DP" + ], + "AF14": [ + "LCD_R5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA13": { + "Name": "PA13", + "AlternateFunctions": { + "AF0": [ + "JTMS-SWDIO" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA14": { + "Name": "PA14", + "AlternateFunctions": { + "AF0": [ + "JTCK-SWCLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA15": { + "Name": "PA15", + "AlternateFunctions": { + "AF0": [ + "JTDI" + ], + "AF1": [ + "TIM2_CH1", + "TIM2_ETR" + ], + "AF4": [ + "HDMI-CEC" + ], + "AF5": [ + "SPI1_NSS", + "I2S1_WS" + ], + "AF6": [ + "SPI3_NSS", + "I2S3_WS" + ], + "AF7": [ + "SPI6_NSS" + ], + "AF8": [ + "UART4_RTS" + ], + "AF11": [ + "CAN3_TX" + ], + "AF12": [ + "UART7_TX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB0": { + "Name": "PB0", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF2": [ + "TIM3_CH3" + ], + "AF3": [ + "TIM8_CH2N" + ], + "AF6": [ + "DFSDM1_CKOUT" + ], + "AF8": [ + "UART4_CTS" + ], + "AF9": [ + "LCD_R3" + ], + "AF10": [ + "OTG_HS_ULPI_D1" + ], + "AF11": [ + "ETH_MII_RXD2" + ], + "AF14": [ + "LCD_G1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB1": { + "Name": "PB1", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3N" + ], + "AF2": [ + "TIM3_CH4" + ], + "AF3": [ + "TIM8_CH3N" + ], + "AF6": [ + "DFSDM1_DATIN1" + ], + "AF9": [ + "LCD_R6" + ], + "AF10": [ + "OTG_HS_ULPI_D2" + ], + "AF11": [ + "ETH_MII_RXD3" + ], + "AF14": [ + "LCD_G0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB2": { + "Name": "PB2", + "AlternateFunctions": { + "AF6": [ + "SAI1_SD_A" + ], + "AF7": [ + "SPI3_MOSI", + "I2S3_SD" + ], + "AF9": [ + "QUADSPI_CLK" + ], + "AF10": [ + "DFSDM1_CKIN1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB3": { + "Name": "PB3", + "AlternateFunctions": { + "AF0": [ + "JTDO", + "TRACESWO" + ], + "AF1": [ + "TIM2_CH2" + ], + "AF5": [ + "SPI1_SCK", + "I2S1_CK" + ], + "AF6": [ + "SPI3_SCK", + "I2S3_CK" + ], + "AF8": [ + "SPI6_SCK" + ], + "AF10": [ + "SDMMC2_D2" + ], + "AF11": [ + "CAN3_RX" + ], + "AF12": [ + "UART7_RX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB4": { + "Name": "PB4", + "AlternateFunctions": { + "AF0": [ + "NJTRST" + ], + "AF2": [ + "TIM3_CH1" + ], + "AF5": [ + "SPI1_MISO" + ], + "AF6": [ + "SPI3_MISO" + ], + "AF7": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF8": [ + "SPI6_MISO" + ], + "AF10": [ + "SDMMC2_D3" + ], + "AF11": [ + "CAN3_TX" + ], + "AF12": [ + "UART7_TX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB5": { + "Name": "PB5", + "AlternateFunctions": { + "AF1": [ + "UART5_RX" + ], + "AF2": [ + "TIM3_CH2" + ], + "AF4": [ + "I2C1_SMBA" + ], + "AF5": [ + "SPI1_MOSI", + "I2S1_SD" + ], + "AF6": [ + "SPI3_MOSI", + "I2S3_SD" + ], + "AF8": [ + "SPI6_MOSI" + ], + "AF9": [ + "CAN2_RX" + ], + "AF10": [ + "OTG_HS_ULPI_D7" + ], + "AF11": [ + "ETH_PPS_OUT" + ], + "AF12": [ + "FMC_SDCKE1" + ], + "AF13": [ + "DCMI_D10" + ], + "AF14": [ + "LCD_G7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB6": { + "Name": "PB6", + "AlternateFunctions": { + "AF1": [ + "UART5_TX" + ], + "AF2": [ + "TIM4_CH1" + ], + "AF3": [ + "HDMI-CEC" + ], + "AF4": [ + "I2C1_SCL" + ], + "AF6": [ + "DFSDM1_DATIN5" + ], + "AF7": [ + "USART1_TX" + ], + "AF9": [ + "CAN2_TX" + ], + "AF10": [ + "QUADSPI_BK1_NCS" + ], + "AF11": [ + "I2C4_SCL" + ], + "AF12": [ + "FMC_SDNE1" + ], + "AF13": [ + "DCMI_D5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB7": { + "Name": "PB7", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH2" + ], + "AF4": [ + "I2C1_SDA" + ], + "AF6": [ + "DFSDM1_CKIN5" + ], + "AF7": [ + "USART1_RX" + ], + "AF11": [ + "I2S4_SDA" + ], + "AF12": [ + "FMC_NL" + ], + "AF13": [ + "DCMI_VSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB8": { + "Name": "PB8", + "AlternateFunctions": { + "AF1": [ + "I2C4_SCL" + ], + "AF2": [ + "TIM4_CH3" + ], + "AF3": [ + "TIM10_CH1" + ], + "AF4": [ + "I2C1_SCL" + ], + "AF6": [ + "DFSDM1_CKIN7" + ], + "AF7": [ + "UART5_RX" + ], + "AF9": [ + "CAN1_RX" + ], + "AF10": [ + "SDMMC2_D4" + ], + "AF11": [ + "ETH_MII_TXD3" + ], + "AF12": [ + "SDMMC_D4" + ], + "AF13": [ + "DCMI_D6" + ], + "AF14": [ + "LCD_B6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB9": { + "Name": "PB9", + "AlternateFunctions": { + "AF1": [ + "I2C4_SDA" + ], + "AF2": [ + "TIM4_CH4" + ], + "AF3": [ + "TIM11_CH1" + ], + "AF4": [ + "I2C1_SDA" + ], + "AF5": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF6": [ + "DFSDM1_DATIN7" + ], + "AF7": [ + "UART5_TX" + ], + "AF9": [ + "CAN1_TX" + ], + "AF10": [ + "SDMMC2_D5" + ], + "AF11": [ + "I2C4_SMBA" + ], + "AF12": [ + "SDMMC_D5" + ], + "AF13": [ + "DCMI_D7" + ], + "AF14": [ + "LCD_B7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB10": { + "Name": "PB10", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH3" + ], + "AF4": [ + "I2C2_SCL" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF6": [ + "DFSDM1_DATIN7" + ], + "AF7": [ + "USART3_TX" + ], + "AF9": [ + "-QUADSPI_BK1_NCS" + ], + "AF10": [ + "OTG_HS_ULPI_D3" + ], + "AF11": [ + "ETH_MII_RX_ER" + ], + "AF14": [ + "LCD_G4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB11": { + "Name": "PB11", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH4" + ], + "AF4": [ + "I2C2_SDA" + ], + "AF6": [ + "DFSDM1_CKIN7" + ], + "AF7": [ + "USART3_RX" + ], + "AF10": [ + "OTG_HS_ULPI_D4" + ], + "AF11": [ + "ETH_MII_TX_EN", + "ETH_RMII_TX_EN" + ], + "AF13": [ + "DSI_TE" + ], + "AF14": [ + "LCD_G5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB12": { + "Name": "PB12", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF4": [ + "I2C2_SMBA" + ], + "AF5": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF6": [ + "DFSDM1_DATIN1" + ], + "AF7": [ + "USART3_CK" + ], + "AF8": [ + "UART5_RX" + ], + "AF9": [ + "CAN2_RX" + ], + "AF10": [ + "OTG_HS_ULPI_D5" + ], + "AF11": [ + "ETH_MII_TXD0", + "ETH_RMII_TXD0" + ], + "AF12": [ + "OTG_HS_ID" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB13": { + "Name": "PB13", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF6": [ + "DFSDM1_CKIN1" + ], + "AF7": [ + "USART3_CTS" + ], + "AF8": [ + "UART5_TX" + ], + "AF9": [ + "CAN2_TX" + ], + "AF10": [ + "OTG_HS_ULPI_D6" + ], + "AF11": [ + "ETH_MII_TXD1", + "ETH_RMII_TXD1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB14": { + "Name": "PB14", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF3": [ + "TIM8_CH2N" + ], + "AF4": [ + "USART1_TX" + ], + "AF5": [ + "SPI2_MISO" + ], + "AF6": [ + "DFSDM1_DATIN2" + ], + "AF7": [ + "USART3_RTS" + ], + "AF8": [ + "UART4_RTS" + ], + "AF9": [ + "TIM12_CH1" + ], + "AF10": [ + "SDMMC2_D0" + ], + "AF12": [ + "OTG_HS_DM" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB15": { + "Name": "PB15", + "AlternateFunctions": { + "AF0": [ + "RTC_REFIN" + ], + "AF1": [ + "TIM1_CH3N" + ], + "AF3": [ + "TIM8_CH3N" + ], + "AF4": [ + "USART1_RX" + ], + "AF5": [ + "SPI2_MOSI", + "I2S2_SD" + ], + "AF6": [ + "DFSDM1_CKIN2" + ], + "AF8": [ + "UART4_CTS" + ], + "AF9": [ + "TIM12_CH2" + ], + "AF10": [ + "SDMMC2_D1" + ], + "AF12": [ + "OTG_HS_DP" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC0": { + "Name": "PC0", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN0" + ], + "AF6": [ + "DFSDM1_DATIN4" + ], + "AF8": [ + "SAI2_FS_B" + ], + "AF10": [ + "OTG_HS_ULPI_STP" + ], + "AF12": [ + "FMC_SDNWE" + ], + "AF14": [ + "LCD_R5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC1": { + "Name": "PC1", + "AlternateFunctions": { + "AF0": [ + "TRACED0" + ], + "AF3": [ + "DFSDM1_DATAIN0" + ], + "AF5": [ + "SPI2_MOSI", + "I2S2_SD" + ], + "AF6": [ + "SAI1_SD_A" + ], + "AF10": [ + "DFSDM1_CKIN4" + ], + "AF11": [ + "ETH_MDC" + ], + "AF12": [ + "MDIOS_MDC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC2": { + "Name": "PC2", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN1" + ], + "AF5": [ + "SPI2_MISO" + ], + "AF6": [ + "DFSDM1_CKOUT" + ], + "AF10": [ + "OTG_HS_ULPI_DIR" + ], + "AF11": [ + "ETH_MII_TXD2" + ], + "AF12": [ + "FMC_SDNE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC3": { + "Name": "PC3", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_DATAIN1" + ], + "AF5": [ + "SPI2_MOSI", + "I2S2_SD" + ], + "AF10": [ + "OTG_HS_ULPI_NXT" + ], + "AF11": [ + "ETH_MII_TX_CLK" + ], + "AF12": [ + "FMC_SDCKE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC4": { + "Name": "PC4", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN2" + ], + "AF5": [ + "I2S1_MCK" + ], + "AF8": [ + "SPDIF_RX2" + ], + "AF11": [ + "ETH_MII_RXD0", + "ETH_RMII_RXD0" + ], + "AF12": [ + "FMC_SDNE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC5": { + "Name": "PC5", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_DATAIN2" + ], + "AF8": [ + "SPDIF_RX3" + ], + "AF11": [ + "ETH_MII_RXD1", + "ETH_RMII_RXD1" + ], + "AF12": [ + "FMC_SDCKE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC6": { + "Name": "PC6", + "AlternateFunctions": { + "AF2": [ + "TIM3_CH1" + ], + "AF3": [ + "TIM8_CH1" + ], + "AF5": [ + "I2S2_MCK" + ], + "AF7": [ + "DFSDM1_CKIN3" + ], + "AF8": [ + "USART6_TX" + ], + "AF9": [ + "FMC_NWAIT" + ], + "AF10": [ + "SDMMC2_D6" + ], + "AF12": [ + "SDMMC_D6" + ], + "AF13": [ + "DCMI_D0" + ], + "AF14": [ + "LCD_HSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC7": { + "Name": "PC7", + "AlternateFunctions": { + "AF2": [ + "TIM3_CH2" + ], + "AF3": [ + "TIM8_CH2" + ], + "AF6": [ + "I2S3_MCK" + ], + "AF7": [ + "DFSDM1_DATAIN3" + ], + "AF8": [ + "USART6_RX" + ], + "AF9": [ + "FMC_NE1" + ], + "AF10": [ + "SDMMC2_D7" + ], + "AF12": [ + "SDMMC_D7" + ], + "AF13": [ + "DCMI_D1" + ], + "AF14": [ + "LCD_G6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC8": { + "Name": "PC8", + "AlternateFunctions": { + "AF0": [ + "TRACED1" + ], + "AF2": [ + "TIM3_CH3" + ], + "AF3": [ + "TIM8_CH3" + ], + "AF7": [ + "UART5_RTS" + ], + "AF8": [ + "USART6_CK" + ], + "AF9": [ + "FMC_NE2", + "FMC_NCE" + ], + "AF12": [ + "SDMMC_D0" + ], + "AF13": [ + "DCMI_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC9": { + "Name": "PC9", + "AlternateFunctions": { + "AF0": [ + "MCO2" + ], + "AF2": [ + "TIM3_CH4" + ], + "AF3": [ + "TIM8_CH4" + ], + "AF4": [ + "I2C3_SDA" + ], + "AF5": [ + "I2S_CKIN" + ], + "AF7": [ + "UART5_CTS" + ], + "AF9": [ + "QUADSPI_BK1_IO0" + ], + "AF10": [ + "LCD_G3" + ], + "AF12": [ + "SDMMC_D1" + ], + "AF13": [ + "DCMI_D3" + ], + "AF14": [ + "LCD_B2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC10": { + "Name": "PC10", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN5" + ], + "AF6": [ + "SPI3_SCK", + "I2S3_CK" + ], + "AF7": [ + "USART3_TX" + ], + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "QUADSPI_BK1_IO1" + ], + "AF12": [ + "SDMMC_D2" + ], + "AF13": [ + "DCMI_D8" + ], + "AF14": [ + "LCD_R2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC11": { + "Name": "PC11", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_DATAIN5" + ], + "AF6": [ + "SPI3_MISO" + ], + "AF7": [ + "USART3_RX" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "QUADSPI_BK2_NCS" + ], + "AF12": [ + "SDMMC_D3" + ], + "AF13": [ + "DCMI_D4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC12": { + "Name": "PC12", + "AlternateFunctions": { + "AF0": [ + "TRACED3" + ], + "AF6": [ + "SPI3_MOSI", + "I2S3_SD" + ], + "AF7": [ + "USART3_CK" + ], + "AF8": [ + "UART5_TX" + ], + "AF12": [ + "SDMMC_CK" + ], + "AF13": [ + "DCMI_D9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC13": { + "Name": "PC13", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC14": { + "Name": "PC14", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC15": { + "Name": "PC15", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD0": { + "Name": "PD0", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN6" + ], + "AF6": [ + "DFSDM1_DATAIN7" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "CAN1_RX" + ], + "AF12": [ + "FMC_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD1": { + "Name": "PD1", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_DATAIN6" + ], + "AF6": [ + "DFSDM1_CKIN7" + ], + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "CAN1_TX" + ], + "AF12": [ + "FMC_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD2": { + "Name": "PD2", + "AlternateFunctions": { + "AF0": [ + "TRACED2" + ], + "AF2": [ + "TIM3_ETR" + ], + "AF8": [ + "UART5_RX" + ], + "AF12": [ + "SDMMC_CMD" + ], + "AF13": [ + "DCMI_D11" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD3": { + "Name": "PD3", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKOUT" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF6": [ + "DFSDM1_DATAIN0" + ], + "AF7": [ + "USART2_CTS" + ], + "AF12": [ + "FMC_CLK" + ], + "AF13": [ + "DCMI_D5" + ], + "AF14": [ + "LCD_G7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD4": { + "Name": "PD4", + "AlternateFunctions": { + "AF6": [ + "DFSDM1_CKIN0" + ], + "AF7": [ + "USART2_RTS" + ], + "AF12": [ + "FMC_NOE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD5": { + "Name": "PD5", + "AlternateFunctions": { + "AF7": [ + "USART2_TX" + ], + "AF12": [ + "FMC_NWE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD6": { + "Name": "PD6", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN4" + ], + "AF5": [ + "SPI3_MOSI", + "I2S3_SD" + ], + "AF6": [ + "SAI1_SD_A" + ], + "AF7": [ + "USART2_RX" + ], + "AF10": [ + "DFSDM1_DATAIN1" + ], + "AF11": [ + "SDMMC2_CK" + ], + "AF12": [ + "FMC_NWAIT" + ], + "AF13": [ + "DCMI_D10" + ], + "AF14": [ + "LCD_B2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD7": { + "Name": "PD7", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_DATAIN4" + ], + "AF5": [ + "SPI1_MOSI", + "I2S1_SD" + ], + "AF6": [ + "DFSDM1_CKIN1" + ], + "AF7": [ + "USART2_CK" + ], + "AF8": [ + "SPDIF_RX0" + ], + "AF11": [ + "SDMMC2_CMD" + ], + "AF12": [ + "FMC_NE1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD8": { + "Name": "PD8", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN3" + ], + "AF7": [ + "USART3_TX" + ], + "AF8": [ + "SPDIF_RX1" + ], + "AF12": [ + "FMC_D13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD9": { + "Name": "PD9", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_DATAIN3" + ], + "AF7": [ + "USART3_RX" + ], + "AF12": [ + "FMC_D14" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD10": { + "Name": "PD10", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKOUT" + ], + "AF7": [ + "USART3_CK" + ], + "AF12": [ + "FMC_D15" + ], + "AF14": [ + "LCD_B3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD11": { + "Name": "PD11", + "AlternateFunctions": { + "AF4": [ + "I2C4_SMBA" + ], + "AF7": [ + "USART3_CTS" + ], + "AF9": [ + "QUADSPI_BK1_IO0" + ], + "AF10": [ + "SAI2_SD_A" + ], + "AF12": [ + "FMC_A16", + "FMC_CLE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD12": { + "Name": "PD12", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH1" + ], + "AF3": [ + "LPTIM1_IN1" + ], + "AF4": [ + "I2C4_SCL" + ], + "AF7": [ + "USART3_RTS" + ], + "AF9": [ + "QUADSPI_BK1_IO1" + ], + "AF10": [ + "SAI2_FS_A" + ], + "AF12": [ + "FMC_A17", + "FMC_ALE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD13": { + "Name": "PD13", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH2" + ], + "AF3": [ + "LPTIM1_OUT" + ], + "AF4": [ + "I2C4_SDA" + ], + "AF9": [ + "QUADSPI_BK1_IO3" + ], + "AF10": [ + "SAI2_SCK_A" + ], + "AF12": [ + "FMC_A18" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD14": { + "Name": "PD14", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH3" + ], + "AF8": [ + "UART8_CTS" + ], + "AF12": [ + "FMC_D0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD15": { + "Name": "PD15", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH4" + ], + "AF8": [ + "UART8_RTS" + ], + "AF12": [ + "FMC_D1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE0": { + "Name": "PE0", + "AlternateFunctions": { + "AF2": [ + "TIM4_ETR" + ], + "AF3": [ + "LPTIM1_ETR" + ], + "AF8": [ + "UART8_Rx" + ], + "AF10": [ + "SAI2_MCK_A" + ], + "AF12": [ + "FMC_NBL0" + ], + "AF13": [ + "DCMI_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE1": { + "Name": "PE1", + "AlternateFunctions": { + "AF3": [ + "LPTIM1_IN2" + ], + "AF8": [ + "UART8_Tx" + ], + "AF12": [ + "FMC_NBL1" + ], + "AF13": [ + "DCMI_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE2": { + "Name": "PE2", + "AlternateFunctions": { + "AF0": [ + "TRACECLK" + ], + "AF5": [ + "SPI4_SCK" + ], + "AF6": [ + "SAI1_MCLK_A" + ], + "AF9": [ + "QUADSPI_BK1_IO2" + ], + "AF11": [ + "ETH_MII_TXD3" + ], + "AF12": [ + "FMC_A23" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE3": { + "Name": "PE3", + "AlternateFunctions": { + "AF0": [ + "TRACED0" + ], + "AF6": [ + "SAI1_SD_B" + ], + "AF12": [ + "FMC_A19" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE4": { + "Name": "PE4", + "AlternateFunctions": { + "AF0": [ + "TRACED1" + ], + "AF5": [ + "SPI4_NSS" + ], + "AF6": [ + "SAI1_FS_A" + ], + "AF10": [ + "DFSDM1_DATAIN3" + ], + "AF12": [ + "FMC_A20" + ], + "AF13": [ + "DCMI_D4" + ], + "AF14": [ + "LCD_B0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE5": { + "Name": "PE5", + "AlternateFunctions": { + "AF0": [ + "TRACED2" + ], + "AF3": [ + "TIM9_CH1" + ], + "AF5": [ + "SPI4_MISO" + ], + "AF6": [ + "SAI1_SCK_A" + ], + "AF10": [ + "DFSDM1_CKIN3" + ], + "AF12": [ + "FMC_A21" + ], + "AF13": [ + "DCMI_D6" + ], + "AF14": [ + "LCD_G0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE6": { + "Name": "PE6", + "AlternateFunctions": { + "AF0": [ + "TRACED3" + ], + "AF1": [ + "TIM1_BKIN2" + ], + "AF3": [ + "TIM9_CH2" + ], + "AF5": [ + "SPI4_MOSI" + ], + "AF6": [ + "SAI1_SD_A" + ], + "AF10": [ + "SAI2_MCK_B" + ], + "AF12": [ + "FMC_A22" + ], + "AF13": [ + "DCMI_D7" + ], + "AF14": [ + "LCD_G1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE7": { + "Name": "PE7", + "AlternateFunctions": { + "AF1": [ + "TIM1_ETR" + ], + "AF6": [ + "DFSDM1_DATAIN2" + ], + "AF8": [ + "UART7_Rx" + ], + "AF10": [ + "QUADSPI_BK2_IO0" + ], + "AF12": [ + "FMC_D4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE8": { + "Name": "PE8", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF6": [ + "DFSDM1_CKIN2" + ], + "AF8": [ + "UART7_Tx" + ], + "AF10": [ + "QUADSPI_BK2_IO1" + ], + "AF12": [ + "FMC_D5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE9": { + "Name": "PE9", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1" + ], + "AF6": [ + "DFSDM1_CKOUT" + ], + "AF8": [ + "UART7_RTS" + ], + "AF10": [ + "QUADSPI_BK2_IO2" + ], + "AF12": [ + "FMC_D6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE10": { + "Name": "PE10", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF6": [ + "DFSDM1_DATAIN4" + ], + "AF8": [ + "UART7_CTS" + ], + "AF10": [ + "QUADSPI_BK2_IO3" + ], + "AF12": [ + "FMC_D7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE11": { + "Name": "PE11", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2" + ], + "AF5": [ + "SPI4_NSS" + ], + "AF6": [ + "DFSDM1_CKIN4" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF12": [ + "FMC_D8" + ], + "AF14": [ + "LCD_G3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE12": { + "Name": "PE12", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3N" + ], + "AF5": [ + "SPI4_SCK" + ], + "AF6": [ + "DFSDM1_DATAIN5" + ], + "AF10": [ + "SAI2_SCK_B" + ], + "AF12": [ + "FMC_D9" + ], + "AF14": [ + "LCD_B4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE13": { + "Name": "PE13", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3" + ], + "AF5": [ + "SPI4_MISO" + ], + "AF6": [ + "DFSDM1_CKIN5" + ], + "AF10": [ + "SAI2_FS_B" + ], + "AF12": [ + "FMC_D10" + ], + "AF14": [ + "LCD_DE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE14": { + "Name": "PE14", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH4" + ], + "AF5": [ + "SPI4_MOSI" + ], + "AF10": [ + "SAI2_MCK_B" + ], + "AF12": [ + "FMC_D11" + ], + "AF14": [ + "LCD_CLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE15": { + "Name": "PE15", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF12": [ + "FMC_D12" + ], + "AF14": [ + "LCD_R7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF0": { + "Name": "PF0", + "AlternateFunctions": { + "AF4": [ + "I2C2_SDA" + ], + "AF12": [ + "FMC_A0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF1": { + "Name": "PF1", + "AlternateFunctions": { + "AF4": [ + "I2C2_SCL" + ], + "AF12": [ + "FMC_A1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF2": { + "Name": "PF2", + "AlternateFunctions": { + "AF4": [ + "I2C2_SMBA" + ], + "AF12": [ + "FMC_A2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF3": { + "Name": "PF3", + "AlternateFunctions": { + "AF12": [ + "FMC_A3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF4": { + "Name": "PF4", + "AlternateFunctions": { + "AF12": [ + "FMC_A4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF5": { + "Name": "PF5", + "AlternateFunctions": { + "AF12": [ + "FMC_A5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF6": { + "Name": "PF6", + "AlternateFunctions": { + "AF3": [ + "TIM10_CH1" + ], + "AF5": [ + "SPI5_NSS" + ], + "AF6": [ + "SAI1_SD_B" + ], + "AF8": [ + "UART7_Rx" + ], + "AF9": [ + "QUADSPI_BK1_IO3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF7": { + "Name": "PF7", + "AlternateFunctions": { + "AF3": [ + "TIM11_CH1" + ], + "AF5": [ + "SPI5_SCK" + ], + "AF6": [ + "SAI1_MCLK_B" + ], + "AF8": [ + "UART7_Tx" + ], + "AF9": [ + "QUADSPI_BK1_IO2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF8": { + "Name": "PF8", + "AlternateFunctions": { + "AF5": [ + "SPI5_MISO" + ], + "AF6": [ + "SAI1_SCK_B" + ], + "AF8": [ + "UART7_RTS" + ], + "AF9": [ + "TIM13_CH1" + ], + "AF10": [ + "QUADSPI_BK1_IO0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF9": { + "Name": "PF9", + "AlternateFunctions": { + "AF5": [ + "SPI5_MOSI" + ], + "AF6": [ + "SAI1_FS_B" + ], + "AF8": [ + "UART7_CTS" + ], + "AF9": [ + "TIM14_CH1" + ], + "AF10": [ + "QUADSPI_BK1_IO1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF10": { + "Name": "PF10", + "AlternateFunctions": { + "AF9": [ + "QUADSPI_CLK" + ], + "AF13": [ + "DCMI_D11" + ], + "AF14": [ + "LCD_DE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF11": { + "Name": "PF11", + "AlternateFunctions": { + "AF5": [ + "SPI5_MOSI" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF12": [ + "FMC_SDNRAS" + ], + "AF13": [ + "DCMI_D12" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF12": { + "Name": "PF12", + "AlternateFunctions": { + "AF12": [ + "FMC_A6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF13": { + "Name": "PF13", + "AlternateFunctions": { + "AF4": [ + "I2C4_SMBA" + ], + "AF6": [ + "DFSDM1_DATAIN6" + ], + "AF12": [ + "FMC_A7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF14": { + "Name": "PF14", + "AlternateFunctions": { + "AF4": [ + "I2C4_SCL" + ], + "AF6": [ + "DFSDM1_CKIN6" + ], + "AF12": [ + "FMC_A8" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF15": { + "Name": "PF15", + "AlternateFunctions": { + "AF4": [ + "I2C4_SDA" + ], + "AF12": [ + "FMC_A9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG0": { + "Name": "PG0", + "AlternateFunctions": { + "AF12": [ + "FMC_A10" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG1": { + "Name": "PG1", + "AlternateFunctions": { + "AF12": [ + "FMC_A11" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG2": { + "Name": "PG2", + "AlternateFunctions": { + "AF12": [ + "FMC_A12" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG3": { + "Name": "PG3", + "AlternateFunctions": { + "AF12": [ + "FMC_A13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG4": { + "Name": "PG4", + "AlternateFunctions": { + "AF12": [ + "FMC_A14", + "FMC_BA0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG5": { + "Name": "PG5", + "AlternateFunctions": { + "AF12": [ + "FMC_A15", + "FMC_BA1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG6": { + "Name": "PG6", + "AlternateFunctions": { + "AF12": [ + "FMC_NE3" + ], + "AF13": [ + "DCMI_D12" + ], + "AF14": [ + "LCD_R7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG7": { + "Name": "PG7", + "AlternateFunctions": { + "AF6": [ + "SAI1_MCLK_A" + ], + "AF8": [ + "USART6_CK" + ], + "AF12": [ + "FMC_INT" + ], + "AF13": [ + "DCMI_D13" + ], + "AF14": [ + "LCD_CLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG8": { + "Name": "PG8", + "AlternateFunctions": { + "AF5": [ + "SPI6_NSS" + ], + "AF7": [ + "SPDIF_RX2" + ], + "AF8": [ + "USART6_RTS" + ], + "AF11": [ + "ETH_PPS_OUT" + ], + "AF12": [ + "FMC_SDCLK" + ], + "AF14": [ + "LCD_G7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG9": { + "Name": "PG9", + "AlternateFunctions": { + "AF5": [ + "SPI1_MISO" + ], + "AF7": [ + "SPDIF_RX3" + ], + "AF8": [ + "USART6_RX" + ], + "AF9": [ + "QUADSPI_BK2_IO2" + ], + "AF10": [ + "SAI2_FS_B" + ], + "AF11": [ + "SDMMC2_D0" + ], + "AF12": [ + "FMC_NE2", + "FMC_NCE" + ], + "AF13": [ + "DCMI_VSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG10": { + "Name": "PG10", + "AlternateFunctions": { + "AF5": [ + "SPI1_NSS", + "I2S1_WS" + ], + "AF9": [ + "LCD_G3" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF11": [ + "SDMMC2_D1" + ], + "AF12": [ + "FMC_NE3" + ], + "AF13": [ + "DCMI_D2" + ], + "AF14": [ + "LCD_B2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG11": { + "Name": "PG11", + "AlternateFunctions": { + "AF5": [ + "SPI1_SCK", + "I2S1_CK" + ], + "AF7": [ + "SPDIF_RX0" + ], + "AF10": [ + "SDMMC2_D2" + ], + "AF11": [ + "ETH_MII_TX_EN", + "ETH_RMII_TX_EN" + ], + "AF13": [ + "DCMI_D3" + ], + "AF14": [ + "LCD_B3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG12": { + "Name": "PG12", + "AlternateFunctions": { + "AF3": [ + "LPTIM1_IN1" + ], + "AF5": [ + "SPI6_MISO" + ], + "AF7": [ + "SPDIF_RX1" + ], + "AF8": [ + "USART6_RTS" + ], + "AF9": [ + "LCD_B4" + ], + "AF11": [ + "SDMMC2_D3" + ], + "AF12": [ + "FMC_NE4" + ], + "AF14": [ + "LCD_B1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG13": { + "Name": "PG13", + "AlternateFunctions": { + "AF0": [ + "TRACED0" + ], + "AF3": [ + "LPTIM1_OUT" + ], + "AF5": [ + "SPI6_SCK" + ], + "AF8": [ + "USART6_CTS" + ], + "AF11": [ + "ETH_MII_TXD0", + "ETH_RMII_TXD0" + ], + "AF12": [ + "FMC_A24" + ], + "AF14": [ + "LCD_R0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG14": { + "Name": "PG14", + "AlternateFunctions": { + "AF0": [ + "TRACED1" + ], + "AF3": [ + "LPTIM1_ETR" + ], + "AF5": [ + "SPI6_MOSI" + ], + "AF8": [ + "USART6_TX" + ], + "AF9": [ + "QUADSPI_BK2_IO3" + ], + "AF11": [ + "ETH_MII_TXD1", + "ETH_RMII_TXD1" + ], + "AF12": [ + "FMC_A25" + ], + "AF14": [ + "LCD_B0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG15": { + "Name": "PG15", + "AlternateFunctions": { + "AF8": [ + "USART6_CTS" + ], + "AF12": [ + "FMC_SDNCAS" + ], + "AF13": [ + "DCMI_D13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH0": { + "Name": "PH0", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH1": { + "Name": "PH1", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH2": { + "Name": "PH2", + "AlternateFunctions": { + "AF3": [ + "LPTIM1_IN2" + ], + "AF9": [ + "QUADSPI_BK2_IO0" + ], + "AF10": [ + "SAI2_SCK_B" + ], + "AF11": [ + "ETH_MII_CRS" + ], + "AF12": [ + "FMC_SDCKE0" + ], + "AF14": [ + "LCD_R0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH3": { + "Name": "PH3", + "AlternateFunctions": { + "AF9": [ + "QUADSPI_BK2_IO1" + ], + "AF10": [ + "SAI2_MCK_B" + ], + "AF11": [ + "ETH_MII_COL" + ], + "AF12": [ + "FMC_SDNE0" + ], + "AF14": [ + "LCD_R1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH4": { + "Name": "PH4", + "AlternateFunctions": { + "AF4": [ + "I2C2_SCL" + ], + "AF9": [ + "LCD_G5" + ], + "AF10": [ + "OTG_HS_ULPI_NXT" + ], + "AF14": [ + "LCD_G4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH5": { + "Name": "PH5", + "AlternateFunctions": { + "AF4": [ + "I2C2_SDA" + ], + "AF5": [ + "SPI5_NSS" + ], + "AF12": [ + "FMC_SDNWE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH6": { + "Name": "PH6", + "AlternateFunctions": { + "AF4": [ + "I2C2_SMBA" + ], + "AF5": [ + "SPI5_SCK" + ], + "AF9": [ + "TIM12_CH1" + ], + "AF11": [ + "ETH_MII_RXD2" + ], + "AF12": [ + "FMC_SDNE1" + ], + "AF13": [ + "DCMI_D8" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH7": { + "Name": "PH7", + "AlternateFunctions": { + "AF4": [ + "I2C3_SCL" + ], + "AF5": [ + "SPI5_MISO" + ], + "AF11": [ + "ETH_MII_RXD3" + ], + "AF12": [ + "FMC_SDCKE1" + ], + "AF13": [ + "DCMI_D9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH8": { + "Name": "PH8", + "AlternateFunctions": { + "AF4": [ + "I2C3_SDA" + ], + "AF12": [ + "FMC_D16" + ], + "AF13": [ + "DCMI_HSYNC" + ], + "AF14": [ + "LCD_R2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH9": { + "Name": "PH9", + "AlternateFunctions": { + "AF4": [ + "I2C3_SMBA" + ], + "AF9": [ + "TIM12_CH2" + ], + "AF12": [ + "FMC_D17" + ], + "AF13": [ + "DCMI_D0" + ], + "AF14": [ + "LCD_R3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH10": { + "Name": "PH10", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH1" + ], + "AF4": [ + "I2C4_SMBA" + ], + "AF12": [ + "FMC_D18" + ], + "AF13": [ + "DCMI_D1" + ], + "AF14": [ + "LCD_R4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH11": { + "Name": "PH11", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH2" + ], + "AF4": [ + "I2C4_SCL" + ], + "AF12": [ + "FMC_D19" + ], + "AF13": [ + "DCMI_D2" + ], + "AF14": [ + "LCD_R5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH12": { + "Name": "PH12", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH3" + ], + "AF4": [ + "I2C4_SDA" + ], + "AF12": [ + "FMC_D20" + ], + "AF13": [ + "DCMI_D3" + ], + "AF14": [ + "LCD_R6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH13": { + "Name": "PH13", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH1N" + ], + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "CAN1_TX" + ], + "AF12": [ + "FMC_D21" + ], + "AF14": [ + "LCD_G2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH14": { + "Name": "PH14", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH2N" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "CAN1_RX" + ], + "AF12": [ + "FMC_D22" + ], + "AF13": [ + "DCMI_D4" + ], + "AF14": [ + "LCD_G3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH15": { + "Name": "PH15", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH3N" + ], + "AF12": [ + "FMC_D23" + ], + "AF13": [ + "DCMI_D11" + ], + "AF14": [ + "LCD_G4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI0": { + "Name": "PI0", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH4" + ], + "AF5": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF12": [ + "FMC_D24" + ], + "AF13": [ + "DCMI_D13" + ], + "AF14": [ + "LCD_G5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI1": { + "Name": "PI1", + "AlternateFunctions": { + "AF3": [ + "TIM8_BKIN2" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF12": [ + "FMC_D25" + ], + "AF13": [ + "DCMI_D8" + ], + "AF14": [ + "LCD_G6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI2": { + "Name": "PI2", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH4" + ], + "AF5": [ + "SPI2_MISO" + ], + "AF12": [ + "FMC_D26" + ], + "AF13": [ + "DCMI_D9" + ], + "AF14": [ + "LCD_G7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI3": { + "Name": "PI3", + "AlternateFunctions": { + "AF3": [ + "TIM8_ETR" + ], + "AF5": [ + "SPI2_MOSI", + "I2S2_SD" + ], + "AF12": [ + "FMC_D27" + ], + "AF13": [ + "DCMI_D10" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI4": { + "Name": "PI4", + "AlternateFunctions": { + "AF3": [ + "TIM8_BKIN" + ], + "AF10": [ + "SAI2_MCK_A" + ], + "AF12": [ + "FMC_NBL2" + ], + "AF13": [ + "DCMI_D5" + ], + "AF14": [ + "LCD_B4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI5": { + "Name": "PI5", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH1" + ], + "AF10": [ + "SAI2_SCK_A" + ], + "AF12": [ + "FMC_NBL3" + ], + "AF13": [ + "DCMI_VSYNC" + ], + "AF14": [ + "LCD_B5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI6": { + "Name": "PI6", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH2" + ], + "AF10": [ + "SAI2_SD_A" + ], + "AF12": [ + "FMC_D28" + ], + "AF13": [ + "DCMI_D6" + ], + "AF14": [ + "LCD_B6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI7": { + "Name": "PI7", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH3" + ], + "AF10": [ + "SAI2_FS_A" + ], + "AF12": [ + "FMC_D29" + ], + "AF13": [ + "DCMI_D7" + ], + "AF14": [ + "LCD_B7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI8": { + "Name": "PI8", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI9": { + "Name": "PI9", + "AlternateFunctions": { + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "CAN1_RX" + ], + "AF12": [ + "FMC_D30" + ], + "AF14": [ + "LCD_VSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI10": { + "Name": "PI10", + "AlternateFunctions": { + "AF11": [ + "ETH_MII_RX_ER" + ], + "AF12": [ + "FMC_D31" + ], + "AF14": [ + "LCD_HSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI11": { + "Name": "PI11", + "AlternateFunctions": { + "AF9": [ + "LCD_G6" + ], + "AF10": [ + "OTG_HS_ULPI_DIR" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI12": { + "Name": "PI12", + "AlternateFunctions": { + "AF14": [ + "LCD_HSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI13": { + "Name": "PI13", + "AlternateFunctions": { + "AF14": [ + "LCD_VSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI14": { + "Name": "PI14", + "AlternateFunctions": { + "AF14": [ + "LCD_CLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI15": { + "Name": "PI15", + "AlternateFunctions": { + "AF9": [ + "LCD_G2" + ], + "AF14": [ + "LCD_R0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ0": { + "Name": "PJ0", + "AlternateFunctions": { + "AF9": [ + "LCD_R7" + ], + "AF14": [ + "LCD_R1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ1": { + "Name": "PJ1", + "AlternateFunctions": { + "AF14": [ + "LCD_R2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ2": { + "Name": "PJ2", + "AlternateFunctions": { + "AF13": [ + "DSI_TE" + ], + "AF14": [ + "LCD_R3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ3": { + "Name": "PJ3", + "AlternateFunctions": { + "AF14": [ + "LCD_R4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ4": { + "Name": "PJ4", + "AlternateFunctions": { + "AF14": [ + "LCD_R5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ5": { + "Name": "PJ5", + "AlternateFunctions": { + "AF14": [ + "LCD_R6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ6": { + "Name": "PJ6", + "AlternateFunctions": { + "AF14": [ + "LCD_R7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ7": { + "Name": "PJ7", + "AlternateFunctions": { + "AF14": [ + "LCD_G0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ8": { + "Name": "PJ8", + "AlternateFunctions": { + "AF14": [ + "LCD_G1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ9": { + "Name": "PJ9", + "AlternateFunctions": { + "AF14": [ + "LCD_G2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ10": { + "Name": "PJ10", + "AlternateFunctions": { + "AF14": [ + "LCD_G3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ11": { + "Name": "PJ11", + "AlternateFunctions": { + "AF14": [ + "LCD_G4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ12": { + "Name": "PJ12", + "AlternateFunctions": { + "AF9": [ + "LCD_G3" + ], + "AF14": [ + "LCD_B0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ13": { + "Name": "PJ13", + "AlternateFunctions": { + "AF9": [ + "LCD_G4" + ], + "AF14": [ + "LCD_B1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ14": { + "Name": "PJ14", + "AlternateFunctions": { + "AF14": [ + "LCD_B2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ15": { + "Name": "PJ15", + "AlternateFunctions": { + "AF14": [ + "LCD_B3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK0": { + "Name": "PK0", + "AlternateFunctions": { + "AF14": [ + "LCD_G5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK1": { + "Name": "PK1", + "AlternateFunctions": { + "AF14": [ + "LCD_G6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK2": { + "Name": "PK2", + "AlternateFunctions": { + "AF14": [ + "LCD_G7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK3": { + "Name": "PK3", + "AlternateFunctions": { + "AF14": [ + "LCD_B4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK4": { + "Name": "PK4", + "AlternateFunctions": { + "AF14": [ + "LCD_B5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK5": { + "Name": "PK5", + "AlternateFunctions": { + "AF14": [ + "LCD_B6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK6": { + "Name": "PK6", + "AlternateFunctions": { + "AF14": [ + "LCD_B7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK7": { + "Name": "PK7", + "AlternateFunctions": { + "AF14": [ + "LCD_DE" + ], + "AF15": [ + "EVENTOUT" + ] + } + } + } + }, + { + "Name": "STM32H563", + "Pins": { + "PA0": { + "Name": "PA0", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH1" + ], + "AF2": [ + "TIM5_CH1" + ], + "AF3": [ + "TIM8_ETR" + ], + "AF4": [ + "TIM15_BKIN" + ], + "AF5": [ + "SPI6_NSS" + ], + "AF6": [ + "SPI3_RDY" + ], + "AF7": [ + "USART2_CTS", + "USART2_NSS" + ], + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "SDMMC2_CMD" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF11": [ + "ETH_MII_CRS" + ], + "AF14": [ + "TIM2_ETR" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA1": { + "Name": "PA1", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH2" + ], + "AF2": [ + "TIM5_CH2" + ], + "AF4": [ + "TIM15_CH1N" + ], + "AF5": [ + "LPTIM1_IN1" + ], + "AF6": [ + "OCTOSPI1_DQS" + ], + "AF7": [ + "USART2_RTS", + "USART2_DE" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "OCTOSPI1_IO3" + ], + "AF10": [ + "SAI2_MCLK_B" + ], + "AF11": [ + "ETH_MII_RX_CLK", + "ETH_RMII_REF_CLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA2": { + "Name": "PA2", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH3" + ], + "AF2": [ + "TIM5_CH3" + ], + "AF4": [ + "TIM15_CH1" + ], + "AF5": [ + "LPTIM1_IN2" + ], + "AF7": [ + "USART2_TX" + ], + "AF8": [ + "SAI2_SCK_B" + ], + "AF11": [ + "ETH_MDIO" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA3": { + "Name": "PA3", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH4" + ], + "AF2": [ + "TIM5_CH4" + ], + "AF3": [ + "OCTOSPI1_CLK" + ], + "AF4": [ + "TIM15_CH2" + ], + "AF5": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF6": [ + "SAI1_SD_B" + ], + "AF7": [ + "USART2_RX" + ], + "AF11": [ + "ETH_MII_COL" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA4": { + "Name": "PA4", + "AlternateFunctions": { + "AF2": [ + "TIM5_ETR" + ], + "AF3": [ + "LPTIM2_CH1" + ], + "AF5": [ + "SPI1_NSS", + "I2S1_WS" + ], + "AF6": [ + "SPI3_NSS", + "I2S3_WS" + ], + "AF7": [ + "USART2_CK" + ], + "AF8": [ + "SPI6_NSS" + ], + "AF13": [ + "DCMI_HSYNC", + "PSSI_DE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA5": { + "Name": "PA5", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH1" + ], + "AF3": [ + "TIM8_CH1N" + ], + "AF5": [ + "SPI1_SCK", + "I2S1_CK" + ], + "AF8": [ + "SPI6_SCK" + ], + "AF11": [ + "ETH_MII_TX_EN", + "ETH_RMII_TX_EN" + ], + "AF13": [ + "PSSI_D14" + ], + "AF14": [ + "TIM2_ETR" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA6": { + "Name": "PA6", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF2": [ + "TIM3_CH1" + ], + "AF3": [ + "TIM8_BKIN" + ], + "AF5": [ + "SPI1_MISO", + "I2S1_SDI" + ], + "AF6": [ + "OCTOSPI1_IO3" + ], + "AF7": [ + "USART11_TX" + ], + "AF8": [ + "SPI6_MISO" + ], + "AF9": [ + "TIM13_CH1" + ], + "AF13": [ + "DCMI_PIXCLK", + "PSSI_PDCK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA7": { + "Name": "PA7", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF2": [ + "TIM3_CH2" + ], + "AF3": [ + "TIM8_CH1N" + ], + "AF5": [ + "SPI1_MOSI", + "I2S1_SDO" + ], + "AF7": [ + "USART11_RX" + ], + "AF8": [ + "SPI6_MOSI" + ], + "AF9": [ + "TIM14_CH1" + ], + "AF10": [ + "OCTOSPI1_IO2" + ], + "AF11": [ + "ETH_MII_RX_DV", + "ETH_RMII_CRS_DV" + ], + "AF12": [ + "FMC_SDNWE" + ], + "AF13": [ + "FMC_NWE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA8": { + "Name": "PA8", + "AlternateFunctions": { + "AF0": [ + "MCO1" + ], + "AF1": [ + "TIM1_CH1" + ], + "AF3": [ + "TIM8_BKIN2" + ], + "AF4": [ + "I2C3_SCL" + ], + "AF5": [ + "SPI1_RDY" + ], + "AF7": [ + "USART1_CK" + ], + "AF10": [ + "USB_SOF" + ], + "AF11": [ + "UART7_RX" + ], + "AF12": [ + "FMC_NOE" + ], + "AF13": [ + "DCMI_D3", + "PSSI_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA9": { + "Name": "PA9", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2" + ], + "AF3": [ + "LPUART1_TX" + ], + "AF4": [ + "I2C3_SMBA" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF7": [ + "USART1_TX" + ], + "AF11": [ + "ETH_MII_TX_ER" + ], + "AF12": [ + "FMC_NWE" + ], + "AF13": [ + "DCMI_D0", + "PSSI_D0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA10": { + "Name": "PA10", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3" + ], + "AF3": [ + "LPUART1_RX" + ], + "AF4": [ + "LPTIM2_IN2" + ], + "AF6": [ + "UCPD1_FRSTX" + ], + "AF7": [ + "USART1_RX" + ], + "AF9": [ + "FDCAN2_TX" + ], + "AF12": [ + "SDMMC1_D0" + ], + "AF13": [ + "DCMI_D1", + "PSSI_D1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA11": { + "Name": "PA11", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH4" + ], + "AF3": [ + "LPUART1_CTS" + ], + "AF5": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF6": [ + "UART4_RX" + ], + "AF7": [ + "USART1_CTS", + "USART1_NSS" + ], + "AF9": [ + "FDCAN1_RX" + ], + "AF10": [ + "USB_DM" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA12": { + "Name": "PA12", + "AlternateFunctions": { + "AF1": [ + "TIM1_ETR" + ], + "AF3": [ + "LPUART1_RTS", + "LPUART1_DE" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF6": [ + "UART4_TX" + ], + "AF7": [ + "USART1_RTS", + "USART1_DE" + ], + "AF8": [ + "SAI2_FS_B" + ], + "AF9": [ + "FDCAN1_TX" + ], + "AF10": [ + "USB_DP" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA13": { + "Name": "PA13", + "AlternateFunctions": { + "AF0": [ + "JTMS", + "SWDIO" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA14": { + "Name": "PA14", + "AlternateFunctions": { + "AF0": [ + "JTCK", + "SWCLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA15": { + "Name": "PA15", + "AlternateFunctions": { + "AF0": [ + "JTDI" + ], + "AF1": [ + "TIM2_CH1" + ], + "AF2": [ + "LPTIM3_IN2" + ], + "AF4": [ + "HDMI_CEC" + ], + "AF5": [ + "SPI1_NSS", + "I2S1_WS" + ], + "AF6": [ + "SPI3_NSS", + "I2S3_WS" + ], + "AF7": [ + "SPI6_NSS" + ], + "AF8": [ + "UART4_RTS", + "UART4_DE" + ], + "AF11": [ + "UART7_TX" + ], + "AF12": [ + "FMC_NBL1" + ], + "AF13": [ + "DCMI_D11", + "PSSI_D11" + ], + "AF14": [ + "TIM2_ETR" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB0": { + "Name": "PB0", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF2": [ + "TIM3_CH3" + ], + "AF3": [ + "TIM8_CH2N" + ], + "AF6": [ + "OCTOSPI1_IO1" + ], + "AF7": [ + "USART11_CK" + ], + "AF8": [ + "UART4_CTS" + ], + "AF11": [ + "ETH_MII_RXD2" + ], + "AF14": [ + "LPTIM3_CH1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB1": { + "Name": "PB1", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3N" + ], + "AF2": [ + "TIM3_CH4" + ], + "AF3": [ + "TIM8_CH3N" + ], + "AF6": [ + "OCTOSPI1_IO0" + ], + "AF11": [ + "ETH_MII_RXD3" + ], + "AF14": [ + "LPTIM3_CH2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB2": { + "Name": "PB2", + "AlternateFunctions": { + "AF0": [ + "RTC_OUT2" + ], + "AF2": [ + "SAI1_D1" + ], + "AF3": [ + "TIM8_CH4N" + ], + "AF4": [ + "SPI1_RDY" + ], + "AF5": [ + "LPTIM1_CH1" + ], + "AF6": [ + "SAI1_SD_A" + ], + "AF7": [ + "SPI3_MOSI", + "I2S3_SDO" + ], + "AF9": [ + "OCTOSPI1_CLK" + ], + "AF10": [ + "OCTOSPI1_DQS" + ], + "AF12": [ + "SDMMC1_CMD" + ], + "AF13": [ + "LPTIM5_ETR" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB3": { + "Name": "PB3", + "AlternateFunctions": { + "AF0": [ + "JTDO", + "TRACESWO" + ], + "AF1": [ + "TIM2_CH2" + ], + "AF4": [ + "I2C2_SDA" + ], + "AF5": [ + "SPI1_SCK", + "I2S1_CK" + ], + "AF6": [ + "SPI3_SCK", + "I2S3_CK" + ], + "AF7": [ + "UART12_CTS", + "UART12_NSS" + ], + "AF8": [ + "SPI6_SCK" + ], + "AF9": [ + "SDMMC2_D2" + ], + "AF10": [ + "CRS_SYNC" + ], + "AF11": [ + "UART7_RX" + ], + "AF14": [ + "LPTIM6_ETR" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB4": { + "Name": "PB4", + "AlternateFunctions": { + "AF0": [ + "NJTRST" + ], + "AF1": [ + "TIM16_BKIN" + ], + "AF2": [ + "TIM3_CH1" + ], + "AF3": [ + "OCTOSPI1_CLK" + ], + "AF4": [ + "LPTIM1_CH2" + ], + "AF5": [ + "SPI1_MISO", + "I2S1_SDI" + ], + "AF6": [ + "SPI3_MISO", + "I2S3_SDI" + ], + "AF7": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF8": [ + "SPI6_MISO" + ], + "AF9": [ + "SDMMC2_D3" + ], + "AF11": [ + "UART7_TX" + ], + "AF13": [ + "DCMI_D7", + "PSSI_D7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB5": { + "Name": "PB5", + "AlternateFunctions": { + "AF1": [ + "TIM17_BKIN" + ], + "AF2": [ + "TIM3_CH2" + ], + "AF3": [ + "OCTOSPI1_NCLK" + ], + "AF4": [ + "I2C1_SMBA" + ], + "AF5": [ + "SPI1_MOSI", + "I2S1_SDO" + ], + "AF6": [ + "I2C4_SMBA" + ], + "AF7": [ + "SPI3_MOSI", + "I2S3_SDO" + ], + "AF8": [ + "SPI6_MOSI" + ], + "AF9": [ + "FDCAN2_RX" + ], + "AF11": [ + "ETH_PPS_OUT" + ], + "AF12": [ + "FMC_SDCKE1" + ], + "AF13": [ + "DCMI_D10", + "PSSI_D10" + ], + "AF14": [ + "UART5_RX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB6": { + "Name": "PB6", + "AlternateFunctions": { + "AF1": [ + "TIM16_CH1N" + ], + "AF2": [ + "TIM4_CH1" + ], + "AF3": [ + "I3C1_SCL" + ], + "AF4": [ + "I2C1_SCL" + ], + "AF5": [ + "HDMI_CEC" + ], + "AF6": [ + "I2C4_SCL" + ], + "AF7": [ + "USART1_TX" + ], + "AF8": [ + "LPUART1_TX" + ], + "AF9": [ + "FDCAN2_TX" + ], + "AF10": [ + "OCTOSPI1_NCS" + ], + "AF12": [ + "FMC_SDNE1" + ], + "AF13": [ + "DCMI_D5", + "PSSI_D5" + ], + "AF14": [ + "UART5_TX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB7": { + "Name": "PB7", + "AlternateFunctions": { + "AF1": [ + "TIM17_CH1N" + ], + "AF2": [ + "TIM4_CH2" + ], + "AF3": [ + "I3C1_SDA" + ], + "AF4": [ + "I2C1_SDA" + ], + "AF6": [ + "I2C4_SDA" + ], + "AF7": [ + "USART1_RX" + ], + "AF8": [ + "LPUART1_RX" + ], + "AF9": [ + "FDCAN1_TX" + ], + "AF10": [ + "SDMMC2_D5" + ], + "AF11": [ + "SDMMC2_CKIN" + ], + "AF12": [ + "FMC_NL" + ], + "AF13": [ + "DCMI_VSYNC", + "PSSI_RDY" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB8": { + "Name": "PB8", + "AlternateFunctions": { + "AF1": [ + "TIM16_CH1" + ], + "AF2": [ + "TIM4_CH3" + ], + "AF3": [ + "I3C1_SCL" + ], + "AF4": [ + "I2C1_SCL" + ], + "AF5": [ + "SPI4_RDY" + ], + "AF6": [ + "I2C4_SCL" + ], + "AF7": [ + "SDMMC1_CKIN" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "FDCAN1_RX" + ], + "AF10": [ + "SDMMC2_D4" + ], + "AF11": [ + "ETH_MII_TXD3" + ], + "AF12": [ + "SDMMC1_D4" + ], + "AF13": [ + "DCMI_D6", + "PSSI_D6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB9": { + "Name": "PB9", + "AlternateFunctions": { + "AF1": [ + "TIM17_CH1" + ], + "AF2": [ + "TIM4_CH4" + ], + "AF3": [ + "I3C1_SDA" + ], + "AF4": [ + "I2C1_SDA" + ], + "AF5": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF6": [ + "I2C4_SDA" + ], + "AF7": [ + "SDMMC1_CDIR" + ], + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "FDCAN1_TX" + ], + "AF10": [ + "SDMMC2_D5" + ], + "AF11": [ + "SDMMC2_CKIN" + ], + "AF12": [ + "SDMMC1_D5" + ], + "AF13": [ + "DCMI_D7", + "PSSI_D7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB10": { + "Name": "PB10", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH3" + ], + "AF2": [ + "LPTIM3_CH1" + ], + "AF3": [ + "LPTIM2_IN1" + ], + "AF4": [ + "I2C2_SCL" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF7": [ + "USART3_TX" + ], + "AF9": [ + "OCTOSPI1_NCS" + ], + "AF11": [ + "ETH_MII_RX_ER" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB11": { + "Name": "PB11", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH4" + ], + "AF3": [ + "LPTIM2_ETR" + ], + "AF4": [ + "I2C2_SDA" + ], + "AF5": [ + "SPI2_RDY" + ], + "AF6": [ + "SPI4_RDY" + ], + "AF7": [ + "USART3_RX" + ], + "AF11": [ + "ETH_MII_TX_EN", + "ETH_RMII_TX_EN" + ], + "AF12": [ + "FMC_NBL1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB12": { + "Name": "PB12", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF3": [ + "OCTOSPI1_NCLK" + ], + "AF4": [ + "I2C2_SDA" + ], + "AF5": [ + "SPI2_NSS", + "I2S2_WS" + ], + "AF6": [ + "UCPD1_FRSTX" + ], + "AF7": [ + "USART3_CK" + ], + "AF9": [ + "FDCAN2_RX" + ], + "AF11": [ + "ETH_MII_TXD0", + "ETH_RMII_TXD0" + ], + "AF14": [ + "UART5_RX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB13": { + "Name": "PB13", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF2": [ + "LPTIM3_IN1" + ], + "AF3": [ + "LPTIM2_CH1" + ], + "AF4": [ + "I2C2_SMBA" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF7": [ + "USART3_CTS", + "USART3_NSS" + ], + "AF9": [ + "FDCAN2_TX" + ], + "AF12": [ + "SDMMC1_D0" + ], + "AF14": [ + "UART5_TX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB14": { + "Name": "PB14", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF2": [ + "TIM12_CH1" + ], + "AF3": [ + "TIM8_CH2N" + ], + "AF4": [ + "USART1_TX" + ], + "AF5": [ + "SPI2_MISO", + "I2S2_SDI" + ], + "AF7": [ + "USART3_RTS", + "USART3_DE" + ], + "AF8": [ + "UART4_RTS", + "UART4_DE" + ], + "AF9": [ + "SDMMC2_D0" + ], + "AF14": [ + "LPTIM3_ETR" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB15": { + "Name": "PB15", + "AlternateFunctions": { + "AF0": [ + "RTC_REFIN" + ], + "AF1": [ + "TIM1_CH3N" + ], + "AF2": [ + "TIM12_CH2" + ], + "AF3": [ + "TIM8_CH3N" + ], + "AF4": [ + "USART1_RX" + ], + "AF5": [ + "SPI2_MOSI", + "I2S2_SDO" + ], + "AF7": [ + "USART11_CTS", + "USART11_NSS" + ], + "AF8": [ + "UART4_CTS" + ], + "AF9": [ + "SDMMC2_D1" + ], + "AF10": [ + "OCTOSPI1_CLK" + ], + "AF11": [ + "ETH_MII_TXD1", + "ETH_RMII_TXD1" + ], + "AF13": [ + "DCMI_D2", + "PSSI_D2" + ], + "AF14": [ + "UART5_RX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC0": { + "Name": "PC0", + "AlternateFunctions": { + "AF1": [ + "TIM16_BKIN" + ], + "AF6": [ + "SAI1_MCLK_A" + ], + "AF7": [ + "SPI2_RDY" + ], + "AF8": [ + "SAI2_FS_B" + ], + "AF9": [ + "FMC_A25" + ], + "AF10": [ + "OCTOSPI1_IO7" + ], + "AF12": [ + "FMC_SDNWE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC1": { + "Name": "PC1", + "AlternateFunctions": { + "AF0": [ + "TRACED0" + ], + "AF2": [ + "SAI1_D1" + ], + "AF5": [ + "SPI2_MOSI", + "I2S2_SDO" + ], + "AF6": [ + "SAI1_SD_A" + ], + "AF7": [ + "USART11_RTS", + "USART11_DE" + ], + "AF8": [ + "SAI2_SD_A" + ], + "AF9": [ + "SDMMC2_CK" + ], + "AF10": [ + "OCTOSPI1_IO4" + ], + "AF11": [ + "ETH_MDC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC2": { + "Name": "PC2", + "AlternateFunctions": { + "AF0": [ + "PWR_CSLEEP" + ], + "AF1": [ + "TIM17_CH1" + ], + "AF2": [ + "TIM4_CH4" + ], + "AF5": [ + "SPI2_MISO", + "I2S2_SDI" + ], + "AF6": [ + "OCTOSPI1_IO5" + ], + "AF9": [ + "OCTOSPI1_IO2" + ], + "AF11": [ + "ETH_MII_TXD2" + ], + "AF12": [ + "FMC_SDNE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC3": { + "Name": "PC3", + "AlternateFunctions": { + "AF0": [ + "PWR_CSTOP" + ], + "AF2": [ + "SAI1_D3" + ], + "AF3": [ + "LPTIM3_CH1" + ], + "AF5": [ + "SPI2_MOSI", + "I2S2_SDO" + ], + "AF6": [ + "OCTOSPI1_IO6" + ], + "AF9": [ + "OCTOSPI1_IO0" + ], + "AF11": [ + "ETH_MII_TX_CLK" + ], + "AF12": [ + "FMC_SDCKE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC4": { + "Name": "PC4", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH4" + ], + "AF2": [ + "SAI1_CK1" + ], + "AF3": [ + "LPTIM2_ETR" + ], + "AF5": [ + "I2S1_MCK" + ], + "AF7": [ + "USART3_RX" + ], + "AF11": [ + "ETH_MII_RXD0", + "ETH_RMII_RXD0" + ], + "AF12": [ + "FMC_SDNE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC5": { + "Name": "PC5", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH4N" + ], + "AF2": [ + "SAI1_D3" + ], + "AF4": [ + "PSSI_D15" + ], + "AF6": [ + "SAI1_FS_A" + ], + "AF7": [ + "UART12_RTS", + "UART12_DE" + ], + "AF10": [ + "OCTOSPI1_DQS" + ], + "AF11": [ + "ETH_MII_RXD1", + "ETH_RMII_RXD1" + ], + "AF12": [ + "FMC_SDCKE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC6": { + "Name": "PC6", + "AlternateFunctions": { + "AF2": [ + "TIM3_CH1" + ], + "AF3": [ + "TIM8_CH1" + ], + "AF5": [ + "I2S2_MCK" + ], + "AF6": [ + "SAI1_SCK_A" + ], + "AF7": [ + "USART6_TX" + ], + "AF8": [ + "SDMMC1_D0DIR" + ], + "AF9": [ + "FMC_NWAIT" + ], + "AF10": [ + "SDMMC2_D6" + ], + "AF11": [ + "OCTOSPI1_IO5" + ], + "AF12": [ + "SDMMC1_D6" + ], + "AF13": [ + "DCMI_D0", + "PSSI_D0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC7": { + "Name": "PC7", + "AlternateFunctions": { + "AF0": [ + "TRGIO" + ], + "AF2": [ + "TIM3_CH2" + ], + "AF3": [ + "TIM8_CH2" + ], + "AF6": [ + "I2S3_MCK" + ], + "AF7": [ + "USART6_RX" + ], + "AF8": [ + "SDMMC1_D123DIR" + ], + "AF9": [ + "FMC_NE1" + ], + "AF10": [ + "SDMMC2_D7" + ], + "AF11": [ + "OCTOSPI1_IO6" + ], + "AF12": [ + "SDMMC1_D7" + ], + "AF13": [ + "DCMI_D1", + "PSSI_D1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC8": { + "Name": "PC8", + "AlternateFunctions": { + "AF0": [ + "TRACED1" + ], + "AF2": [ + "TIM3_CH3" + ], + "AF3": [ + "TIM8_CH3" + ], + "AF7": [ + "USART6_CK" + ], + "AF8": [ + "UART5_RTS", + "UART5_DE" + ], + "AF9": [ + "FMC_NE2", + "FMC_NCE" + ], + "AF10": [ + "FMC_INT" + ], + "AF11": [ + "FMC_ALE" + ], + "AF12": [ + "SDMMC1_D0" + ], + "AF13": [ + "DCMI_D2", + "PSSI_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC9": { + "Name": "PC9", + "AlternateFunctions": { + "AF0": [ + "MCO2" + ], + "AF2": [ + "TIM3_CH4" + ], + "AF3": [ + "TIM8_CH4" + ], + "AF4": [ + "I2C3_SDA" + ], + "AF5": [ + "AUDIOCLK" + ], + "AF8": [ + "UART5_CTS" + ], + "AF9": [ + "OCTOSPI1_IO0" + ], + "AF11": [ + "FMC_CLE" + ], + "AF12": [ + "SDMMC1_D1" + ], + "AF13": [ + "DCMI_D3", + "PSSI_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC10": { + "Name": "PC10", + "AlternateFunctions": { + "AF2": [ + "LPTIM3_ETR" + ], + "AF6": [ + "SPI3_SCK", + "I2S3_CK" + ], + "AF7": [ + "USART3_TX" + ], + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "OCTOSPI1_IO1" + ], + "AF11": [ + "ETH_MII_TXD0", + "ETH_RMII_TXD0" + ], + "AF12": [ + "SDMMC1_D2" + ], + "AF13": [ + "DCMI_D8", + "PSSI_D8" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC11": { + "Name": "PC11", + "AlternateFunctions": { + "AF2": [ + "LPTIM3_IN1" + ], + "AF6": [ + "SPI3_MISO", + "I2S3_SDI" + ], + "AF7": [ + "USART3_RX" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "OCTOSPI1_NCS" + ], + "AF12": [ + "SDMMC1_D3" + ], + "AF13": [ + "DCMI_D4", + "PSSI_D4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC12": { + "Name": "PC12", + "AlternateFunctions": { + "AF0": [ + "TRACED3" + ], + "AF2": [ + "TIM15_CH1" + ], + "AF5": [ + "SPI6_SCK" + ], + "AF6": [ + "SPI3_MOSI", + "I2S3_SDO" + ], + "AF7": [ + "USART3_CK" + ], + "AF8": [ + "UART5_TX" + ], + "AF12": [ + "SDMMC1_CK" + ], + "AF13": [ + "DCMI_D9", + "PSSI_D9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC13": { + "Name": "PC13", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC14": { + "Name": "PC14", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC15": { + "Name": "PC15", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD0": { + "Name": "PD0", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH4N" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "FDCAN1_RX" + ], + "AF11": [ + "UART9_CTS" + ], + "AF12": [ + "FMC_D2", + "FMC_AD2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD1": { + "Name": "PD1", + "AlternateFunctions": { + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "FDCAN1_TX" + ], + "AF12": [ + "FMC_D3", + "FMC_AD3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD2": { + "Name": "PD2", + "AlternateFunctions": { + "AF0": [ + "TRACED2" + ], + "AF2": [ + "TIM3_ETR" + ], + "AF4": [ + "TIM15_BKIN" + ], + "AF8": [ + "UART5_RX" + ], + "AF12": [ + "SDMMC1_CMD" + ], + "AF13": [ + "DCMI_D11", + "PSSI_D11" + ], + "AF14": [ + "LPTIM4_ETR" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD3": { + "Name": "PD3", + "AlternateFunctions": { + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF7": [ + "USART2_CTS", + "USART2_NSS" + ], + "AF12": [ + "FMC_CLK" + ], + "AF13": [ + "DCMI_D5", + "PSSI_D5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD4": { + "Name": "PD4", + "AlternateFunctions": { + "AF7": [ + "USART2_RTS", + "USART2_DE" + ], + "AF10": [ + "OCTOSPI1_IO4" + ], + "AF12": [ + "FMC_NOE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD5": { + "Name": "PD5", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH4N" + ], + "AF5": [ + "SPI2_RDY" + ], + "AF7": [ + "USART2_TX" + ], + "AF9": [ + "FDCAN1_TX" + ], + "AF10": [ + "OCTOSPI1_IO5" + ], + "AF12": [ + "FMC_NWE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD6": { + "Name": "PD6", + "AlternateFunctions": { + "AF2": [ + "SAI1_D1" + ], + "AF5": [ + "SPI3_MOSI", + "I2S3_SDO" + ], + "AF6": [ + "SAI1_SD_A" + ], + "AF7": [ + "USART2_RX" + ], + "AF10": [ + "OCTOSPI1_IO6" + ], + "AF11": [ + "SDMMC2_CK" + ], + "AF12": [ + "FMC_NWAIT" + ], + "AF13": [ + "DCMI_D10", + "PSSI_D10" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD7": { + "Name": "PD7", + "AlternateFunctions": { + "AF5": [ + "SPI1_MOSI", + "I2S1_SDO" + ], + "AF7": [ + "USART2_CK" + ], + "AF10": [ + "OCTOSPI1_IO7" + ], + "AF11": [ + "SDMMC2_CMD" + ], + "AF12": [ + "FMC_NE1", + "FMC_NCE" + ], + "AF14": [ + "LPTIM4_OUT" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD8": { + "Name": "PD8", + "AlternateFunctions": { + "AF7": [ + "USART3_TX" + ], + "AF12": [ + "FMC_D13", + "FMC_AD13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD9": { + "Name": "PD9", + "AlternateFunctions": { + "AF7": [ + "USART3_RX" + ], + "AF9": [ + "FDCAN2_RX" + ], + "AF12": [ + "FMC_D14", + "FMC_AD14" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD10": { + "Name": "PD10", + "AlternateFunctions": { + "AF3": [ + "LPTIM2_CH2" + ], + "AF7": [ + "USART3_CK" + ], + "AF12": [ + "FMC_D15", + "FMC_AD15" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD11": { + "Name": "PD11", + "AlternateFunctions": { + "AF2": [ + "SAI1_CK1" + ], + "AF3": [ + "LPTIM2_IN2" + ], + "AF4": [ + "I2C4_SMBA" + ], + "AF7": [ + "USART3_CTS", + "USART3_NSS" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "OCTOSPI1_IO0" + ], + "AF10": [ + "SAI2_SD_A" + ], + "AF12": [ + "FMC_A16", + "FMC_CLE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD12": { + "Name": "PD12", + "AlternateFunctions": { + "AF1": [ + "LPTIM1_IN1" + ], + "AF2": [ + "TIM4_CH1" + ], + "AF3": [ + "LPTIM2_IN1" + ], + "AF4": [ + "I2C4_SCL" + ], + "AF5": [ + "I3C1_SCL" + ], + "AF6": [ + "SAI1_D1" + ], + "AF7": [ + "USART3_RTS", + "USART3_DE" + ], + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "OCTOSPI1_IO1" + ], + "AF10": [ + "SAI2_FS_A" + ], + "AF12": [ + "FMC_A17", + "FMC_ALE" + ], + "AF13": [ + "DCMI_D12", + "PSSI_D12" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD13": { + "Name": "PD13", + "AlternateFunctions": { + "AF1": [ + "LPTIM1_CH1" + ], + "AF2": [ + "TIM4_CH2" + ], + "AF3": [ + "LPTIM2_CH1" + ], + "AF4": [ + "I2C4_SDA" + ], + "AF5": [ + "I3C1_SDA" + ], + "AF9": [ + "OCTOSPI1_IO3" + ], + "AF10": [ + "SAI2_SCK_A" + ], + "AF11": [ + "UART9_RTS", + "UART9_DE" + ], + "AF12": [ + "FMC_A18" + ], + "AF13": [ + "DCMI_D13", + "PSSI_D13" + ], + "AF14": [ + "LPTIM4_IN1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD14": { + "Name": "PD14", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH3" + ], + "AF8": [ + "UART8_CTS" + ], + "AF11": [ + "UART9_RX" + ], + "AF12": [ + "FMC_D0", + "FMC_AD0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD15": { + "Name": "PD15", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH4" + ], + "AF8": [ + "UART8_RTS", + "UART8_DE" + ], + "AF11": [ + "UART9_TX" + ], + "AF12": [ + "FMC_D1", + "FMC_AD1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE0": { + "Name": "PE0", + "AlternateFunctions": { + "AF1": [ + "LPTIM1_ETR" + ], + "AF2": [ + "TIM4_ETR" + ], + "AF3": [ + "LPTIM2_CH2" + ], + "AF4": [ + "LPTIM2_ETR" + ], + "AF6": [ + "SPI3_RDY" + ], + "AF8": [ + "UART8_RX" + ], + "AF9": [ + "FDCAN1_RX" + ], + "AF10": [ + "SAI2_MCLK_A" + ], + "AF12": [ + "FMC_NBL0" + ], + "AF13": [ + "DCMI_D2", + "PSSI_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE1": { + "Name": "PE1", + "AlternateFunctions": { + "AF1": [ + "LPTIM1_IN2" + ], + "AF8": [ + "UART8_TX" + ], + "AF9": [ + "FDCAN1_TX" + ], + "AF12": [ + "FMC_NBL1" + ], + "AF13": [ + "DCMI_D3", + "PSSI_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE2": { + "Name": "PE2", + "AlternateFunctions": { + "AF0": [ + "TRACECLK" + ], + "AF1": [ + "LPTIM1_IN2" + ], + "AF2": [ + "SAI1_CK1" + ], + "AF5": [ + "SPI4_SCK" + ], + "AF6": [ + "SAI1_MCLK_A" + ], + "AF7": [ + "USART10_RX" + ], + "AF8": [ + "UART8_TX" + ], + "AF9": [ + "OCTOSPI1_IO2" + ], + "AF11": [ + "ETH_MII_TXD3" + ], + "AF12": [ + "FMC_A23" + ], + "AF13": [ + "DCMI_D3", + "PSSI_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE3": { + "Name": "PE3", + "AlternateFunctions": { + "AF0": [ + "TRACED0" + ], + "AF4": [ + "TIM15_BKIN" + ], + "AF6": [ + "SAI1_SD_B" + ], + "AF7": [ + "USART10_TX" + ], + "AF12": [ + "FMC_A19" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE4": { + "Name": "PE4", + "AlternateFunctions": { + "AF0": [ + "TRACED1" + ], + "AF2": [ + "SAI1_D2" + ], + "AF4": [ + "TIM15_CH1N" + ], + "AF5": [ + "SPI4_NSS" + ], + "AF6": [ + "SAI1_FS_A" + ], + "AF12": [ + "FMC_A20" + ], + "AF13": [ + "DCMI_D4", + "PSSI_D4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE5": { + "Name": "PE5", + "AlternateFunctions": { + "AF0": [ + "TRACED2" + ], + "AF2": [ + "SAI1_CK2" + ], + "AF4": [ + "TIM15_CH1" + ], + "AF5": [ + "SPI4_MISO" + ], + "AF6": [ + "SAI1_SCK_A" + ], + "AF12": [ + "FMC_A21" + ], + "AF13": [ + "DCMI_D6", + "PSSI_D6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE6": { + "Name": "PE6", + "AlternateFunctions": { + "AF0": [ + "TRACED3" + ], + "AF1": [ + "TIM1_BKIN2" + ], + "AF2": [ + "SAI1_D1" + ], + "AF4": [ + "TIM15_CH2" + ], + "AF5": [ + "SPI4_MOSI" + ], + "AF6": [ + "SAI1_SD_A" + ], + "AF10": [ + "SAI2_MCLK_B" + ], + "AF12": [ + "FMC_A22" + ], + "AF13": [ + "DCMI_D7", + "PSSI_D7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE7": { + "Name": "PE7", + "AlternateFunctions": { + "AF1": [ + "TIM1_ETR" + ], + "AF6": [ + "UART12_RTS", + "UART12_DE" + ], + "AF7": [ + "UART7_RX" + ], + "AF10": [ + "OCTOSPI1_IO4" + ], + "AF12": [ + "FMC_D4", + "FMC_AD4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE8": { + "Name": "PE8", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF6": [ + "UART12_CTS", + "UART12_NSS" + ], + "AF7": [ + "UART7_TX" + ], + "AF10": [ + "OCTOSPI1_IO5" + ], + "AF12": [ + "FMC_D5", + "FMC_AD5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE9": { + "Name": "PE9", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1" + ], + "AF6": [ + "UART12_RX" + ], + "AF7": [ + "UART7_RTS", + "UART7_DE" + ], + "AF10": [ + "OCTOSPI1_IO6" + ], + "AF12": [ + "FMC_D6", + "FMC_AD6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE10": { + "Name": "PE10", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF6": [ + "UART12_TX" + ], + "AF7": [ + "UART7_CTS" + ], + "AF10": [ + "OCTOSPI1_IO7" + ], + "AF12": [ + "FMC_D7", + "FMC_AD7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE11": { + "Name": "PE11", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2" + ], + "AF4": [ + "SPI1_RDY" + ], + "AF5": [ + "SPI4_NSS" + ], + "AF6": [ + "OCTOSPI1_NCS" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF12": [ + "FMC_D8", + "FMC_AD8" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE12": { + "Name": "PE12", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3N" + ], + "AF5": [ + "SPI4_SCK" + ], + "AF10": [ + "SAI2_SCK_B" + ], + "AF12": [ + "FMC_D9", + "FMC_AD9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE13": { + "Name": "PE13", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3" + ], + "AF5": [ + "SPI4_MISO" + ], + "AF10": [ + "SAI2_FS_B" + ], + "AF12": [ + "FMC_D10", + "FMC_AD10" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE14": { + "Name": "PE14", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH4" + ], + "AF5": [ + "SPI4_MOSI" + ], + "AF10": [ + "SAI2_MCLK_B" + ], + "AF12": [ + "FMC_D11", + "FMC_AD11" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE15": { + "Name": "PE15", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF3": [ + "TIM1_CH4N" + ], + "AF7": [ + "USART10_CK" + ], + "AF12": [ + "FMC_D12", + "FMC_AD12" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF0": { + "Name": "PF0", + "AlternateFunctions": { + "AF4": [ + "I2C2_SDA" + ], + "AF12": [ + "FMC_A0" + ], + "AF13": [ + "LPTIM5_CH1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF1": { + "Name": "PF1", + "AlternateFunctions": { + "AF4": [ + "I2C2_SCL" + ], + "AF12": [ + "FMC_A1" + ], + "AF13": [ + "LPTIM5_CH2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF2": { + "Name": "PF2", + "AlternateFunctions": { + "AF2": [ + "LPTIM3_CH2" + ], + "AF3": [ + "LPTIM3_IN2" + ], + "AF4": [ + "I2C2_SMBA" + ], + "AF6": [ + "UART12_TX" + ], + "AF7": [ + "USART11_CK" + ], + "AF12": [ + "FMC_A2" + ], + "AF13": [ + "LPTIM5_IN1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF3": { + "Name": "PF3", + "AlternateFunctions": { + "AF2": [ + "LPTIM3_IN1" + ], + "AF7": [ + "USART11_TX" + ], + "AF12": [ + "FMC_A3" + ], + "AF13": [ + "LPTIM5_IN2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF4": { + "Name": "PF4", + "AlternateFunctions": { + "AF2": [ + "LPTIM3_ETR" + ], + "AF7": [ + "USART11_RX" + ], + "AF12": [ + "FMC_A4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF5": { + "Name": "PF5", + "AlternateFunctions": { + "AF2": [ + "LPTIM3_CH1" + ], + "AF4": [ + "I2C4_SCL" + ], + "AF5": [ + "I3C1_SCL" + ], + "AF6": [ + "UART12_RX" + ], + "AF7": [ + "USART11_CTS", + "USART11_NSS" + ], + "AF12": [ + "FMC_A5" + ], + "AF14": [ + "LPTIM3_IN1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF6": { + "Name": "PF6", + "AlternateFunctions": { + "AF1": [ + "TIM16_CH1" + ], + "AF5": [ + "SPI5_NSS" + ], + "AF6": [ + "SAI1_SD_B" + ], + "AF7": [ + "UART7_RX" + ], + "AF10": [ + "OCTOSPI1_IO3" + ], + "AF13": [ + "LPTIM5_CH1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF7": { + "Name": "PF7", + "AlternateFunctions": { + "AF1": [ + "TIM17_CH1" + ], + "AF5": [ + "SPI5_SCK" + ], + "AF6": [ + "SAI1_MCLK_B" + ], + "AF7": [ + "UART7_TX" + ], + "AF10": [ + "OCTOSPI1_IO2" + ], + "AF13": [ + "LPTIM5_CH2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF8": { + "Name": "PF8", + "AlternateFunctions": { + "AF1": [ + "TIM16_CH1N" + ], + "AF5": [ + "SPI5_MISO" + ], + "AF6": [ + "SAI1_SCK_B" + ], + "AF7": [ + "UART7_RTS", + "UART7_DE" + ], + "AF9": [ + "TIM13_CH1" + ], + "AF10": [ + "OCTOSPI1_IO0" + ], + "AF13": [ + "LPTIM5_IN1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF9": { + "Name": "PF9", + "AlternateFunctions": { + "AF1": [ + "TIM17_CH1N" + ], + "AF5": [ + "SPI5_MOSI" + ], + "AF6": [ + "SAI1_FS_B" + ], + "AF7": [ + "UART7_CTS" + ], + "AF9": [ + "TIM14_CH1" + ], + "AF10": [ + "OCTOSPI1_IO1" + ], + "AF13": [ + "LPTIM5_IN2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF10": { + "Name": "PF10", + "AlternateFunctions": { + "AF1": [ + "TIM16_BKIN" + ], + "AF2": [ + "SAI1_D3" + ], + "AF4": [ + "PSSI_D15" + ], + "AF9": [ + "OCTOSPI1_CLK" + ], + "AF13": [ + "DCMI_D11", + "PSSI_D11" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF11": { + "Name": "PF11", + "AlternateFunctions": { + "AF5": [ + "SPI5_MOSI" + ], + "AF9": [ + "OCTOSPI1_NCLK" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF12": [ + "FMC_NRAS" + ], + "AF13": [ + "DCMI_D12", + "PSSI_D12" + ], + "AF14": [ + "LPTIM6_CH1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF12": { + "Name": "PF12", + "AlternateFunctions": { + "AF12": [ + "FMC_A6" + ], + "AF14": [ + "LPTIM6_CH2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF13": { + "Name": "PF13", + "AlternateFunctions": { + "AF4": [ + "I2C4_SMBA" + ], + "AF12": [ + "FMC_A7" + ], + "AF14": [ + "LPTIM6_IN1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF14": { + "Name": "PF14", + "AlternateFunctions": { + "AF12": [ + "FMC_A8" + ], + "AF14": [ + "LPTIM6_IN2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF15": { + "Name": "PF15", + "AlternateFunctions": { + "AF4": [ + "I2C4_SDA" + ], + "AF5": [ + "I3C1_SDA" + ], + "AF12": [ + "FMC_A9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG0": { + "Name": "PG0", + "AlternateFunctions": { + "AF11": [ + "UART9_RX" + ], + "AF12": [ + "FMC_A10" + ], + "AF14": [ + "LPTIM4_IN1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG1": { + "Name": "PG1", + "AlternateFunctions": { + "AF7": [ + "SPI2_MOSI", + "I2S2_SDO" + ], + "AF11": [ + "UART9_TX" + ], + "AF12": [ + "FMC_A11" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG2": { + "Name": "PG2", + "AlternateFunctions": { + "AF3": [ + "TIM8_BKIN" + ], + "AF7": [ + "UART12_RX" + ], + "AF12": [ + "FMC_A12" + ], + "AF14": [ + "LPTIM6_ETR" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG3": { + "Name": "PG3", + "AlternateFunctions": { + "AF3": [ + "TIM8_BKIN2" + ], + "AF7": [ + "UART12_TX" + ], + "AF12": [ + "FMC_A13" + ], + "AF13": [ + "LPTIM5_ETR" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG4": { + "Name": "PG4", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN2" + ], + "AF12": [ + "FMC_A14", + "FMC_BA0" + ], + "AF14": [ + "LPTIM4_ETR" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG5": { + "Name": "PG5", + "AlternateFunctions": { + "AF1": [ + "TIM1_ETR" + ], + "AF12": [ + "FMC_A15", + "FMC_BA1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG6": { + "Name": "PG6", + "AlternateFunctions": { + "AF1": [ + "TIM17_BKIN" + ], + "AF3": [ + "I3C1_SDA" + ], + "AF4": [ + "I2C4_SDA" + ], + "AF5": [ + "SPI1_RDY" + ], + "AF10": [ + "OCTOSPI1_NCS" + ], + "AF11": [ + "UCPD1_FRSTX" + ], + "AF12": [ + "FMC_NE3" + ], + "AF13": [ + "DCMI_D12", + "PSSI_D12" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG7": { + "Name": "PG7", + "AlternateFunctions": { + "AF2": [ + "SAI1_CK2" + ], + "AF3": [ + "I3C1_SCL" + ], + "AF4": [ + "I2C4_SCL" + ], + "AF6": [ + "SAI1_MCLK_A" + ], + "AF7": [ + "USART6_CK" + ], + "AF11": [ + "UCPD1_FRSTX" + ], + "AF12": [ + "FMC_INT" + ], + "AF13": [ + "DCMI_D13", + "PSSI_D13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG8": { + "Name": "PG8", + "AlternateFunctions": { + "AF3": [ + "TIM8_ETR" + ], + "AF5": [ + "SPI6_NSS" + ], + "AF7": [ + "USART6_RTS", + "USART6_DE" + ], + "AF11": [ + "ETH_PPS_OUT" + ], + "AF12": [ + "FMC_SDCLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG9": { + "Name": "PG9", + "AlternateFunctions": { + "AF5": [ + "SPI1_MISO", + "I2S1_SDI" + ], + "AF7": [ + "USART6_RX" + ], + "AF9": [ + "OCTOSPI1_IO6" + ], + "AF10": [ + "SAI2_FS_B" + ], + "AF11": [ + "SDMMC2_D0" + ], + "AF12": [ + "FMC_NE2", + "FMC_NCE" + ], + "AF13": [ + "DCMI_VSYNC", + "PSSI_RDY" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG10": { + "Name": "PG10", + "AlternateFunctions": { + "AF5": [ + "SPI1_NSS", + "I2S1_WS" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF11": [ + "SDMMC2_D1" + ], + "AF12": [ + "FMC_NE3" + ], + "AF13": [ + "DCMI_D2", + "PSSI_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG11": { + "Name": "PG11", + "AlternateFunctions": { + "AF1": [ + "LPTIM1_IN2" + ], + "AF5": [ + "SPI1_SCK", + "I2S1_CK" + ], + "AF6": [ + "USART10_RX" + ], + "AF7": [ + "USART11_RTS", + "USART11_DE" + ], + "AF10": [ + "SDMMC2_D2" + ], + "AF11": [ + "ETH_MII_TX_EN", + "ETH_RMII_TX_EN" + ], + "AF13": [ + "DCMI_D3", + "PSSI_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG12": { + "Name": "PG12", + "AlternateFunctions": { + "AF1": [ + "LPTIM1_IN1" + ], + "AF4": [ + "PSSI_D15" + ], + "AF5": [ + "SPI6_MISO" + ], + "AF6": [ + "USART10_TX" + ], + "AF7": [ + "USART6_RTS", + "USART6_DE" + ], + "AF10": [ + "SDMMC2_D3" + ], + "AF11": [ + "ETH_MII_TXD1", + "ETH_RMII_TXD1" + ], + "AF12": [ + "FMC_NE4" + ], + "AF13": [ + "DCMI_D11", + "PSSI_D11" + ], + "AF14": [ + "LPTIM5_CH1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG13": { + "Name": "PG13", + "AlternateFunctions": { + "AF0": [ + "TRACED0" + ], + "AF1": [ + "LPTIM1_CH1" + ], + "AF5": [ + "SPI6_SCK" + ], + "AF6": [ + "USART10_CTS", + "USART10_NSS" + ], + "AF7": [ + "USART6_CTS", + "USART6_NSS" + ], + "AF10": [ + "SDMMC2_D6" + ], + "AF11": [ + "ETH_MII_TXD0", + "ETH_RMII_TXD0" + ], + "AF12": [ + "FMC_A24" + ], + "AF13": [ + "LPTIM5_CH2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG14": { + "Name": "PG14", + "AlternateFunctions": { + "AF0": [ + "TRACED1" + ], + "AF1": [ + "LPTIM1_ETR" + ], + "AF4": [ + "LPTIM1_CH2" + ], + "AF5": [ + "SPI6_MOSI" + ], + "AF6": [ + "USART10_RTS", + "USART10_DE" + ], + "AF7": [ + "USART6_TX" + ], + "AF9": [ + "OCTOSPI1_IO7" + ], + "AF10": [ + "SDMMC2_D7" + ], + "AF11": [ + "ETH_MII_TXD1", + "ETH_RMII_TXD1" + ], + "AF12": [ + "FMC_A25" + ], + "AF13": [ + "LPTIM5_IN1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG15": { + "Name": "PG15", + "AlternateFunctions": { + "AF5": [ + "SPI4_RDY" + ], + "AF6": [ + "USART10_CK" + ], + "AF7": [ + "USART6_CTS", + "USART6_NSS" + ], + "AF12": [ + "FMC_NCAS" + ], + "AF13": [ + "DCMI_D13", + "PSSI_D13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH0": { + "Name": "PH0", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH1": { + "Name": "PH1", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH2": { + "Name": "PH2", + "AlternateFunctions": { + "AF1": [ + "LPTIM1_IN2" + ], + "AF9": [ + "OCTOSPI1_IO4" + ], + "AF10": [ + "SAI2_SCK_B" + ], + "AF11": [ + "ETH_MII_CRS" + ], + "AF12": [ + "FMC_SDCKE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH3": { + "Name": "PH3", + "AlternateFunctions": { + "AF9": [ + "OCTOSPI1_IO5" + ], + "AF10": [ + "SAI2_MCLK_B" + ], + "AF11": [ + "ETH_MII_COL" + ], + "AF12": [ + "FMC_SDNE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH4": { + "Name": "PH4", + "AlternateFunctions": { + "AF4": [ + "I2C2_SCL" + ], + "AF5": [ + "SPI5_RDY" + ], + "AF7": [ + "SPI6_RDY" + ], + "AF13": [ + "PSSI_D14" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH5": { + "Name": "PH5", + "AlternateFunctions": { + "AF4": [ + "I2C2_SDA" + ], + "AF5": [ + "SPI5_NSS" + ], + "AF7": [ + "SPI6_RDY" + ], + "AF12": [ + "FMC_SDNWE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH6": { + "Name": "PH6", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3N" + ], + "AF2": [ + "TIM12_CH1" + ], + "AF3": [ + "TIM8_CH1" + ], + "AF4": [ + "I2C2_SMBA" + ], + "AF5": [ + "SPI5_SCK" + ], + "AF11": [ + "ETH_MII_RXD2" + ], + "AF12": [ + "FMC_SDNE1" + ], + "AF13": [ + "DCMI_D8", + "PSSI_D8" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH7": { + "Name": "PH7", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3" + ], + "AF3": [ + "TIM8_CH1N" + ], + "AF4": [ + "I2C3_SCL" + ], + "AF5": [ + "SPI5_MISO" + ], + "AF11": [ + "ETH_MII_RXD3" + ], + "AF12": [ + "FMC_SDCKE1" + ], + "AF13": [ + "DCMI_D9", + "PSSI_D9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH8": { + "Name": "PH8", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF2": [ + "TIM5_ETR" + ], + "AF3": [ + "TIM8_CH2" + ], + "AF4": [ + "I2C3_SDA" + ], + "AF5": [ + "SPI5_MOSI" + ], + "AF13": [ + "DCMI_HSYNC", + "PSSI_DE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH9": { + "Name": "PH9", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2" + ], + "AF2": [ + "TIM12_CH2" + ], + "AF3": [ + "TIM8_CH2N" + ], + "AF4": [ + "I2C3_SMBA" + ], + "AF5": [ + "SPI5_NSS" + ], + "AF13": [ + "DCMI_D0", + "PSSI_D0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH10": { + "Name": "PH10", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF2": [ + "TIM5_CH1" + ], + "AF3": [ + "TIM8_CH3" + ], + "AF4": [ + "I2C4_SMBA" + ], + "AF5": [ + "SPI5_RDY" + ], + "AF13": [ + "DCMI_D1", + "PSSI_D1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH11": { + "Name": "PH11", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1" + ], + "AF2": [ + "TIM5_CH2" + ], + "AF3": [ + "TIM8_CH3N" + ], + "AF4": [ + "I2C4_SCL" + ], + "AF5": [ + "I3C1_SCL" + ], + "AF13": [ + "DCMI_D2", + "PSSI_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH12": { + "Name": "PH12", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF2": [ + "TIM5_CH3" + ], + "AF3": [ + "TIM8_BKIN" + ], + "AF4": [ + "I2C4_SDA" + ], + "AF5": [ + "I3C1_SDA" + ], + "AF10": [ + "TIM8_CH4N" + ], + "AF13": [ + "DCMI_D3", + "PSSI_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH13": { + "Name": "PH13", + "AlternateFunctions": { + "AF1": [ + "LPTIM1_IN2" + ], + "AF3": [ + "TIM8_CH1N" + ], + "AF7": [ + "UART8_TX" + ], + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "FDCAN1_TX" + ], + "AF13": [ + "DCMI_D3", + "PSSI_D3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH14": { + "Name": "PH14", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH2N" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "FDCAN1_RX" + ], + "AF13": [ + "DCMI_D4", + "PSSI_D4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH15": { + "Name": "PH15", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH3N" + ], + "AF13": [ + "DCMI_D11", + "PSSI_D11" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI0": { + "Name": "PI0", + "AlternateFunctions": { + "AF13": [ + "DCMI_D13", + "PSSI_D13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI1": { + "Name": "PI1", + "AlternateFunctions": { + "AF13": [ + "DCMI_D8", + "PSSI_D8" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI2": { + "Name": "PI2", + "AlternateFunctions": { + "AF13": [ + "DCMI_D9", + "PSSI_D9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI3": { + "Name": "PI3", + "AlternateFunctions": { + "AF13": [ + "DCMI_D10", + "PSSI_D10" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI4": { + "Name": "PI4", + "AlternateFunctions": { + "AF10": [ + "SAI2_MCLK_A" + ], + "AF13": [ + "DCMI_D5", + "PSSI_D5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI5": { + "Name": "PI5", + "AlternateFunctions": { + "AF10": [ + "SAI2_SCK_A" + ], + "AF13": [ + "DCMI_VSYNC", + "PSSI_RDY" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI6": { + "Name": "PI6", + "AlternateFunctions": { + "AF10": [ + "SAI2_SD_A" + ], + "AF13": [ + "DCMI_D6", + "PSSI_D6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI7": { + "Name": "PI7", + "AlternateFunctions": { + "AF10": [ + "SAI2_FS_A" + ], + "AF13": [ + "DCMI_D7", + "PSSI_D7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI8": { + "Name": "PI8", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI9": { + "Name": "PI9", + "AlternateFunctions": { + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "FDCAN1_RX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI10": { + "Name": "PI10", + "AlternateFunctions": { + "AF9": [ + "FDCAN1_RX" + ], + "AF11": [ + "ETH_MII_RX_ER" + ], + "AF13": [ + "PSSI_D14" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI11": { + "Name": "PI11", + "AlternateFunctions": { + "AF13": [ + "PSSI_D15" + ], + "AF15": [ + "EVENTOUT" + ] + } + } + } + }, + { + "Name": "STM32H7A3", + "Pins": { + "PA0": { + "Name": "PA0", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH1", + "TIM2_ETR" + ], + "AF2": [ + "TIM5_CH1" + ], + "AF3": [ + "TIM8_ETR" + ], + "AF4": [ + "TIM15_BKIN" + ], + "AF5": [ + "SPI6_SS", + "I2S6_WS" + ], + "AF7": [ + "USART2_CTS", + "USART2_NSS" + ], + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "SDMMC2_CMD" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA1": { + "Name": "PA1", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH2" + ], + "AF2": [ + "TIM5_CH2" + ], + "AF3": [ + "LPTIM3_OUT" + ], + "AF4": [ + "TIM15_CH1N" + ], + "AF7": [ + "USART2_RTS" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "OCTOSPIM_P1_IO3" + ], + "AF10": [ + "SAI2_MCK_B" + ], + "AF11": [ + "OCTOSPIM_P1_DQS" + ], + "AF14": [ + "LCD_R2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA2": { + "Name": "PA2", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH3" + ], + "AF2": [ + "TIM5_CH3" + ], + "AF4": [ + "TIM15_CH1" + ], + "AF6": [ + "DFSDM2_CKIN1" + ], + "AF7": [ + "USART2_TX" + ], + "AF8": [ + "SAI2_SCK_B" + ], + "AF12": [ + "MDIOS_MDIO" + ], + "AF14": [ + "LCD_R1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA3": { + "Name": "PA3", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH4" + ], + "AF2": [ + "TIM5_CH4" + ], + "AF3": [ + "OCTOSPIM_P1_CLK" + ], + "AF4": [ + "TIM15_CH2" + ], + "AF5": [ + "I2S6_MCK" + ], + "AF7": [ + "USART2_RX" + ], + "AF9": [ + "LCD_B2" + ], + "AF10": [ + "OTG_HS_ULPI_D0" + ], + "AF14": [ + "LCD_B5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA4": { + "Name": "PA4", + "AlternateFunctions": { + "AF2": [ + "TIM5_ETR" + ], + "AF5": [ + "SPI1_SS", + "I2S1_WS" + ], + "AF6": [ + "SPI3_SS", + "I2S3_WS" + ], + "AF7": [ + "USART2_CK" + ], + "AF8": [ + "SPI6_SS", + "I2S6_WS" + ], + "AF13": [ + "DCMI_HSYNC", + "PSSI_DE" + ], + "AF14": [ + "LCD_VSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA5": { + "Name": "PA5", + "AlternateFunctions": { + "AF0": [ + "PWR_NDSTOP2" + ], + "AF1": [ + "TIM2_CH1", + "TIM2_ETR" + ], + "AF3": [ + "TIM8_CH1N" + ], + "AF5": [ + "SPI1_SCK", + "I2S1_CK" + ], + "AF8": [ + "SPI6_SCK", + "I2S6_CK" + ], + "AF10": [ + "OTG_HS_ULPI_CK" + ], + "AF13": [ + "PSSI_D14" + ], + "AF14": [ + "LCD_R4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA6": { + "Name": "PA6", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF2": [ + "TIM3_CH1" + ], + "AF3": [ + "TIM8_BKIN" + ], + "AF5": [ + "SPI1_MISO", + "I2S1_SDI" + ], + "AF6": [ + "OCTOSPIM_P1_IO3" + ], + "AF8": [ + "SPI6_MISO", + "I2S6_SDI" + ], + "AF9": [ + "TIM13_CH1" + ], + "AF10": [ + "TIM8_BKIN_COMP12" + ], + "AF11": [ + "MDIOS_MDC" + ], + "AF12": [ + "TIM1_BKIN_COMP12" + ], + "AF13": [ + "DCMI_PIXCLK", + "PSSI_PDCK" + ], + "AF14": [ + "LCD_G2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA7": { + "Name": "PA7", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF2": [ + "TIM3_CH2" + ], + "AF3": [ + "TIM8_CH1N" + ], + "AF4": [ + "DFSDM2_DATIN1" + ], + "AF5": [ + "SPI1_MOSI", + "I2S1_SDO" + ], + "AF8": [ + "SPI6_MOSI", + "I2S6_SDO" + ], + "AF9": [ + "TIM14_CH1" + ], + "AF10": [ + "OCTOSPIM_P1_IO2" + ], + "AF12": [ + "FMC_SDNWE" + ], + "AF14": [ + "LCD_VSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA8": { + "Name": "PA8", + "AlternateFunctions": { + "AF0": [ + "MCO1" + ], + "AF1": [ + "TIM1_CH1" + ], + "AF3": [ + "TIM8_BKIN2" + ], + "AF4": [ + "I2C3_SCL" + ], + "AF7": [ + "USART1_CK" + ], + "AF10": [ + "OTG_HS_SOF" + ], + "AF11": [ + "UART7_RX" + ], + "AF12": [ + "TIM8_BKIN2_COMP12" + ], + "AF13": [ + "LCD_B3" + ], + "AF14": [ + "LCD_R6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA9": { + "Name": "PA9", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2" + ], + "AF3": [ + "LPUART1_TX" + ], + "AF4": [ + "I2C3_SMBA" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF7": [ + "USART1_TX" + ], + "AF13": [ + "DCMI_D0", + "PSSI_D0" + ], + "AF14": [ + "LCD_R5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA10": { + "Name": "PA10", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3" + ], + "AF3": [ + "LPUART1_RX" + ], + "AF7": [ + "USART1_RX" + ], + "AF10": [ + "OTG_HS_ID" + ], + "AF11": [ + "MDIOS_MDIO" + ], + "AF12": [ + "LCD_B4" + ], + "AF13": [ + "DCMI_D1", + "PSSI_D1" + ], + "AF14": [ + "LCD_B1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA11": { + "Name": "PA11", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH4" + ], + "AF3": [ + "LPUART1_CTS" + ], + "AF5": [ + "SPI2_SS", + "I2S2_WS" + ], + "AF6": [ + "UART4_RX" + ], + "AF7": [ + "USART1_CTS", + "USART1_NSS" + ], + "AF9": [ + "FDCAN1_RX" + ], + "AF14": [ + "LCD_R4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA12": { + "Name": "PA12", + "AlternateFunctions": { + "AF1": [ + "TIM1_ETR" + ], + "AF3": [ + "LPUART1_RTS" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF6": [ + "UART4_TX" + ], + "AF7": [ + "USART1_RTS" + ], + "AF8": [ + "SAI2_FS_B" + ], + "AF9": [ + "FDCAN1_TX" + ], + "AF14": [ + "LCD_R5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA13": { + "Name": "PA13", + "AlternateFunctions": { + "AF0": [ + "JTMS", + "SWDIO" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA14": { + "Name": "PA14", + "AlternateFunctions": { + "AF0": [ + "JTCK", + "SWCLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PA15": { + "Name": "PA15", + "AlternateFunctions": { + "AF0": [ + "JTDI" + ], + "AF1": [ + "TIM2_CH1", + "TIM2_ETR" + ], + "AF4": [ + "HDMI_CEC" + ], + "AF5": [ + "SPI1_SS", + "I2S1_WS" + ], + "AF6": [ + "SPI3_SS", + "I2S3_WS" + ], + "AF7": [ + "SPI6_SS", + "I2S6_WS" + ], + "AF8": [ + "UART4_RTS" + ], + "AF9": [ + "LCD_R3" + ], + "AF11": [ + "UART7_TX" + ], + "AF14": [ + "LCD_B6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB0": { + "Name": "PB0", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF2": [ + "TIM3_CH3" + ], + "AF3": [ + "TIM8_CH2N" + ], + "AF4": [ + "DFSDM2_CKOUT" + ], + "AF6": [ + "DFSDM1_CKOUT" + ], + "AF8": [ + "UART4_CTS" + ], + "AF9": [ + "LCD_R3" + ], + "AF10": [ + "OTG_HS_ULPI_D1" + ], + "AF11": [ + "OCTOSPIM_P1_IO1" + ], + "AF14": [ + "LCD_G1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB1": { + "Name": "PB1", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3N" + ], + "AF2": [ + "TIM3_CH4" + ], + "AF3": [ + "TIM8_CH3N" + ], + "AF6": [ + "DFSDM1_DATIN1" + ], + "AF9": [ + "LCD_R6" + ], + "AF10": [ + "OTG_HS_ULPI_D2" + ], + "AF11": [ + "OCTOSPIM_P1_IO0" + ], + "AF14": [ + "LCD_G0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB2": { + "Name": "PB2", + "AlternateFunctions": { + "AF0": [ + "RTC_OUT2" + ], + "AF2": [ + "SAI1_D1" + ], + "AF4": [ + "DFSDM1_CKIN1" + ], + "AF6": [ + "SAI1_SD_A" + ], + "AF7": [ + "SPI3_MOSI", + "I2S3_SDO" + ], + "AF9": [ + "OCTOSPIM_P1_CLK" + ], + "AF10": [ + "OCTOSPIM_P1_DQS" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB3": { + "Name": "PB3", + "AlternateFunctions": { + "AF0": [ + "JTDO", + "TRACESWO" + ], + "AF1": [ + "TIM2_CH2" + ], + "AF5": [ + "SPI1_SCK", + "I2S1_CK" + ], + "AF6": [ + "SPI3_SCK", + "I2S3_CK" + ], + "AF8": [ + "SPI6_SCK", + "I2S6_CK" + ], + "AF9": [ + "SDMMC2_D2" + ], + "AF10": [ + "CRS_SYNC" + ], + "AF11": [ + "UART7_RX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB4": { + "Name": "PB4", + "AlternateFunctions": { + "AF0": [ + "NJTRST" + ], + "AF1": [ + "TIM16_BKIN" + ], + "AF2": [ + "TIM3_CH1" + ], + "AF5": [ + "SPI1_MISO", + "I2S1_SDI" + ], + "AF6": [ + "SPI3_MISO", + "I2S3_SDI" + ], + "AF7": [ + "SPI2_SS", + "I2S2_WS" + ], + "AF8": [ + "SPI6_MISO", + "I2S6_SDI" + ], + "AF9": [ + "SDMMC2_D3" + ], + "AF11": [ + "UART7_TX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB5": { + "Name": "PB5", + "AlternateFunctions": { + "AF1": [ + "TIM17_BKIN" + ], + "AF2": [ + "TIM3_CH2" + ], + "AF4": [ + "I2C1_SMBA" + ], + "AF5": [ + "SPI1_MOSI", + "I2S1_SDO" + ], + "AF6": [ + "I2C4_SMBA" + ], + "AF7": [ + "SPI3_MOSI", + "I2S3_SDO" + ], + "AF8": [ + "SPI6_MOSI", + "I2S6_SDO" + ], + "AF9": [ + "FDCAN2_RX" + ], + "AF10": [ + "OTG_HS_ULPI_D7" + ], + "AF11": [ + "LCD_B5" + ], + "AF12": [ + "FMC_SDCKE1" + ], + "AF13": [ + "DCMI_D10", + "PSSI_D10" + ], + "AF14": [ + "UART5_RX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB6": { + "Name": "PB6", + "AlternateFunctions": { + "AF1": [ + "TIM16_CH1N" + ], + "AF2": [ + "TIM4_CH1" + ], + "AF4": [ + "I2C1_SCL" + ], + "AF5": [ + "HDMI_CEC" + ], + "AF6": [ + "I2C4_SCL" + ], + "AF7": [ + "USART1_TX" + ], + "AF8": [ + "LPUART1_TX" + ], + "AF9": [ + "FDCAN2_TX" + ], + "AF10": [ + "OCTOSPIM_P1_NCS" + ], + "AF11": [ + "DFSDM1_DATIN5" + ], + "AF12": [ + "FMC_SDNE1" + ], + "AF13": [ + "DCMI_D5", + "PSSI_D5" + ], + "AF14": [ + "UART5_TX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB7": { + "Name": "PB7", + "AlternateFunctions": { + "AF1": [ + "TIM17_CH1N" + ], + "AF2": [ + "TIM4_CH2" + ], + "AF4": [ + "I2C1_SDA" + ], + "AF6": [ + "I2C4_SDA" + ], + "AF7": [ + "USART1_RX" + ], + "AF8": [ + "LPUART1_RX" + ], + "AF11": [ + "DFSDM1_CKIN5" + ], + "AF12": [ + "FMC_NL" + ], + "AF13": [ + "DCMI_VSYNC", + "PSSI_RDY" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB8": { + "Name": "PB8", + "AlternateFunctions": { + "AF1": [ + "TIM16_CH1" + ], + "AF2": [ + "TIM4_CH3" + ], + "AF3": [ + "DFSDM1_CKIN7" + ], + "AF4": [ + "I2C1_SCL" + ], + "AF6": [ + "I2C4_SCL" + ], + "AF7": [ + "SDMMC1_CKIN" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "FDCAN1_RX" + ], + "AF10": [ + "SDMMC2_D4" + ], + "AF12": [ + "SDMMC1_D4" + ], + "AF13": [ + "DCMI_D6", + "PSSI_D6" + ], + "AF14": [ + "LCD_B6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB9": { + "Name": "PB9", + "AlternateFunctions": { + "AF1": [ + "TIM17_CH1" + ], + "AF2": [ + "TIM4_CH4" + ], + "AF3": [ + "DFSDM1_DATIN7" + ], + "AF4": [ + "I2C1_SDA" + ], + "AF5": [ + "SPI2_SS", + "I2S2_WS" + ], + "AF6": [ + "I2C4_SDA" + ], + "AF7": [ + "SDMMC1_CDIR" + ], + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "FDCAN1_TX" + ], + "AF10": [ + "SDMMC2_D5" + ], + "AF11": [ + "I2C4_SMBA" + ], + "AF12": [ + "SDMMC1_D5" + ], + "AF13": [ + "DCMI_D7", + "PSSI_D7" + ], + "AF14": [ + "LCD_B7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB10": { + "Name": "PB10", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH3" + ], + "AF3": [ + "LPTIM2_IN1" + ], + "AF4": [ + "I2C2_SCL" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF6": [ + "DFSDM1_DATIN7" + ], + "AF7": [ + "USART3_TX" + ], + "AF9": [ + "OCTOSPIM_P1_NCS" + ], + "AF10": [ + "OTG_HS_ULPI_D3" + ], + "AF14": [ + "LCD_G4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB11": { + "Name": "PB11", + "AlternateFunctions": { + "AF1": [ + "TIM2_CH4" + ], + "AF3": [ + "LPTIM2_ETR" + ], + "AF4": [ + "I2C2_SDA" + ], + "AF6": [ + "DFSDM1_CKIN7" + ], + "AF7": [ + "USART3_RX" + ], + "AF10": [ + "OTG_HS_ULPI_D4" + ], + "AF14": [ + "LCD_G5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB12": { + "Name": "PB12", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF3": [ + "OCTOSPIM_P1_NCLK" + ], + "AF4": [ + "I2C2_SMBA" + ], + "AF5": [ + "SPI2_SS", + "I2S2_WS" + ], + "AF6": [ + "DFSDM1_DATIN1" + ], + "AF7": [ + "USART3_CK" + ], + "AF9": [ + "FDCAN2_RX" + ], + "AF10": [ + "OTG_HS_ULPI_D5" + ], + "AF11": [ + "DFSDM2_DATIN1" + ], + "AF13": [ + "TIM1_BKIN_COMP12" + ], + "AF14": [ + "UART5_RX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB13": { + "Name": "PB13", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF3": [ + "LPTIM2_OUT" + ], + "AF4": [ + "DFSDM2_CKIN1" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF6": [ + "DFSDM1_CKIN1" + ], + "AF7": [ + "USART3_CTS", + "USART3_NSS" + ], + "AF9": [ + "FDCAN2_TX" + ], + "AF10": [ + "OTG_HS_ULPI_D6" + ], + "AF12": [ + "SDMMC1_D0" + ], + "AF13": [ + "DCMI_D2", + "PSSI_D2" + ], + "AF14": [ + "UART5_TX" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB14": { + "Name": "PB14", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF2": [ + "TIM12_CH1" + ], + "AF3": [ + "TIM8_CH2N" + ], + "AF4": [ + "USART1_TX" + ], + "AF5": [ + "SPI2_MISO", + "I2S2_SDI" + ], + "AF6": [ + "DFSDM1_DATIN2" + ], + "AF7": [ + "USART3_RTS" + ], + "AF8": [ + "UART4_RTS" + ], + "AF9": [ + "SDMMC2_D0" + ], + "AF14": [ + "LCD_CLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PB15": { + "Name": "PB15", + "AlternateFunctions": { + "AF0": [ + "RTC_REFIN" + ], + "AF1": [ + "TIM1_CH3N" + ], + "AF2": [ + "TIM12_CH2" + ], + "AF3": [ + "TIM8_CH3N" + ], + "AF4": [ + "USART1_RX" + ], + "AF5": [ + "SPI2_MOSI", + "I2S2_SDO" + ], + "AF6": [ + "DFSDM1_CKIN2" + ], + "AF8": [ + "UART4_CTS" + ], + "AF9": [ + "SDMMC2_D1" + ], + "AF14": [ + "LCD_G7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC0": { + "Name": "PC0", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN0" + ], + "AF6": [ + "DFSDM1_DATIN4" + ], + "AF8": [ + "SAI2_FS_B" + ], + "AF9": [ + "FMC_A25" + ], + "AF10": [ + "OTG_HS_ULPI_STP" + ], + "AF11": [ + "LCD_G2" + ], + "AF12": [ + "FMC_SDNWE" + ], + "AF14": [ + "LCD_R5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC1": { + "Name": "PC1", + "AlternateFunctions": { + "AF0": [ + "TRACED0" + ], + "AF2": [ + "SAI1_D1" + ], + "AF3": [ + "DFSDM1_DATIN0" + ], + "AF4": [ + "DFSDM1_CKIN4" + ], + "AF5": [ + "SPI2_MOSI", + "I2S2_SDO" + ], + "AF6": [ + "SAI1_SD_A" + ], + "AF9": [ + "SDMMC2_CK" + ], + "AF10": [ + "OCTOSPIM_P1_IO4" + ], + "AF12": [ + "MDIOS_MDC" + ], + "AF14": [ + "LCD_G5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC2": { + "Name": "PC2", + "AlternateFunctions": { + "AF0": [ + "PWR_CSTOP" + ], + "AF3": [ + "DFSDM1_CKIN1" + ], + "AF5": [ + "SPI2_MISO", + "I2S2_SDI" + ], + "AF6": [ + "DFSDM1_CKOUT" + ], + "AF9": [ + "OCTOSPIM_P1_IO2" + ], + "AF10": [ + "OTG_HS_ULPI_DIR" + ], + "AF11": [ + "OCTOSPIM_P1_IO5" + ], + "AF12": [ + "FMC_SDNE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC3": { + "Name": "PC3", + "AlternateFunctions": { + "AF0": [ + "PWR_CSLEEP" + ], + "AF3": [ + "DFSDM1_DATIN1" + ], + "AF5": [ + "SPI2_MOSI", + "I2S2_SDO" + ], + "AF9": [ + "OCTOSPIM_P1_IO0" + ], + "AF10": [ + "OTG_HS_ULPI_NXT" + ], + "AF11": [ + "OCTOSPIM_P1_IO6" + ], + "AF12": [ + "FMC_SDCKE0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC4": { + "Name": "PC4", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN2" + ], + "AF5": [ + "I2S1_MCK" + ], + "AF9": [ + "SPDIFRX1_IN2" + ], + "AF12": [ + "FMC_SDNE0" + ], + "AF14": [ + "LCD_R7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC5": { + "Name": "PC5", + "AlternateFunctions": { + "AF2": [ + "SAI1_D3" + ], + "AF3": [ + "DFSDM1_DATIN2" + ], + "AF4": [ + "PSSI_D15" + ], + "AF9": [ + "SPDIFRX1_IN3" + ], + "AF10": [ + "OCTOSPIM_P1_DQS" + ], + "AF12": [ + "FMC_SDCKE0" + ], + "AF13": [ + "COMP1_OUT" + ], + "AF14": [ + "LCD_DE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC6": { + "Name": "PC6", + "AlternateFunctions": { + "AF2": [ + "TIM3_CH1" + ], + "AF3": [ + "TIM8_CH1" + ], + "AF4": [ + "DFSDM1_CKIN3" + ], + "AF5": [ + "I2S2_MCK" + ], + "AF7": [ + "USART6_TX" + ], + "AF8": [ + "SDMMC1_D0DIR" + ], + "AF9": [ + "FMC_NWAIT" + ], + "AF10": [ + "SDMMC2_D6" + ], + "AF12": [ + "SDMMC1_D6" + ], + "AF13": [ + "DCMI_D0", + "PSSI_D0" + ], + "AF14": [ + "LCD_HSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC7": { + "Name": "PC7", + "AlternateFunctions": { + "AF0": [ + "TRGIO" + ], + "AF2": [ + "TIM3_CH2" + ], + "AF3": [ + "TIM8_CH2" + ], + "AF4": [ + "DFSDM1_DATIN3" + ], + "AF6": [ + "I2S3_MCK" + ], + "AF7": [ + "USART6_RX" + ], + "AF8": [ + "SDMMC1_D123DIR" + ], + "AF9": [ + "FMC_NE1" + ], + "AF10": [ + "SDMMC2_D7" + ], + "AF11": [ + "SWPMI_TX" + ], + "AF12": [ + "SDMMC1_D7" + ], + "AF13": [ + "DCMI_D1", + "PSSI_D1" + ], + "AF14": [ + "LCD_G6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC8": { + "Name": "PC8", + "AlternateFunctions": { + "AF0": [ + "TRACED1" + ], + "AF2": [ + "TIM3_CH3" + ], + "AF3": [ + "TIM8_CH3" + ], + "AF7": [ + "USART6_CK" + ], + "AF8": [ + "UART5_RTS" + ], + "AF9": [ + "FMC_NE2", + "FMC_NCE" + ], + "AF10": [ + "FMC_INT" + ], + "AF11": [ + "SWPMI_RX" + ], + "AF12": [ + "SDMMC1_D0" + ], + "AF13": [ + "DCMI_D2", + "PSSI_D2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC9": { + "Name": "PC9", + "AlternateFunctions": { + "AF0": [ + "MCO2" + ], + "AF2": [ + "TIM3_CH4" + ], + "AF3": [ + "TIM8_CH4" + ], + "AF4": [ + "I2C3_SDA" + ], + "AF5": [ + "I2S_CKIN" + ], + "AF8": [ + "UART5_CTS" + ], + "AF9": [ + "OCTOSPIM_P1_IO0" + ], + "AF10": [ + "LCD_G3" + ], + "AF11": [ + "SWPMI_SUSPEND" + ], + "AF12": [ + "SDMMC1_D1" + ], + "AF13": [ + "DCMI_D3", + "PSSI_D3" + ], + "AF14": [ + "LCD_B2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC10": { + "Name": "PC10", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN5" + ], + "AF4": [ + "DFSDM2_CKIN0" + ], + "AF6": [ + "SPI3_SCK", + "I2S3_CK" + ], + "AF7": [ + "USART3_TX" + ], + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "OCTOSPIM_P1_IO1" + ], + "AF10": [ + "LCD_B1" + ], + "AF11": [ + "SWPMI_RX" + ], + "AF12": [ + "SDMMC1_D2" + ], + "AF13": [ + "DCMI_D8", + "PSSI_D8" + ], + "AF14": [ + "LCD_R2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC11": { + "Name": "PC11", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_DATIN5" + ], + "AF4": [ + "DFSDM2_DATIN0" + ], + "AF6": [ + "SPI3_MISO", + "I2S3_SDI" + ], + "AF7": [ + "USART3_RX" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "OCTOSPIM_P1_NCS" + ], + "AF12": [ + "SDMMC1_D3" + ], + "AF13": [ + "DCMI_D4", + "PSSI_D4" + ], + "AF14": [ + "LCD_B4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC12": { + "Name": "PC12", + "AlternateFunctions": { + "AF0": [ + "TRACED3" + ], + "AF2": [ + "TIM15_CH1" + ], + "AF4": [ + "DFSDM2_CKOUT" + ], + "AF5": [ + "SPI6_SCK", + "I2S6_CK" + ], + "AF6": [ + "SPI3_MOSI", + "I2S3_SDO" + ], + "AF7": [ + "USART3_CK" + ], + "AF8": [ + "UART5_TX" + ], + "AF12": [ + "SDMMC1_CK" + ], + "AF13": [ + "DCMI_D9", + "PSSI_D9" + ], + "AF14": [ + "LCD_R6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC13": { + "Name": "PC13", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC14": { + "Name": "PC14", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PC15": { + "Name": "PC15", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD0": { + "Name": "PD0", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN6" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "FDCAN1_RX" + ], + "AF11": [ + "UART9_CTS" + ], + "AF12": [ + "FMC_D2", + "FMC_DA2" + ], + "AF14": [ + "LCD_B1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD1": { + "Name": "PD1", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_DATIN6" + ], + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "FDCAN1_TX" + ], + "AF12": [ + "FMC_D3", + "FMC_DA3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD2": { + "Name": "PD2", + "AlternateFunctions": { + "AF0": [ + "TRACED2" + ], + "AF2": [ + "TIM3_ETR" + ], + "AF4": [ + "TIM15_BKIN" + ], + "AF8": [ + "UART5_RX" + ], + "AF9": [ + "LCD_B7" + ], + "AF12": [ + "SDMMC1_CMD" + ], + "AF13": [ + "DCMI_D11", + "PSSI_D11" + ], + "AF14": [ + "LCD_B2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD3": { + "Name": "PD3", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKOUT" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF7": [ + "USART2_CTS", + "USART2_NSS" + ], + "AF12": [ + "FMC_CLK" + ], + "AF13": [ + "DCMI_D5", + "PSSI_D5" + ], + "AF14": [ + "LCD_G7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD4": { + "Name": "PD4", + "AlternateFunctions": { + "AF7": [ + "USART2_RTS" + ], + "AF10": [ + "OCTOSPIM_P1_IO4" + ], + "AF12": [ + "FMC_NOE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD5": { + "Name": "PD5", + "AlternateFunctions": { + "AF7": [ + "USART2_TX" + ], + "AF10": [ + "OCTOSPIM_P1_IO5" + ], + "AF12": [ + "FMC_NWE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD6": { + "Name": "PD6", + "AlternateFunctions": { + "AF2": [ + "SAI1_D1" + ], + "AF3": [ + "DFSDM1_CKIN4" + ], + "AF4": [ + "DFSDM1_DATIN1" + ], + "AF5": [ + "SPI3_MOSI", + "I2S3_SDO" + ], + "AF6": [ + "SAI1_SD_A" + ], + "AF7": [ + "USART2_RX" + ], + "AF10": [ + "OCTOSPIM_P1_IO6" + ], + "AF11": [ + "SDMMC2_CK" + ], + "AF12": [ + "FMC_NWAIT" + ], + "AF13": [ + "DCMI_D10", + "PSSI_D10" + ], + "AF14": [ + "LCD_B2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD7": { + "Name": "PD7", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_DATIN4" + ], + "AF5": [ + "SPI1_MOSI", + "I2S1_SDO" + ], + "AF6": [ + "DFSDM1_CKIN1" + ], + "AF7": [ + "USART2_CK" + ], + "AF9": [ + "SPDIFRX1_IN0" + ], + "AF10": [ + "OCTOSPIM_P1_IO7" + ], + "AF11": [ + "SDMMC2_CMD" + ], + "AF12": [ + "FMC_NE1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD8": { + "Name": "PD8", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN3" + ], + "AF7": [ + "USART3_TX" + ], + "AF9": [ + "SPDIFRX1_IN1" + ], + "AF12": [ + "FMC_D13", + "FMC_DA13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD9": { + "Name": "PD9", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_DATIN3" + ], + "AF7": [ + "USART3_RX" + ], + "AF12": [ + "FMC_D14", + "FMC_DA14" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD10": { + "Name": "PD10", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKOUT" + ], + "AF4": [ + "DFSDM2_CKOUT" + ], + "AF7": [ + "USART3_CK" + ], + "AF12": [ + "FMC_D15", + "FMC_DA15" + ], + "AF14": [ + "LCD_B3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD11": { + "Name": "PD11", + "AlternateFunctions": { + "AF3": [ + "LPTIM2_IN2" + ], + "AF4": [ + "I2C4_SMBA" + ], + "AF7": [ + "USART3_CTS", + "USART3_NSS" + ], + "AF9": [ + "OCTOSPIM_P1_IO0" + ], + "AF10": [ + "SAI2_SD_A" + ], + "AF12": [ + "FMC_A16", + "FMC_CLE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD12": { + "Name": "PD12", + "AlternateFunctions": { + "AF1": [ + "LPTIM1_IN1" + ], + "AF2": [ + "TIM4_CH1" + ], + "AF3": [ + "LPTIM2_IN1" + ], + "AF4": [ + "I2C4_SCL" + ], + "AF7": [ + "USART3_RTS" + ], + "AF9": [ + "OCTOSPIM_P1_IO1" + ], + "AF10": [ + "SAI2_FS_A" + ], + "AF12": [ + "FMC_A17", + "FMC_ALE" + ], + "AF13": [ + "DCMI_D12", + "PSSI_D12" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD13": { + "Name": "PD13", + "AlternateFunctions": { + "AF1": [ + "LPTIM1_OUT" + ], + "AF2": [ + "TIM4_CH2" + ], + "AF4": [ + "I2C4_SDA" + ], + "AF9": [ + "OCTOSPIM_P1_IO3" + ], + "AF10": [ + "SAI2_SCK_A" + ], + "AF11": [ + "UART9_RTS" + ], + "AF12": [ + "FMC_A18" + ], + "AF13": [ + "DCMI_D13", + "PSSI_D13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD14": { + "Name": "PD14", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH3" + ], + "AF8": [ + "UART8_CTS" + ], + "AF11": [ + "UART9_RX" + ], + "AF12": [ + "FMC_D0", + "FMC_DA0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PD15": { + "Name": "PD15", + "AlternateFunctions": { + "AF2": [ + "TIM4_CH4" + ], + "AF8": [ + "UART8_RTS" + ], + "AF11": [ + "UART9_TX" + ], + "AF12": [ + "FMC_D1", + "FMC_DA1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE0": { + "Name": "PE0", + "AlternateFunctions": { + "AF1": [ + "LPTIM1_ETR" + ], + "AF2": [ + "TIM4_ETR" + ], + "AF4": [ + "LPTIM2_ETR" + ], + "AF8": [ + "UART8_Rx" + ], + "AF10": [ + "SAI2_MCK_A" + ], + "AF12": [ + "FMC_NBL0" + ], + "AF13": [ + "DCMI_D2", + "PSSI_D2" + ], + "AF14": [ + "LCD_R0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE1": { + "Name": "PE1", + "AlternateFunctions": { + "AF1": [ + "LPTIM1_IN2" + ], + "AF8": [ + "UART8_Tx" + ], + "AF12": [ + "FMC_NBL1" + ], + "AF13": [ + "DCMI_D3", + "PSSI_D3" + ], + "AF14": [ + "LCD_R6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE2": { + "Name": "PE2", + "AlternateFunctions": { + "AF0": [ + "TRACECLK" + ], + "AF2": [ + "SAI1_CK1" + ], + "AF5": [ + "SPI4_SCK" + ], + "AF6": [ + "SAI1_MCLK_A" + ], + "AF9": [ + "OCTOSPIM_P1_IO2" + ], + "AF11": [ + "USART10_RX" + ], + "AF12": [ + "FMC_A23" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE3": { + "Name": "PE3", + "AlternateFunctions": { + "AF0": [ + "TRACED0" + ], + "AF4": [ + "TIM15_BKIN" + ], + "AF6": [ + "SAI1_SD_B" + ], + "AF11": [ + "USART10_TX" + ], + "AF12": [ + "FMC_A19" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE4": { + "Name": "PE4", + "AlternateFunctions": { + "AF0": [ + "TRACED1" + ], + "AF2": [ + "SAI1_D2" + ], + "AF3": [ + "DFSDM1_DATIN3" + ], + "AF4": [ + "TIM15_CH1N" + ], + "AF5": [ + "SPI4_SS" + ], + "AF6": [ + "SAI1_FS_A" + ], + "AF12": [ + "FMC_A20" + ], + "AF13": [ + "DCMI_D4", + "PSSI_D4" + ], + "AF14": [ + "LCD_B0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE5": { + "Name": "PE5", + "AlternateFunctions": { + "AF0": [ + "TRACED2" + ], + "AF2": [ + "SAI1_CK2" + ], + "AF3": [ + "DFSDM1_CKIN3" + ], + "AF4": [ + "TIM15_CH1" + ], + "AF5": [ + "SPI4_MISO" + ], + "AF6": [ + "SAI1_SCK_A" + ], + "AF12": [ + "FMC_A21" + ], + "AF13": [ + "DCMI_D6", + "PSSI_D6" + ], + "AF14": [ + "LCD_G0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE6": { + "Name": "PE6", + "AlternateFunctions": { + "AF0": [ + "TRACED3" + ], + "AF1": [ + "TIM1_BKIN2" + ], + "AF2": [ + "SAI1_D1" + ], + "AF4": [ + "TIM15_CH2" + ], + "AF5": [ + "SPI4_MOSI" + ], + "AF6": [ + "SAI1_SD_A" + ], + "AF10": [ + "SAI2_MCK_B" + ], + "AF11": [ + "TIM1_BKIN2_COMP12" + ], + "AF12": [ + "FMC_A22" + ], + "AF13": [ + "DCMI_D7", + "PSSI_D7" + ], + "AF14": [ + "LCD_G1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE7": { + "Name": "PE7", + "AlternateFunctions": { + "AF1": [ + "TIM1_ETR" + ], + "AF3": [ + "DFSDM1_DATIN2" + ], + "AF7": [ + "UART7_RX" + ], + "AF10": [ + "OCTOSPIM_P1_IO4" + ], + "AF12": [ + "FMC_D4", + "FMC_DA4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE8": { + "Name": "PE8", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF3": [ + "DFSDM1_CKIN2" + ], + "AF7": [ + "UART7_TX" + ], + "AF10": [ + "OCTOSPIM_P1_IO5" + ], + "AF12": [ + "FMC_D5", + "FMC_DA5" + ], + "AF13": [ + "COMP2_OUT" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE9": { + "Name": "PE9", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1" + ], + "AF3": [ + "DFSDM1_CKOUT" + ], + "AF7": [ + "UART7_RTS" + ], + "AF10": [ + "OCTOSPIM_P1_IO6" + ], + "AF12": [ + "FMC_D6", + "FMC_DA6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE10": { + "Name": "PE10", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF3": [ + "DFSDM1_DATIN4" + ], + "AF7": [ + "UART7_CTS" + ], + "AF10": [ + "OCTOSPIM_P1_IO7" + ], + "AF12": [ + "FMC_D7", + "FMC_DA7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE11": { + "Name": "PE11", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2" + ], + "AF3": [ + "DFSDM1_CKIN4" + ], + "AF5": [ + "SPI4_SS" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF11": [ + "OCTOSPIM_P1_NCS" + ], + "AF12": [ + "FMC_D8", + "FMC_DA8" + ], + "AF14": [ + "LCD_G3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE12": { + "Name": "PE12", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3N" + ], + "AF3": [ + "DFSDM1_DATIN5" + ], + "AF5": [ + "SPI4_SCK" + ], + "AF10": [ + "SAI2_SCK_B" + ], + "AF12": [ + "FMC_D9", + "FMC_DA9" + ], + "AF13": [ + "COMP1_OUT" + ], + "AF14": [ + "LCD_B4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE13": { + "Name": "PE13", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3" + ], + "AF3": [ + "DFSDM1_CKIN5" + ], + "AF5": [ + "SPI4_MISO" + ], + "AF10": [ + "SAI2_FS_B" + ], + "AF12": [ + "FMC_D10", + "FMC_DA10" + ], + "AF13": [ + "COMP2_OUT" + ], + "AF14": [ + "LCD_DE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE14": { + "Name": "PE14", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH4" + ], + "AF5": [ + "SPI4_MOSI" + ], + "AF10": [ + "SAI2_MCK_B" + ], + "AF12": [ + "FMC_D11", + "FMC_DA11" + ], + "AF14": [ + "LCD_CLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PE15": { + "Name": "PE15", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF11": [ + "USART10_CK" + ], + "AF12": [ + "FMC_D12", + "FMC_DA12" + ], + "AF13": [ + "TIM1_BKIN_COMP12" + ], + "AF14": [ + "LCD_R7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF0": { + "Name": "PF0", + "AlternateFunctions": { + "AF4": [ + "I2C2_SDA" + ], + "AF9": [ + "OCTOSPIM_P2_IO0" + ], + "AF12": [ + "FMC_A0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF1": { + "Name": "PF1", + "AlternateFunctions": { + "AF4": [ + "I2C2_SCL" + ], + "AF9": [ + "OCTOSPIM_P2_IO1" + ], + "AF12": [ + "FMC_A1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF2": { + "Name": "PF2", + "AlternateFunctions": { + "AF4": [ + "I2C2_SMBA" + ], + "AF9": [ + "OCTOSPIM_P2_IO2" + ], + "AF12": [ + "FMC_A2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF3": { + "Name": "PF3", + "AlternateFunctions": { + "AF9": [ + "OCTOSPIM_P2_IO3" + ], + "AF12": [ + "FMC_A3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF4": { + "Name": "PF4", + "AlternateFunctions": { + "AF9": [ + "OCTOSPIM_P2_CLK" + ], + "AF12": [ + "FMC_A4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF5": { + "Name": "PF5", + "AlternateFunctions": { + "AF9": [ + "OCTOSPIM_P2_NCLK" + ], + "AF12": [ + "FMC_A5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF6": { + "Name": "PF6", + "AlternateFunctions": { + "AF1": [ + "TIM16_CH1" + ], + "AF5": [ + "SPI5_SS" + ], + "AF6": [ + "SAI1_SD_B" + ], + "AF7": [ + "UART7_Rx" + ], + "AF10": [ + "OCTOSPIM_P1_IO3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF7": { + "Name": "PF7", + "AlternateFunctions": { + "AF1": [ + "TIM17_CH1" + ], + "AF5": [ + "SPI5_SCK" + ], + "AF6": [ + "SAI1_MCLK_B" + ], + "AF7": [ + "UART7_Tx" + ], + "AF10": [ + "OCTOSPIM_P1_IO2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF8": { + "Name": "PF8", + "AlternateFunctions": { + "AF1": [ + "TIM16_CH1N" + ], + "AF5": [ + "SPI5_MISO" + ], + "AF6": [ + "SAI1_SCK_B" + ], + "AF7": [ + "UART7_RTS" + ], + "AF9": [ + "TIM13_CH1" + ], + "AF10": [ + "OCTOSPIM_P1_IO0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF9": { + "Name": "PF9", + "AlternateFunctions": { + "AF1": [ + "TIM17_CH1N" + ], + "AF5": [ + "SPI5_MOSI" + ], + "AF6": [ + "SAI1_FS_B" + ], + "AF7": [ + "UART7_CTS" + ], + "AF9": [ + "TIM14_CH1" + ], + "AF10": [ + "OCTOSPIM_P1_IO1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF10": { + "Name": "PF10", + "AlternateFunctions": { + "AF1": [ + "TIM16_BKIN" + ], + "AF2": [ + "SAI1_D3" + ], + "AF4": [ + "PSSI_D15" + ], + "AF9": [ + "OCTOSPIM_P1_CLK" + ], + "AF13": [ + "DCMI_D11", + "PSSI_D11" + ], + "AF14": [ + "LCD_DE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF11": { + "Name": "PF11", + "AlternateFunctions": { + "AF5": [ + "SPI5_MOSI" + ], + "AF9": [ + "OCTOSPIM_P1_NCLK" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF12": [ + "FMC_SDNRAS" + ], + "AF13": [ + "DCMI_D12", + "PSSI_D12" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF12": { + "Name": "PF12", + "AlternateFunctions": { + "AF9": [ + "OCTOSPIM_P2_DQS" + ], + "AF12": [ + "FMC_A6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF13": { + "Name": "PF13", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_DATIN6" + ], + "AF4": [ + "I2C4_SMBA" + ], + "AF12": [ + "FMC_A7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF14": { + "Name": "PF14", + "AlternateFunctions": { + "AF3": [ + "DFSDM1_CKIN6" + ], + "AF4": [ + "I2C4_SCL" + ], + "AF12": [ + "FMC_A8" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PF15": { + "Name": "PF15", + "AlternateFunctions": { + "AF4": [ + "I2C4_SDA" + ], + "AF12": [ + "FMC_A9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG0": { + "Name": "PG0", + "AlternateFunctions": { + "AF9": [ + "OCTOSPIM_P2_IO4" + ], + "AF11": [ + "UART9_RX" + ], + "AF12": [ + "FMC_A10" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG1": { + "Name": "PG1", + "AlternateFunctions": { + "AF9": [ + "OCTOSPIM_P2_IO5" + ], + "AF11": [ + "UART9_TX" + ], + "AF12": [ + "FMC_A11" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG2": { + "Name": "PG2", + "AlternateFunctions": { + "AF3": [ + "TIM8_BKIN" + ], + "AF11": [ + "TIM8_BKIN_COMP12" + ], + "AF12": [ + "FMC_A12" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG3": { + "Name": "PG3", + "AlternateFunctions": { + "AF3": [ + "TIM8_BKIN2" + ], + "AF11": [ + "TIM8_BKIN2_COMP12" + ], + "AF12": [ + "FMC_A13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG4": { + "Name": "PG4", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN2" + ], + "AF11": [ + "TIM1_BKIN2_COMP12" + ], + "AF12": [ + "FMC_A14", + "FMC_BA0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG5": { + "Name": "PG5", + "AlternateFunctions": { + "AF1": [ + "TIM1_ETR" + ], + "AF12": [ + "FMC_A15", + "FMC_BA1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG6": { + "Name": "PG6", + "AlternateFunctions": { + "AF1": [ + "TIM17_BKIN" + ], + "AF10": [ + "OCTOSPIM_P1_NCS" + ], + "AF12": [ + "FMC_NE3" + ], + "AF13": [ + "DCMI_D12", + "PSSI_D12" + ], + "AF14": [ + "LCD_R7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG7": { + "Name": "PG7", + "AlternateFunctions": { + "AF6": [ + "SAI1_MCLK_A" + ], + "AF7": [ + "USART6_CK" + ], + "AF9": [ + "OCTOSPIM_P2_DQS" + ], + "AF12": [ + "FMC_INT" + ], + "AF13": [ + "DCMI_D13", + "PSSI_D13" + ], + "AF14": [ + "LCD_CLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG8": { + "Name": "PG8", + "AlternateFunctions": { + "AF3": [ + "TIM8_ETR" + ], + "AF5": [ + "SPI6_SS", + "I2S6_WS" + ], + "AF7": [ + "USART6_RTS" + ], + "AF8": [ + "SPDIFRX1_IN2" + ], + "AF12": [ + "FMC_SDCLK" + ], + "AF14": [ + "LCD_G7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG9": { + "Name": "PG9", + "AlternateFunctions": { + "AF5": [ + "SPI1_MISO", + "I2S1_SDI" + ], + "AF7": [ + "USART6_RX" + ], + "AF8": [ + "SPDIFRX1_IN3" + ], + "AF9": [ + "OCTOSPIM_P1_IO6" + ], + "AF10": [ + "SAI2_FS_B" + ], + "AF11": [ + "SDMMC2_D0" + ], + "AF12": [ + "FMC_NE2", + "FMC_NCE" + ], + "AF13": [ + "DCMI_VSYNC", + "PSSI_RDY" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG10": { + "Name": "PG10", + "AlternateFunctions": { + "AF3": [ + "OCTOSPIM_P2_IO6" + ], + "AF5": [ + "SPI1_SS", + "I2S1_WS" + ], + "AF9": [ + "LCD_G3" + ], + "AF10": [ + "SAI2_SD_B" + ], + "AF11": [ + "SDMMC2_D1" + ], + "AF12": [ + "FMC_NE3" + ], + "AF13": [ + "DCMI_D2", + "PSSI_D2" + ], + "AF14": [ + "LCD_B2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG11": { + "Name": "PG11", + "AlternateFunctions": { + "AF1": [ + "LPTIM1_IN2" + ], + "AF5": [ + "SPI1_SCK", + "I2S1_CK" + ], + "AF8": [ + "SPDIFRX1_IN0" + ], + "AF9": [ + "OCTOSPIM_P2_IO7" + ], + "AF10": [ + "SDMMC2_D2" + ], + "AF11": [ + "USART10_RX" + ], + "AF13": [ + "DCMI_D3", + "PSSI_D3" + ], + "AF14": [ + "LCD_B3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG12": { + "Name": "PG12", + "AlternateFunctions": { + "AF1": [ + "LPTIM1_IN1" + ], + "AF3": [ + "OCTOSPIM_P2_NCS" + ], + "AF5": [ + "SPI6_MISO", + "I2S6_SDI" + ], + "AF7": [ + "USART6_RTS" + ], + "AF8": [ + "SPDIFRX1_IN1" + ], + "AF9": [ + "LCD_B4" + ], + "AF10": [ + "SDMMC2_D3" + ], + "AF11": [ + "USART10_TX" + ], + "AF14": [ + "LCD_B1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG13": { + "Name": "PG13", + "AlternateFunctions": { + "AF0": [ + "TRACED0" + ], + "AF1": [ + "LPTIM1_OUT" + ], + "AF5": [ + "SPI6_SCK", + "I2S6_CK" + ], + "AF7": [ + "USART6_CTS", + "USART6_NSS" + ], + "AF10": [ + "SDMMC2_D6" + ], + "AF11": [ + "USART10_CTS", + "USART10_NSS" + ], + "AF14": [ + "LCD_R0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG14": { + "Name": "PG14", + "AlternateFunctions": { + "AF0": [ + "TRACED1" + ], + "AF1": [ + "LPTIM1_ETR" + ], + "AF5": [ + "SPI6_MOSI", + "I2S6_SDO" + ], + "AF7": [ + "USART6_TX" + ], + "AF9": [ + "OCTOSPIM_P1_IO7" + ], + "AF10": [ + "SDMMC2_D7" + ], + "AF11": [ + "USART10_RTS" + ], + "AF14": [ + "LCD_B0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PG15": { + "Name": "PG15", + "AlternateFunctions": { + "AF7": [ + "USART6_CTS", + "USART6_NSS" + ], + "AF9": [ + "OCTOSPIM_P2_DQS" + ], + "AF13": [ + "DCMI_D13", + "PSSI_D13" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH0": { + "Name": "PH0", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH1": { + "Name": "PH1", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH2": { + "Name": "PH2", + "AlternateFunctions": { + "AF1": [ + "LPTIM1_IN2" + ], + "AF9": [ + "OCTOSPIM_P1_IO4" + ], + "AF10": [ + "SAI2_SCK_B" + ], + "AF12": [ + "FMC_SDCKE0" + ], + "AF14": [ + "LCD_R0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH3": { + "Name": "PH3", + "AlternateFunctions": { + "AF9": [ + "OCTOSPIM_P1_IO5" + ], + "AF10": [ + "SAI2_MCK_B" + ], + "AF12": [ + "FMC_SDNE0" + ], + "AF14": [ + "LCD_R1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH4": { + "Name": "PH4", + "AlternateFunctions": { + "AF4": [ + "I2C2_SCL" + ], + "AF9": [ + "LCD_G5" + ], + "AF10": [ + "OTG_HS_ULPI_NXT" + ], + "AF13": [ + "PSSI_D14" + ], + "AF14": [ + "LCD_G4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH5": { + "Name": "PH5", + "AlternateFunctions": { + "AF4": [ + "I2C2_SDA" + ], + "AF5": [ + "SPI5_SS" + ], + "AF12": [ + "FMC_SDNWE" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH6": { + "Name": "PH6", + "AlternateFunctions": { + "AF2": [ + "TIM12_CH1" + ], + "AF4": [ + "I2C2_SMBA" + ], + "AF5": [ + "SPI5_SCK" + ], + "AF12": [ + "FMC_SDNE1" + ], + "AF13": [ + "DCMI_D8", + "PSSI_D8" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH7": { + "Name": "PH7", + "AlternateFunctions": { + "AF4": [ + "I2C3_SCL" + ], + "AF5": [ + "SPI5_MISO" + ], + "AF12": [ + "FMC_SDCKE1" + ], + "AF13": [ + "DCMI_D9", + "PSSI_D9" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH8": { + "Name": "PH8", + "AlternateFunctions": { + "AF2": [ + "TIM5_ETR" + ], + "AF4": [ + "I2C3_SDA" + ], + "AF12": [ + "FMC_D16" + ], + "AF13": [ + "DCMI_HSYNC", + "PSSI_DE" + ], + "AF14": [ + "LCD_R2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH9": { + "Name": "PH9", + "AlternateFunctions": { + "AF2": [ + "TIM12_CH2" + ], + "AF4": [ + "I2C3_SMBA" + ], + "AF12": [ + "FMC_D17" + ], + "AF13": [ + "DCMI_D0", + "PSSI_D0" + ], + "AF14": [ + "LCD_R3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH10": { + "Name": "PH10", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH1" + ], + "AF4": [ + "I2C4_SMBA" + ], + "AF12": [ + "FMC_D18" + ], + "AF13": [ + "DCMI_D1", + "PSSI_D1" + ], + "AF14": [ + "LCD_R4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH11": { + "Name": "PH11", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH2" + ], + "AF4": [ + "I2C4_SCL" + ], + "AF12": [ + "FMC_D19" + ], + "AF13": [ + "DCMI_D2", + "PSSI_D2" + ], + "AF14": [ + "LCD_R5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH12": { + "Name": "PH12", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH3" + ], + "AF4": [ + "I2C4_SDA" + ], + "AF12": [ + "FMC_D20" + ], + "AF13": [ + "DCMI_D3", + "PSSI_D3" + ], + "AF14": [ + "LCD_R6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH13": { + "Name": "PH13", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH1N" + ], + "AF8": [ + "UART4_TX" + ], + "AF9": [ + "FDCAN1_TX" + ], + "AF12": [ + "FMC_D21" + ], + "AF14": [ + "LCD_G2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH14": { + "Name": "PH14", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH2N" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "FDCAN1_RX" + ], + "AF12": [ + "FMC_D22" + ], + "AF13": [ + "DCMI_D4", + "PSSI_D4" + ], + "AF14": [ + "LCD_G3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PH15": { + "Name": "PH15", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH3N" + ], + "AF12": [ + "FMC_D23" + ], + "AF13": [ + "DCMI_D11", + "PSSI_D11" + ], + "AF14": [ + "LCD_G4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI0": { + "Name": "PI0", + "AlternateFunctions": { + "AF2": [ + "TIM5_CH4" + ], + "AF5": [ + "SPI2_SS", + "I2S2_WS" + ], + "AF12": [ + "FMC_D24" + ], + "AF13": [ + "DCMI_D13", + "PSSI_D13" + ], + "AF14": [ + "LCD_G5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI1": { + "Name": "PI1", + "AlternateFunctions": { + "AF3": [ + "TIM8_BKIN2" + ], + "AF5": [ + "SPI2_SCK", + "I2S2_CK" + ], + "AF11": [ + "TIM8_BKIN2_COMP12" + ], + "AF12": [ + "FMC_D25" + ], + "AF13": [ + "DCMI_D8", + "PSSI_D8" + ], + "AF14": [ + "LCD_G6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI2": { + "Name": "PI2", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH4" + ], + "AF5": [ + "SPI2_MISO", + "I2S2_SDI" + ], + "AF12": [ + "FMC_D26" + ], + "AF13": [ + "DCMI_D9", + "PSSI_D9" + ], + "AF14": [ + "LCD_G7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI3": { + "Name": "PI3", + "AlternateFunctions": { + "AF3": [ + "TIM8_ETR" + ], + "AF5": [ + "SPI2_MOSI", + "I2S2_SDO" + ], + "AF12": [ + "FMC_D27" + ], + "AF13": [ + "DCMI_D10", + "PSSI_D10" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI4": { + "Name": "PI4", + "AlternateFunctions": { + "AF3": [ + "TIM8_BKIN" + ], + "AF10": [ + "SAI2_MCK_A" + ], + "AF11": [ + "TIM8_BKIN_COMP12" + ], + "AF12": [ + "FMC_NBL2" + ], + "AF13": [ + "DCMI_D5", + "PSSI_D5" + ], + "AF14": [ + "LCD_B4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI5": { + "Name": "PI5", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH1" + ], + "AF10": [ + "SAI2_SCK_A" + ], + "AF12": [ + "FMC_NBL3" + ], + "AF13": [ + "DCMI_VSYNC", + "PSSI_RDY" + ], + "AF14": [ + "LCD_B5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI6": { + "Name": "PI6", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH2" + ], + "AF10": [ + "SAI2_SD_A" + ], + "AF12": [ + "FMC_D28" + ], + "AF13": [ + "DCMI_D6", + "PSSI_D6" + ], + "AF14": [ + "LCD_B6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI7": { + "Name": "PI7", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH3" + ], + "AF10": [ + "SAI2_FS_A" + ], + "AF12": [ + "FMC_D29" + ], + "AF13": [ + "DCMI_D7", + "PSSI_D7" + ], + "AF14": [ + "LCD_B7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI8": { + "Name": "PI8", + "AlternateFunctions": { + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI9": { + "Name": "PI9", + "AlternateFunctions": { + "AF3": [ + "OCTOSPIM_P2_IO0" + ], + "AF8": [ + "UART4_RX" + ], + "AF9": [ + "FDCAN1_RX" + ], + "AF12": [ + "FMC_D30" + ], + "AF14": [ + "LCD_VSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI10": { + "Name": "PI10", + "AlternateFunctions": { + "AF3": [ + "OCTOSPIM_P2_IO1" + ], + "AF12": [ + "FMC_D31" + ], + "AF13": [ + "PSSI_D14" + ], + "AF14": [ + "LCD_HSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI11": { + "Name": "PI11", + "AlternateFunctions": { + "AF3": [ + "OCTOSPIM_P2_IO2" + ], + "AF9": [ + "LCD_G6" + ], + "AF10": [ + "OTG_HS_ULPI_DIR" + ], + "AF13": [ + "PSSI_D15" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI12": { + "Name": "PI12", + "AlternateFunctions": { + "AF3": [ + "OCTOSPIM_P2_IO3" + ], + "AF14": [ + "LCD_HSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI13": { + "Name": "PI13", + "AlternateFunctions": { + "AF3": [ + "OCTOSPIM_P2_CLK" + ], + "AF14": [ + "LCD_VSYNC" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI14": { + "Name": "PI14", + "AlternateFunctions": { + "AF3": [ + "OCTOSPIM_P2_NCLK" + ], + "AF14": [ + "LCD_CLK" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PI15": { + "Name": "PI15", + "AlternateFunctions": { + "AF9": [ + "LCD_G2" + ], + "AF14": [ + "LCD_R0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ0": { + "Name": "PJ0", + "AlternateFunctions": { + "AF9": [ + "LCD_R7" + ], + "AF14": [ + "LCD_R1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ1": { + "Name": "PJ1", + "AlternateFunctions": { + "AF3": [ + "OCTOSPIM_P2_IO4" + ], + "AF14": [ + "LCD_R2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ2": { + "Name": "PJ2", + "AlternateFunctions": { + "AF3": [ + "OCTOSPIM_P2_IO5" + ], + "AF14": [ + "LCD_R3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ3": { + "Name": "PJ3", + "AlternateFunctions": { + "AF11": [ + "UART9_RTS" + ], + "AF14": [ + "LCD_R4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ4": { + "Name": "PJ4", + "AlternateFunctions": { + "AF11": [ + "UART9_CTS" + ], + "AF14": [ + "LCD_R5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ5": { + "Name": "PJ5", + "AlternateFunctions": { + "AF14": [ + "LCD_R6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ6": { + "Name": "PJ6", + "AlternateFunctions": { + "AF3": [ + "TIM8_CH2" + ], + "AF14": [ + "LCD_R7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ7": { + "Name": "PJ7", + "AlternateFunctions": { + "AF0": [ + "TRGIN" + ], + "AF3": [ + "TIM8_CH2N" + ], + "AF14": [ + "LCD_G0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ8": { + "Name": "PJ8", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3N" + ], + "AF3": [ + "TIM8_CH1" + ], + "AF8": [ + "UART8_TX" + ], + "AF14": [ + "LCD_G1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ9": { + "Name": "PJ9", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH3" + ], + "AF3": [ + "TIM8_CH1N" + ], + "AF8": [ + "UART8_RX" + ], + "AF14": [ + "LCD_G2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ10": { + "Name": "PJ10", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2N" + ], + "AF3": [ + "TIM8_CH2" + ], + "AF5": [ + "SPI5_MOSI" + ], + "AF14": [ + "LCD_G3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ11": { + "Name": "PJ11", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH2" + ], + "AF3": [ + "TIM8_CH2N" + ], + "AF5": [ + "SPI5_MISO" + ], + "AF14": [ + "LCD_G4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ12": { + "Name": "PJ12", + "AlternateFunctions": { + "AF0": [ + "TRGOUT" + ], + "AF9": [ + "LCD_G3" + ], + "AF14": [ + "LCD_B0" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ13": { + "Name": "PJ13", + "AlternateFunctions": { + "AF9": [ + "LCD_B4" + ], + "AF14": [ + "LCD_B1" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ14": { + "Name": "PJ14", + "AlternateFunctions": { + "AF14": [ + "LCD_B2" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PJ15": { + "Name": "PJ15", + "AlternateFunctions": { + "AF14": [ + "LCD_B3" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK0": { + "Name": "PK0", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1N" + ], + "AF3": [ + "TIM8_CH3" + ], + "AF5": [ + "SPI5_SCK" + ], + "AF14": [ + "LCD_G5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK1": { + "Name": "PK1", + "AlternateFunctions": { + "AF1": [ + "TIM1_CH1" + ], + "AF3": [ + "TIM8_CH3N" + ], + "AF5": [ + "SPI5_SS" + ], + "AF14": [ + "LCD_G6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK2": { + "Name": "PK2", + "AlternateFunctions": { + "AF1": [ + "TIM1_BKIN" + ], + "AF3": [ + "TIM8_BKIN" + ], + "AF10": [ + "TIM8_BKIN_COMP12" + ], + "AF11": [ + "TIM1_BKIN_COMP12" + ], + "AF14": [ + "LCD_G7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK3": { + "Name": "PK3", + "AlternateFunctions": { + "AF3": [ + "OCTOSPIM_P2_IO6" + ], + "AF14": [ + "LCD_B4" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK4": { + "Name": "PK4", + "AlternateFunctions": { + "AF3": [ + "OCTOSPIM_P2_IO7" + ], + "AF14": [ + "LCD_B5" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK5": { + "Name": "PK5", + "AlternateFunctions": { + "AF3": [ + "OCTOSPIM_P2_NCS" + ], + "AF14": [ + "LCD_B6" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK6": { + "Name": "PK6", + "AlternateFunctions": { + "AF3": [ + "OCTOSPIM_P2_DQS" + ], + "AF14": [ + "LCD_B7" + ], + "AF15": [ + "EVENTOUT" + ] + } + }, + "PK7": { + "Name": "PK7", + "AlternateFunctions": { + "AF14": [ + "LCD_DE" + ], + "AF15": [ + "EVENTOUT" + ] + } + } + } + } + ] +} \ No newline at end of file