diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index 8199a99d68..71b49ea5c2 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -37,6 +37,7 @@ pipeline { def nuttx_builds_archive = [ target: [ "airmind_mindpx-v2_default", + "ark_can-flow_canbootloader", "ark_can-flow_default", "av_x-v1_default", "bitcraze_crazyflie_default", @@ -49,36 +50,32 @@ pipeline { "cubepilot_cubeorange_default", "cubepilot_cubeyellow_console", "cubepilot_cubeyellow_default", + "holybro_can-gps-v1_canbootloader", + "holybro_can-gps-v1_default", "holybro_durandal-v1_default", "holybro_kakutef7_default", "holybro_pix32v5_default", - "holybro_can-gps-v1", - "holybro_can-gps-v1_canbootloader", "intel_aerofc-v1_default", "modalai_fc-v1_default", "mro_ctrl-zero-f7_default", "mro_pixracerpro_default", "mro_x21-777_default", "mro_x21_default", + "nxp_fmuk66-e_default", "nxp_fmuk66-v3_default", "nxp_fmuk66-v3_rtps", "nxp_fmuk66-v3_socketcan", - "nxp_fmuk66-e_default", "nxp_fmurt1062-v1_default", "nxp_ucans32k146_default", "omnibus_f4sd_default", "px4_fmu-v2_default", "px4_fmu-v2_fixedwing", - "px4_fmu-v2_lpe", "px4_fmu-v2_multicopter", "px4_fmu-v2_rover", "px4_fmu-v2_test", - "px4_fmu-v3_ctrlalloc", "px4_fmu-v3_default", "px4_fmu-v4_cannode", - "px4_fmu-v4_ctrlalloc", "px4_fmu-v4_default", - "px4_fmu-v4_optimized", "px4_fmu-v4pro_default", "px4_fmu-v5_ctrlalloc", "px4_fmu-v5_debug", @@ -89,10 +86,12 @@ pipeline { "px4_fmu-v5_rover", "px4_fmu-v5_rtps", "px4_fmu-v5_stackcheck", + "px4_fmu-v5_uavcanv0periph", + "px4_fmu-v5_uavcanv1", "px4_fmu-v5x_base_phy_DP83848C", "px4_fmu-v5x_default", - "px4_fmu-v6x_default", "px4_fmu-v6u_default", + "px4_fmu-v6x_default", "px4_io-v2_default", "spracing_h7extreme_default", "uvify_core_default" diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index adc2570937..4071bfece9 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -481,8 +481,7 @@ pipeline { // configure sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOCONFIG 2"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 13000"' // generic vtol standard - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 2000"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001"' // generic quadcopter sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io forceupdate 14662 /etc/extras/px4_io-v2_default.bin" || true' // force px4io update sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true' // reboot to apply @@ -635,7 +634,7 @@ pipeline { // configure sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOCONFIG 2"' - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 13000"' // generic vtol standard + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001"' // generic quadcopter sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save"' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true' // reboot to apply } diff --git a/.github/workflows/checks.yml b/.github/workflows/checks.yml index c4b0e2a328..a9c93ecc3b 100644 --- a/.github/workflows/checks.yml +++ b/.github/workflows/checks.yml @@ -52,8 +52,8 @@ jobs: mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf + echo "compression_level = 5" >> ~/.ccache/ccache.conf + echo "max_size = 100M" >> ~/.ccache/ccache.conf ccache -s ccache -z - name: check environment diff --git a/.github/workflows/clang-tidy.yml b/.github/workflows/clang-tidy.yml index eb12f605b5..19d3bfee31 100644 --- a/.github/workflows/clang-tidy.yml +++ b/.github/workflows/clang-tidy.yml @@ -34,8 +34,8 @@ jobs: mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf + echo "compression_level = 5" >> ~/.ccache/ccache.conf + echo "max_size = 100M" >> ~/.ccache/ccache.conf ccache -s ccache -z diff --git a/.github/workflows/compile_linux.yml b/.github/workflows/compile_linux.yml index 3ce87d237d..af3b7f8fc1 100644 --- a/.github/workflows/compile_linux.yml +++ b/.github/workflows/compile_linux.yml @@ -43,8 +43,8 @@ jobs: mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf + echo "compression_level = 5" >> ~/.ccache/ccache.conf + echo "max_size = 100M" >> ~/.ccache/ccache.conf ccache -s ccache -z diff --git a/.github/workflows/compile_linux_arm64.yml b/.github/workflows/compile_linux_arm64.yml index 2f16acffef..68877079c5 100644 --- a/.github/workflows/compile_linux_arm64.yml +++ b/.github/workflows/compile_linux_arm64.yml @@ -39,8 +39,8 @@ jobs: mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf + echo "compression_level = 5" >> ~/.ccache/ccache.conf + echo "max_size = 100M" >> ~/.ccache/ccache.conf ccache -s ccache -z diff --git a/.github/workflows/compile_macos.yml b/.github/workflows/compile_macos.yml index 1800a12070..83764400f9 100644 --- a/.github/workflows/compile_macos.yml +++ b/.github/workflows/compile_macos.yml @@ -43,8 +43,8 @@ jobs: mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf + echo "compression_level = 5" >> ~/.ccache/ccache.conf + echo "max_size = 100M" >> ~/.ccache/ccache.conf ccache -s ccache -z diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml index 0569cd13ed..f7d9194157 100644 --- a/.github/workflows/compile_nuttx.yml +++ b/.github/workflows/compile_nuttx.yml @@ -19,8 +19,8 @@ jobs: ark_can-flow_canbootloader, ark_can-flow_default, av_x-v1_default, - bitcraze_crazyflie_default, bitcraze_crazyflie21_default, + bitcraze_crazyflie_default, cuav_can-gps-v1_canbootloader, cuav_can-gps-v1_default, cuav_nora_default, @@ -49,17 +49,12 @@ jobs: omnibus_f4sd_default, px4_fmu-v2_default, px4_fmu-v2_fixedwing, - px4_fmu-v2_lpe, px4_fmu-v2_multicopter, px4_fmu-v2_rover, px4_fmu-v2_test, - px4_fmu-v3_ctrlalloc, px4_fmu-v3_default, px4_fmu-v4_cannode, - px4_fmu-v4_ctrlalloc, px4_fmu-v4_default, - px4_fmu-v4_optimized, - px4_fmu-v4_uavcanv1, px4_fmu-v4pro_default, px4_fmu-v5_ctrlalloc, px4_fmu-v5_default, @@ -68,12 +63,12 @@ jobs: px4_fmu-v5_optimized, px4_fmu-v5_rover, px4_fmu-v5_rtps, - px4_fmu-v5_stackcheck, + px4_fmu-v5_uavcanv0periph, px4_fmu-v5_uavcanv1, px4_fmu-v5x_base_phy_DP83848C, px4_fmu-v5x_default, - px4_fmu-v6x_default, px4_fmu-v6u_default, + px4_fmu-v6x_default, px4_io-v2_default, spracing_h7extreme_default, uvify_core_default @@ -100,8 +95,8 @@ jobs: mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf + echo "compression_level = 5" >> ~/.ccache/ccache.conf + echo "max_size = 100M" >> ~/.ccache/ccache.conf ccache -s ccache -z diff --git a/.github/workflows/compile_nuttx_cannode.yml b/.github/workflows/compile_nuttx_cannode.yml index cdc14ab4ac..51acadf150 100644 --- a/.github/workflows/compile_nuttx_cannode.yml +++ b/.github/workflows/compile_nuttx_cannode.yml @@ -43,8 +43,8 @@ jobs: mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf + echo "compression_level = 5" >> ~/.ccache/ccache.conf + echo "max_size = 100M" >> ~/.ccache/ccache.conf ccache -s ccache -z diff --git a/.github/workflows/mavros_avoidance_tests.yml b/.github/workflows/mavros_avoidance_tests.yml index 24dd53629f..24bf94b3f4 100644 --- a/.github/workflows/mavros_avoidance_tests.yml +++ b/.github/workflows/mavros_avoidance_tests.yml @@ -43,8 +43,8 @@ jobs: mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf + echo "compression_level = 5" >> ~/.ccache/ccache.conf + echo "max_size = 100M" >> ~/.ccache/ccache.conf ccache -s ccache -z diff --git a/.github/workflows/mavros_mission_tests.yml b/.github/workflows/mavros_mission_tests.yml index 6463e17d12..60f158f240 100644 --- a/.github/workflows/mavros_mission_tests.yml +++ b/.github/workflows/mavros_mission_tests.yml @@ -49,8 +49,8 @@ jobs: mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf + echo "compression_level = 5" >> ~/.ccache/ccache.conf + echo "max_size = 100M" >> ~/.ccache/ccache.conf ccache -s ccache -z diff --git a/.github/workflows/mavros_offboard_tests.yml b/.github/workflows/mavros_offboard_tests.yml index ed3832ca65..ddf5cc3a76 100644 --- a/.github/workflows/mavros_offboard_tests.yml +++ b/.github/workflows/mavros_offboard_tests.yml @@ -44,8 +44,8 @@ jobs: mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf + echo "compression_level = 5" >> ~/.ccache/ccache.conf + echo "max_size = 100M" >> ~/.ccache/ccache.conf ccache -s ccache -z diff --git a/.github/workflows/sitl_tests.yml b/.github/workflows/sitl_tests.yml index c96d60feed..11ad4ebea9 100644 --- a/.github/workflows/sitl_tests.yml +++ b/.github/workflows/sitl_tests.yml @@ -49,8 +49,8 @@ jobs: mkdir -p ~/.ccache echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 400M" >> ~/.ccache/ccache.conf + echo "compression_level = 5" >> ~/.ccache/ccache.conf + echo "max_size = 100M" >> ~/.ccache/ccache.conf ccache -s ccache -z @@ -94,7 +94,7 @@ jobs: PX4_HOME_LON: ${{matrix.config.longitude}} PX4_HOME_ALT: ${{matrix.config.altitude}} PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} - run: test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 20 --abort-early --model ${{matrix.config.model}} test/mavsdk_tests/configs/sitl.json + run: test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 20 --abort-early --model ${{matrix.config.model}} --upload test/mavsdk_tests/configs/sitl.json - name: Look at core files if: failure() @@ -106,10 +106,6 @@ jobs: name: coredump path: px4.core - - name: Upload logs to flight review - if: failure() - run: ./Tools/upload_log.py -q --description "${GITHUB_WORKFLOW} ${GITHUB_RUN_ID}" --feedback "${GITHUB_WORKFLOW} ${GITHUB_RUN_ID} ${GITHUB_REPOSITORY} ${GITHUB_REF}" --source CI ${GITHUB_WORKSPACE}/build/px4_sitl_default/tmp_mavsdk_tests/rootfs/log/*/*.ulg - - name: Upload px4 binary if: failure() uses: actions/upload-artifact@v2-preview diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index 988bdb420a..308ed68f50 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -131,6 +131,16 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: mro_ctrl-zero-f7_default + mro_pixracerpro_bootloader: + short: mro_pixracerpro_bootloader + buildType: MinSizeRel + settings: + CONFIG: mro_pixracerpro_bootloader + mro_pixracerpro_default: + short: mro_pixracerpro_default + buildType: MinSizeRel + settings: + CONFIG: mro_pixracerpro_default mro_x21-777_default: short: mro_x2.1-777 buildType: MinSizeRel diff --git a/CMakeLists.txt b/CMakeLists.txt index 3891c085c2..d0a54d987a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -406,7 +406,6 @@ add_subdirectory(src/lib EXCLUDE_FROM_ALL) add_subdirectory(platforms/${PX4_PLATFORM}/src/px4) add_subdirectory(platforms EXCLUDE_FROM_ALL) -add_subdirectory(src/modules/uORB EXCLUDE_FROM_ALL) # TODO: platform layer if(EXISTS "${PX4_BOARD_DIR}/CMakeLists.txt") add_subdirectory(${PX4_BOARD_DIR}) diff --git a/Jenkinsfile b/Jenkinsfile index 8a6b05f8d9..c7c3f119e1 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -67,11 +67,9 @@ pipeline { unset ROS_DISTRO; mkdir -p colcon_ws/src; cd colcon_ws; - git -C ${WORKSPACE}/colcon_ws/src/Firmware submodule update --init --recursive --force Tools/sitl_gazebo - git clone --recursive ${WORKSPACE}/colcon_ws/src/Firmware/Tools/sitl_gazebo src/mavlink_sitl_gazebo; + git -C ${WORKSPACE}/colcon_ws/src/Firmware submodule update --init --recursive --force Tools/sitl_gazebo; git -C ${WORKSPACE}/colcon_ws/src/Firmware fetch --tags; - source /opt/ros/bouncy/setup.sh; - source /opt/ros/melodic/setup.sh; + source /opt/ros/foxy/setup.sh; colcon build --event-handlers console_direct+ --symlink-install; ''' } diff --git a/ROMFS/CMakeLists.txt b/ROMFS/CMakeLists.txt index eb428bf393..b7fcfb3044 100644 --- a/ROMFS/CMakeLists.txt +++ b/ROMFS/CMakeLists.txt @@ -187,6 +187,46 @@ foreach(board_rc_file ${OPTIONAL_BOARD_RC}) endforeach() + +if(config_uavcan_peripheral_firmware) + + include(ExternalProject) + + foreach(uavcan_peripheral_config ${config_uavcan_peripheral_firmware}) + # include the px4io binary in ROMFS + message(STATUS "ROMFS: Building and including UAVCAN peripheral ${uavcan_peripheral_config}") + ExternalProject_Add(build_${uavcan_peripheral_config} + SOURCE_DIR ${CMAKE_SOURCE_DIR} + DOWNLOAD_COMMAND "" + UPDATE_COMMAND "" + CMAKE_ARGS -DCONFIG=${uavcan_peripheral_config} + INSTALL_COMMAND "" + USES_TERMINAL_BUILD true + DEPENDS git_nuttx git_nuttx_apps + BUILD_ALWAYS 1 + ) + + ExternalProject_Get_Property(build_${uavcan_peripheral_config} BINARY_DIR) + + add_custom_command( + OUTPUT + ${romfs_gen_root_dir}/uavcan/fw/_${uavcan_board_id}.bin + ${uavcan_peripheral_config}.stamp + COMMAND ${CMAKE_COMMAND} -E make_directory ${romfs_gen_root_dir}/uavcan/fw/ + COMMAND ${CMAKE_COMMAND} -E copy ${BINARY_DIR}/deploy/*.bin ${romfs_gen_root_dir}/uavcan/fw/ + COMMAND ls -lsa ${romfs_gen_root_dir}/uavcan/fw/ + COMMAND ${CMAKE_COMMAND} -E touch ${uavcan_peripheral_config}.stamp + DEPENDS + build_${uavcan_peripheral_config} + COMMENT "ROMFS: copying ${uavcan_peripheral_config}" + ) + + list(APPEND extras_dependencies + ${uavcan_peripheral_config}.stamp + ) + endforeach() +endif() + list(APPEND extras_dependencies ${config_romfs_extra_dependencies} ) diff --git a/ROMFS/cannode/init.d/rcS b/ROMFS/cannode/init.d/rcS index deb4f11aa9..f6c5a44d47 100644 --- a/ROMFS/cannode/init.d/rcS +++ b/ROMFS/cannode/init.d/rcS @@ -30,13 +30,6 @@ sercon # ver all -# -# Start the ORB (first app to start) -# tone_alarm and tune_control -# is dependent. -# -uorb start - # # Set the parameter file if mtd starts successfully. # diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10020_if750a b/ROMFS/px4fmu_common/init.d-posix/airframes/10020_if750a index 61df0d0fc7..80f44d814b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10020_if750a +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10020_if750a @@ -12,6 +12,11 @@ if [ $AUTOCNF = yes ] then # EKF2: Multi GPS blending (as the model has 2 GPS's) param set EKF2_GPS_MASK 7 # Uses speed, hpos and vpos accuracy + param set TRIG_INTERFACE 3 + param set TRIG_MODE 4 + param set MNT_MODE_IN 4 + param set MNT_MODE_OUT 2 + param set MNT_DO_STAB 2 fi set MIXER quad_x diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/3011_hexarotor_x b/ROMFS/px4fmu_common/init.d-posix/airframes/3011_hexarotor_x index 4fd04e14c4..36086a09f2 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/3011_hexarotor_x +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/3011_hexarotor_x @@ -26,7 +26,6 @@ then param set TRIG_MODE 4 param set MNT_MODE_IN 4 param set MNT_DO_STAB 2 - param set MAV_PROTO_VER 2 fi set MAV_TYPE 13 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/6011_typhoon_h480 b/ROMFS/px4fmu_common/init.d-posix/airframes/6011_typhoon_h480 index 0ae40a2248..1666c0bd44 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/6011_typhoon_h480 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/6011_typhoon_h480 @@ -24,7 +24,8 @@ then param set TRIG_INTERFACE 3 param set TRIG_MODE 4 - param set MNT_MODE_IN 0 + param set MNT_MODE_IN 4 + param set MNT_MODE_OUT 2 param set MAV_PROTO_VER 2 fi diff --git a/ROMFS/px4fmu_common/init.d-posix/rc.replay b/ROMFS/px4fmu_common/init.d-posix/rc.replay index 25a9612197..89c999a38c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rc.replay +++ b/ROMFS/px4fmu_common/init.d-posix/rc.replay @@ -20,7 +20,6 @@ module: replay ignore_others: false EOF -uorb start param set SDLOG_DIRS_MAX 7 # apply all params before ekf starts, as some params cannot be changed after startup diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index edf49e203a..7b535701ae 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -53,8 +53,6 @@ else fi fi -uorb start - # Load parameters set PARAM_FILE eeprom/parameters_"$REQUESTED_AUTOSTART" param select $PARAM_FILE @@ -111,6 +109,8 @@ udp_offboard_port_remote=$((14540+px4_instance)) [ $px4_instance -gt 9 ] && udp_offboard_port_remote=14549 # use the same ports for more than 10 instances to avoid port overlaps udp_onboard_payload_port_local=$((14280+px4_instance)) udp_onboard_payload_port_remote=$((14030+px4_instance)) +udp_onboard_gimbal_port_local=$((13030+px4_instance)) +udp_onboard_gimbal_port_remote=$((13280+px4_instance)) udp_gcs_port_local=$((18570+px4_instance)) if [ $AUTOCNF = yes ] @@ -244,7 +244,7 @@ then sh etc/init.d-posix/rc.mavlink_override else # GCS link - mavlink start -x -u $udp_gcs_port_local -r 4000000 + mavlink start -x -u $udp_gcs_port_local -r 4000000 -f mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u $udp_gcs_port_local mavlink stream -r 50 -s LOCAL_POSITION_NED -u $udp_gcs_port_local mavlink stream -r 50 -s GLOBAL_POSITION_INT -u $udp_gcs_port_local @@ -256,12 +256,15 @@ else mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local # API/Offboard link - mavlink start -x -u $udp_offboard_port_local -r 4000000 -m onboard -o $udp_offboard_port_remote + mavlink start -x -u $udp_offboard_port_local -r 4000000 -f -m onboard -o $udp_offboard_port_remote # Onboard link to camera mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote + # Onboard link to gimbal + mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -m gimbal -o $udp_onboard_gimbal_port_remote fi + # execute autostart post script if any [ -e "$autostart_file".post ] && . "$autostart_file".post diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6c2d616505..3c49fee229 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -73,13 +73,6 @@ sercon # ver all -# -# Start the ORB (first app to start) -# tone_alarm and tune_control -# is dependent. -# -uorb start - # # Try to mount the microSD card. # @@ -289,33 +282,18 @@ else # tune Program PX4IO tune_control play -t 16 # tune 16 = PROG_PX4IO - if px4io start - then - # Try to safety px4 io so motor outputs don't go crazy. - if ! px4io safety_on - then - # px4io did not respond to the safety command. - px4io stop - fi - fi - - if px4io forceupdate 14662 ${IOFW} + if px4io update ${IOFW} then usleep 10000 tune_control stop if px4io checkcrc ${IOFW} then - echo "PX4IO CRC OK after updating" tune_control play -t 17 # tune 17 = PROG_PX4IO_OK set IO_PRESENT yes + else + tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR fi fi - - if [ $IO_PRESENT = no ] - then - echo "PX4IO update failed" - tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR - fi fi fi fi diff --git a/ROMFS/px4fmu_test/init.d/rc.standalone b/ROMFS/px4fmu_test/init.d/rc.standalone index 84f305e881..a0072d5d1d 100644 --- a/ROMFS/px4fmu_test/init.d/rc.standalone +++ b/ROMFS/px4fmu_test/init.d/rc.standalone @@ -5,9 +5,4 @@ echo "[i] doing standalone PX4FMU startup..." -# -# Start the ORB -# -uorb start - echo "[i] startup done" diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 2097d6acc7..dfd74cd828 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -8,8 +8,6 @@ set unit_test_failure 0 set BOARD_RC ${R}etc/init.d/rc.board -uorb start - if rgbled start -X then led_control on -c blue @@ -41,11 +39,6 @@ fi # Start a minimal system # -# -# Start the ORB (first app to start) -# -uorb start - # # Load parameters # @@ -91,7 +84,7 @@ then else echo "PX4IO CRC failure" tune_control play -t 16 # tune 16 = PROG_PX4IO - if px4io forceupdate 14662 $io_file + if px4io update $io_file then if px4io start then diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index 2501a2a62f..408fca1556 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -17,7 +17,7 @@ exec find boards msg src platforms test \ -path src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis -prune -o \ -path src/lib/ecl -prune -o \ -path src/lib/matrix -prune -o \ - -path src/lib/systemlib/uthash -prune -o \ + -path src/lib/parameters/uthash -prune -o \ -path src/examples/gyro_fft/CMSIS_5 -prune -o \ -path src/modules/micrortps_bridge/micro-CDR -prune -o \ -path src/modules/micrortps_bridge/microRTPS_client -prune -o \ diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 4f26815d41..bebb9a95f0 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 4f26815d4127e3af84e01bbc1f8683c4fe30521f +Subproject commit bebb9a95f0b61bf9e4c3de345fab70985c1329b3 diff --git a/boards/aerotenna/ocpoc/default.cmake b/boards/aerotenna/ocpoc/default.cmake index 76c9993f09..0a8ec6719d 100644 --- a/boards/aerotenna/ocpoc/default.cmake +++ b/boards/aerotenna/ocpoc/default.cmake @@ -83,9 +83,11 @@ px4_add_board( #top topic_listener tune_control + uorb ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 507f621ee8..7fab0bc386 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -105,10 +105,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/ark/can-flow/canbootloader.cmake b/boards/ark/can-flow/canbootloader.cmake index e9b1a27581..1ca01c65a6 100644 --- a/boards/ark/can-flow/canbootloader.cmake +++ b/boards/ark/can-flow/canbootloader.cmake @@ -1,21 +1,4 @@ - -# UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) -add_definitions( - -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} - -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} -) - -set(uavcanblid_hw_version_major 1) -set(uavcanblid_hw_version_minor 0) -set(uavcanblid_name "\"org.ark.can-flow\"") - -add_definitions( - -DHW_UAVCAN_NAME=${uavcanblid_name} - -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} - -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} -) +include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) px4_add_board( PLATFORM nuttx diff --git a/boards/ark/can-flow/default.cmake b/boards/ark/can-flow/default.cmake index 73cf17bcac..eaaa5120de 100644 --- a/boards/ark/can-flow/default.cmake +++ b/boards/ark/can-flow/default.cmake @@ -1,22 +1,4 @@ - - -# UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) -add_definitions( - -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} - -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} -) - -set(uavcanblid_hw_version_major 1) -set(uavcanblid_hw_version_minor 0) -set(uavcanblid_name "\"org.ark.can-flow\"") - -add_definitions( - -DHW_UAVCAN_NAME=${uavcanblid_name} - -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} - -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} -) +include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) px4_add_board( PLATFORM nuttx diff --git a/boards/ark/can-flow/firmware.prototype b/boards/ark/can-flow/firmware.prototype index 4d40e94dea..a59e1adca6 100644 --- a/boards/ark/can-flow/firmware.prototype +++ b/boards/ark/can-flow/firmware.prototype @@ -1,5 +1,5 @@ { - "board_id": 11, + "board_id": 80, "magic": "PX4FWv1", "description": "Firmware for the ARK flow board", "image": "", diff --git a/boards/ark/can-flow/uavcan_board_identity b/boards/ark/can-flow/uavcan_board_identity new file mode 100644 index 0000000000..8f389d0657 --- /dev/null +++ b/boards/ark/can-flow/uavcan_board_identity @@ -0,0 +1,17 @@ +# UAVCAN boot loadable Module ID +set(uavcanblid_sw_version_major 0) +set(uavcanblid_sw_version_minor 1) +add_definitions( + -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} + -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} +) + +set(uavcanblid_hw_version_major 0) +set(uavcanblid_hw_version_minor 80) +set(uavcanblid_name "\"org.ark.can-flow\"") + +add_definitions( + -DHW_UAVCAN_NAME=${uavcanblid_name} + -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} + -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} +) \ No newline at end of file diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake index 1a03e8b536..6b8d4ff453 100644 --- a/boards/atlflight/eagle/default.cmake +++ b/boards/atlflight/eagle/default.cmake @@ -110,9 +110,11 @@ px4_add_board( #top topic_listener tune_control + uorb ver work_queue EXAMPLES + #fake_gps #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello #hwtest # Hardware test diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake index b67f9dbd34..25dbf2f533 100644 --- a/boards/atlflight/excelsior/default.cmake +++ b/boards/atlflight/excelsior/default.cmake @@ -108,9 +108,11 @@ px4_add_board( #top topic_listener tune_control + uorb ver work_queue EXAMPLES + #fake_gps #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello #hwtest # Hardware test diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index 9d9e681e5d..717d7e7964 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -102,9 +102,11 @@ px4_add_board( top topic_listener tune_control + uorb ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/beaglebone/blue/default.cmake b/boards/beaglebone/blue/default.cmake index dcd771167d..cdcd011127 100644 --- a/boards/beaglebone/blue/default.cmake +++ b/boards/beaglebone/blue/default.cmake @@ -79,9 +79,11 @@ px4_add_board( #top topic_listener tune_control + uorb ver work_queue EXAMPLES + fake_gps dyn_hello # dynamically loading modules example fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake index 0cfe49246b..23caa2cd31 100644 --- a/boards/bitcraze/crazyflie/default.cmake +++ b/boards/bitcraze/crazyflie/default.cmake @@ -64,6 +64,7 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue diff --git a/boards/bitcraze/crazyflie21/default.cmake b/boards/bitcraze/crazyflie21/default.cmake index 7761ac7203..3ce93a5b89 100644 --- a/boards/bitcraze/crazyflie21/default.cmake +++ b/boards/bitcraze/crazyflie21/default.cmake @@ -61,6 +61,7 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue diff --git a/boards/cuav/can-gps-v1/canbootloader.cmake b/boards/cuav/can-gps-v1/canbootloader.cmake index d87b5b3511..048aba3880 100644 --- a/boards/cuav/can-gps-v1/canbootloader.cmake +++ b/boards/cuav/can-gps-v1/canbootloader.cmake @@ -1,21 +1,4 @@ - -# UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) -add_definitions( - -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} - -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} -) - -set(uavcanblid_hw_version_major 1) -set(uavcanblid_hw_version_minor 0) -set(uavcanblid_name "\"org.cuav.can-gps-v1\"") - -add_definitions( - -DHW_UAVCAN_NAME=${uavcanblid_name} - -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} - -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} -) +include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) px4_add_board( PLATFORM nuttx diff --git a/boards/cuav/can-gps-v1/debug.cmake b/boards/cuav/can-gps-v1/debug.cmake new file mode 100644 index 0000000000..dd8613a8f4 --- /dev/null +++ b/boards/cuav/can-gps-v1/debug.cmake @@ -0,0 +1,53 @@ + + +# UAVCAN boot loadable Module ID +set(uavcanblid_sw_version_major 0) +set(uavcanblid_sw_version_minor 1) +add_definitions( + -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} + -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} +) + +set(uavcanblid_hw_version_major 1) +set(uavcanblid_hw_version_minor 0) +set(uavcanblid_name "\"org.cuav.can-gps-v1\"") + +add_definitions( + -DHW_UAVCAN_NAME=${uavcanblid_name} + -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} + -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} +) +add_definitions(-DUSE_S_RGB_LED_DMA) + +px4_add_board( + PLATFORM nuttx + VENDOR cuav + MODEL can-gps-v1 + LABEL debug + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m4 + ROMFSROOT cannode + UAVCAN_INTERFACES 1 + DRIVERS + barometer/ms5611 + bootloaders + gps + lights/neopixel + magnetometer/rm3100 + safety_button + tone_alarm + uavcannode + MODULES + load_mon + SYSTEMCMDS + i2cdetect + led_control + param + perf + reboot + top + topic_listener + tune_control + ver + work_queue +) diff --git a/boards/cuav/can-gps-v1/default.cmake b/boards/cuav/can-gps-v1/default.cmake index 8a03049938..ac89746511 100644 --- a/boards/cuav/can-gps-v1/default.cmake +++ b/boards/cuav/can-gps-v1/default.cmake @@ -1,22 +1,5 @@ +include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) - -# UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) -add_definitions( - -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} - -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} -) - -set(uavcanblid_hw_version_major 1) -set(uavcanblid_hw_version_minor 0) -set(uavcanblid_name "\"org.cuav.can-gps-v1\"") - -add_definitions( - -DHW_UAVCAN_NAME=${uavcanblid_name} - -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} - -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} -) add_definitions(-DUSE_S_RGB_LED_DMA) px4_add_board( @@ -49,6 +32,7 @@ px4_add_board( top topic_listener tune_control + uorb ver work_queue ) diff --git a/boards/cuav/can-gps-v1/nuttx-config/canbootloader/defconfig b/boards/cuav/can-gps-v1/nuttx-config/canbootloader/defconfig index 40dffb67e8..73eae9027f 100644 --- a/boards/cuav/can-gps-v1/nuttx-config/canbootloader/defconfig +++ b/boards/cuav/can-gps-v1/nuttx-config/canbootloader/defconfig @@ -61,6 +61,7 @@ CONFIG_STACK_COLORATION=y CONFIG_START_DAY=30 CONFIG_START_MONTH=11 CONFIG_STDIO_DISABLE_BUFFERING=y +CONFIG_STM32_JTAG_SW_ENABLE=y CONFIG_STM32_NOEXT_VECTORS=y CONFIG_STM32_TIM8=y CONFIG_TASK_NAME_SIZE=0 diff --git a/boards/cuav/can-gps-v1/nuttx-config/nsh/defconfig b/boards/cuav/can-gps-v1/nuttx-config/nsh/defconfig index 7059f5bbe9..07587454f6 100644 --- a/boards/cuav/can-gps-v1/nuttx-config/nsh/defconfig +++ b/boards/cuav/can-gps-v1/nuttx-config/nsh/defconfig @@ -139,6 +139,7 @@ CONFIG_STM32_RTC_MAGIC_REG=1 CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef CONFIG_STM32_SERIALBRK_BSDCOMPAT=y CONFIG_STM32_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_JTAG_SW_ENABLE=y CONFIG_STM32_SPI1=y CONFIG_STM32_TIM8=y CONFIG_STM32_USART1=y diff --git a/boards/cuav/can-gps-v1/nuttx-config/scripts/canbootloader_script.ld b/boards/cuav/can-gps-v1/nuttx-config/scripts/canbootloader_script.ld index cbf9fddc32..758a42609b 100644 --- a/boards/cuav/can-gps-v1/nuttx-config/scripts/canbootloader_script.ld +++ b/boards/cuav/can-gps-v1/nuttx-config/scripts/canbootloader_script.ld @@ -50,8 +50,9 @@ MEMORY { - flash (rx) : ORIGIN = 0x08000000, LENGTH = 32K - sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K + flash (rx) : ORIGIN = 0x08000000, LENGTH = 32K + shared_sram (rwx) : ORIGIN = 0x20000000, LENGTH = 256 + sram (rwx) : ORIGIN = 0x20000100, LENGTH = 192K - 256 } OUTPUT_ARCH(arm) @@ -101,6 +102,10 @@ SECTIONS _eronly = ABSOLUTE(.); + .app_bl_shared : { + _sapp_bl_shared = ABSOLUTE(.); + } > shared_sram + .data : { _sdata = ABSOLUTE(.); *(.data .data.*) diff --git a/boards/cuav/can-gps-v1/nuttx-config/scripts/script.ld b/boards/cuav/can-gps-v1/nuttx-config/scripts/script.ld index ccd8ad4b0b..4e866c5acf 100644 --- a/boards/cuav/can-gps-v1/nuttx-config/scripts/script.ld +++ b/boards/cuav/can-gps-v1/nuttx-config/scripts/script.ld @@ -50,8 +50,9 @@ MEMORY { - flash (rx) : ORIGIN = 0x08010000, LENGTH = 448K - sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K + flash (rx) : ORIGIN = 0x08010000, LENGTH = 448K + shared_sram (rwx) : ORIGIN = 0x20000000, LENGTH = 256 + sram (rwx) : ORIGIN = 0x20000100, LENGTH = 192K - 256 } OUTPUT_ARCH(arm) @@ -114,6 +115,10 @@ SECTIONS _eronly = ABSOLUTE(.); + .app_bl_shared : { + _sapp_bl_shared = ABSOLUTE(.); + } > shared_sram + .data : { _sdata = ABSOLUTE(.); *(.data .data.*) diff --git a/boards/cuav/can-gps-v1/src/CMakeLists.txt b/boards/cuav/can-gps-v1/src/CMakeLists.txt index 0b038c9fdf..7b84c71cc5 100644 --- a/boards/cuav/can-gps-v1/src/CMakeLists.txt +++ b/boards/cuav/can-gps-v1/src/CMakeLists.txt @@ -34,10 +34,8 @@ if("${PX4_BOARD_LABEL}" STREQUAL "canbootloader") add_library(drivers_board - boot_config.h boot.c led.cpp - led.h ) target_link_libraries(drivers_board diff --git a/boards/cuav/can-gps-v1/src/init.c b/boards/cuav/can-gps-v1/src/init.c index 46db91809a..36f445546e 100644 --- a/boards/cuav/can-gps-v1/src/init.c +++ b/boards/cuav/can-gps-v1/src/init.c @@ -66,6 +66,7 @@ #include #include +#include #include @@ -88,6 +89,7 @@ __EXPORT void stm32_boardinitialize(void) { + watchdog_init(); // Configure CAN interface stm32_configgpio(GPIO_CAN1_RX); stm32_configgpio(GPIO_CAN1_TX); diff --git a/boards/cuav/can-gps-v1/uavcan_board_identity b/boards/cuav/can-gps-v1/uavcan_board_identity new file mode 100644 index 0000000000..a547589079 --- /dev/null +++ b/boards/cuav/can-gps-v1/uavcan_board_identity @@ -0,0 +1,18 @@ +# UAVCAN boot loadable Module ID +set(uavcanblid_sw_version_major 0) +set(uavcanblid_sw_version_minor 1) +add_definitions( + -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} + -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} +) + +set(uavcanblid_hw_version_major 3) +set(uavcanblid_hw_version_minor 233) +set(uavcanblid_name "\"org.cuav.can-gps-v1\"") +add_definitions( + -DHW_UAVCAN_NAME=${uavcanblid_name} + -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} + -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} + -DSUPPORT_ALT_CAN_BOOTLOADER + +) diff --git a/boards/cuav/nora/default.cmake b/boards/cuav/nora/default.cmake index c5688aeb9a..585b2fba9e 100644 --- a/boards/cuav/nora/default.cmake +++ b/boards/cuav/nora/default.cmake @@ -109,10 +109,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/cuav/nora/init/rc.board_sensors b/boards/cuav/nora/init/rc.board_sensors index 05375d625c..fd626eb54a 100644 --- a/boards/cuav/nora/init/rc.board_sensors +++ b/boards/cuav/nora/init/rc.board_sensors @@ -16,7 +16,7 @@ bmi088 -s -b 4 -G -R 2 start ms5611 -s -b 4 start # SPI6 -icm20649 -s -b 6 -R 2 start +#icm20649 -s -b 6 -R 2 start # TODO: not started by default until NuttX SPI6 BDMA is fixed ms5611 -s -b 6 start # External compass on GPS1/I2C1: standard CUAV GPS/compass puck (with lights, safety button, and buzzer) diff --git a/boards/cuav/x7pro/default.cmake b/boards/cuav/x7pro/default.cmake index 1e011dcc1e..a16f4e8e28 100644 --- a/boards/cuav/x7pro/default.cmake +++ b/boards/cuav/x7pro/default.cmake @@ -112,10 +112,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/cubepilot/cubeorange/console.cmake b/boards/cubepilot/cubeorange/console.cmake index 043e4ce6cd..baddd1a3b9 100644 --- a/boards/cubepilot/cubeorange/console.cmake +++ b/boards/cubepilot/cubeorange/console.cmake @@ -113,10 +113,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/cubepilot/cubeorange/default.cmake b/boards/cubepilot/cubeorange/default.cmake index 0ba9f31077..2c7f66b911 100644 --- a/boards/cubepilot/cubeorange/default.cmake +++ b/boards/cubepilot/cubeorange/default.cmake @@ -113,10 +113,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/cubepilot/cubeyellow/console.cmake b/boards/cubepilot/cubeyellow/console.cmake index 2f2201d4d3..8437416762 100644 --- a/boards/cubepilot/cubeyellow/console.cmake +++ b/boards/cubepilot/cubeyellow/console.cmake @@ -112,10 +112,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/cubepilot/cubeyellow/default.cmake b/boards/cubepilot/cubeyellow/default.cmake index 4b3a5d67b3..bae4a05b0c 100644 --- a/boards/cubepilot/cubeyellow/default.cmake +++ b/boards/cubepilot/cubeyellow/default.cmake @@ -112,10 +112,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/emlid/navio2/default.cmake b/boards/emlid/navio2/default.cmake index fea8499992..ee0d6c30a4 100644 --- a/boards/emlid/navio2/default.cmake +++ b/boards/emlid/navio2/default.cmake @@ -79,10 +79,12 @@ px4_add_board( #top topic_listener tune_control + uorb ver work_queue EXAMPLES dyn_hello # dynamically loading modules example + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test diff --git a/boards/holybro/can-gps-v1/canbootloader.cmake b/boards/holybro/can-gps-v1/canbootloader.cmake index 6678c66511..510604a31c 100644 --- a/boards/holybro/can-gps-v1/canbootloader.cmake +++ b/boards/holybro/can-gps-v1/canbootloader.cmake @@ -1,21 +1,4 @@ - -# UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) -add_definitions( - -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} - -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} -) - -set(uavcanblid_hw_version_major 1) -set(uavcanblid_hw_version_minor 0) -set(uavcanblid_name "\"org.holybro.can-gps-v1\"") - -add_definitions( - -DHW_UAVCAN_NAME=${uavcanblid_name} - -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} - -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} -) +include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) px4_add_board( PLATFORM nuttx diff --git a/boards/holybro/can-gps-v1/default.cmake b/boards/holybro/can-gps-v1/default.cmake index 9c63f035dd..69ad84971b 100644 --- a/boards/holybro/can-gps-v1/default.cmake +++ b/boards/holybro/can-gps-v1/default.cmake @@ -1,22 +1,4 @@ - - -# UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) -add_definitions( - -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} - -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} -) - -set(uavcanblid_hw_version_major 1) -set(uavcanblid_hw_version_minor 0) -set(uavcanblid_name "\"org.holybro.can-gps-v1\"") - -add_definitions( - -DHW_UAVCAN_NAME=${uavcanblid_name} - -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} - -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} -) +include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity) px4_add_board( PLATFORM nuttx diff --git a/boards/holybro/can-gps-v1/uavcan_board_identity b/boards/holybro/can-gps-v1/uavcan_board_identity new file mode 100644 index 0000000000..c34b51cd77 --- /dev/null +++ b/boards/holybro/can-gps-v1/uavcan_board_identity @@ -0,0 +1,17 @@ +# UAVCAN boot loadable Module ID +set(uavcanblid_sw_version_major 0) +set(uavcanblid_sw_version_minor 1) +add_definitions( + -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} + -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} +) + +set(uavcanblid_hw_version_major 0) +set(uavcanblid_hw_version_minor 79) +set(uavcanblid_name "\"org.holybro.can-gps-v1\"") + +add_definitions( + -DHW_UAVCAN_NAME=${uavcanblid_name} + -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} + -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} +) diff --git a/boards/holybro/durandal-v1/default.cmake b/boards/holybro/durandal-v1/default.cmake index 82c153a347..eba0412b43 100644 --- a/boards/holybro/durandal-v1/default.cmake +++ b/boards/holybro/durandal-v1/default.cmake @@ -110,10 +110,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/holybro/kakutef7/default.cmake b/boards/holybro/kakutef7/default.cmake index 6cb637baf0..0e76f42608 100644 --- a/boards/holybro/kakutef7/default.cmake +++ b/boards/holybro/kakutef7/default.cmake @@ -72,6 +72,7 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue diff --git a/boards/holybro/pix32v5/default.cmake b/boards/holybro/pix32v5/default.cmake index 20f8115817..625c6cc5e4 100644 --- a/boards/holybro/pix32v5/default.cmake +++ b/boards/holybro/pix32v5/default.cmake @@ -117,10 +117,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/intel/aerofc-v1/default.cmake b/boards/intel/aerofc-v1/default.cmake index 77a8168204..86fb32f8bf 100644 --- a/boards/intel/aerofc-v1/default.cmake +++ b/boards/intel/aerofc-v1/default.cmake @@ -88,9 +88,11 @@ px4_add_board( top #topic_listener tune_control + uorb ver work_queue EXAMPLES + #fake_gps #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello #hwtest # Hardware test diff --git a/boards/intel/aerofc-v1/rtps.cmake b/boards/intel/aerofc-v1/rtps.cmake deleted file mode 100644 index dd47790a00..0000000000 --- a/boards/intel/aerofc-v1/rtps.cmake +++ /dev/null @@ -1,102 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR intel - MODEL aerofc-v1 - LABEL rtps - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - SERIAL_PORTS - GPS1:/dev/ttyS5 - TEL1:/dev/ttyS3 - TEL2:/dev/ttyS1 - DRIVERS - barometer/ms5611 - #camera_trigger - #differential_pressure # all available differential pressure drivers - distance_sensor - gps - imu/invensense/mpu9250 - #irlock - #magnetometer # all available magnetometer drivers - magnetometer/hmc5883 - magnetometer/isentek/ist8310 - #optical_flow/px4flow - protocol_splitter - pwm_out_sim - pwm_out - rc_input - tap_esc - #telemetry # all available telemetry drivers - #uavcan - MODULES - #airspeed_selector - attitude_estimator_q - battery_status - #camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - #fw_att_control - #fw_pos_control_l1 - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - micrortps_bridge - navigator - rc_update - #rover_pos_control - sensors - #sih - #temperature_compensation - vmount - #vtol_att_control - SYSTEMCMDS - bl_update - #dmesg - dumpfile - esc_calib - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - system_time - #tests # tests and test runner - top - #topic_listener - tune_control - ver - work_queue - EXAMPLES - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - #work_item - ) diff --git a/boards/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake index 54bfb8901f..1cba5226b5 100644 --- a/boards/modalai/fc-v1/default.cmake +++ b/boards/modalai/fc-v1/default.cmake @@ -104,10 +104,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/mro/ctrl-zero-f7-oem/default.cmake b/boards/mro/ctrl-zero-f7-oem/default.cmake index 887020703c..609f544966 100644 --- a/boards/mro/ctrl-zero-f7-oem/default.cmake +++ b/boards/mro/ctrl-zero-f7-oem/default.cmake @@ -14,7 +14,7 @@ px4_add_board( TEL2:/dev/ttyS1 GPS1:/dev/ttyS2 #RC:/dev/ttyS3 - #CONSOLE:/dev/ttyS4 + TEL3:/dev/ttyS4 #FRSKY:/dev/ttyS5 DRIVERS adc/board_adc @@ -109,10 +109,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/mro/ctrl-zero-f7-oem/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-f7-oem/nuttx-config/nsh/defconfig index fdad0fa052..2cc405eb3f 100644 --- a/boards/mro/ctrl-zero-f7-oem/nuttx-config/nsh/defconfig +++ b/boards/mro/ctrl-zero-f7-oem/nuttx-config/nsh/defconfig @@ -160,8 +160,8 @@ CONFIG_STM32F7_DMA2=y CONFIG_STM32F7_DMACAPABLE=y CONFIG_STM32F7_FLOWCONTROL_BROKEN=y CONFIG_STM32F7_I2C1=y -CONFIG_STM32H7_I2C3=y -CONFIG_STM32H7_I2C4=y +CONFIG_STM32F7_I2C3=y +CONFIG_STM32F7_I2C4=y CONFIG_STM32F7_I2C_DYNTIMEO=y CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 CONFIG_STM32F7_OTGFS=y @@ -208,11 +208,11 @@ CONFIG_UART4_RXDMA=y CONFIG_UART4_TXBUFSIZE=1500 CONFIG_UART7_BAUD=57600 CONFIG_UART7_RXBUFSIZE=600 -CONFIG_UART7_SERIAL_CONSOLE=y CONFIG_UART7_TXBUFSIZE=1500 CONFIG_UART8_BAUD=57600 CONFIG_UART8_RXBUFSIZE=600 CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_UART8_SERIAL_CONSOLE=y CONFIG_USART2_BAUD=57600 CONFIG_USART2_IFLOWCONTROL=y CONFIG_USART2_OFLOWCONTROL=y diff --git a/boards/mro/ctrl-zero-f7-oem/src/board_config.h b/boards/mro/ctrl-zero-f7-oem/src/board_config.h index a98f9c40e4..ea3efff8a7 100644 --- a/boards/mro/ctrl-zero-f7-oem/src/board_config.h +++ b/boards/mro/ctrl-zero-f7-oem/src/board_config.h @@ -104,6 +104,7 @@ /* CAN Silence: Silent mode control \ ESC Mux select */ #define GPIO_CAN1_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) +#define GPIO_CAN2_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) /* PWM */ #define DIRECT_PWM_OUTPUT_CHANNELS 8 @@ -190,8 +191,9 @@ GPIO_CAN1_TX, \ GPIO_CAN1_RX, \ GPIO_CAN2_TX, \ - GPIO_CAN2_RX, \ + GPIO_CAN2_RX, \ GPIO_CAN1_SILENT_S0, \ + GPIO_CAN2_SILENT_S0, \ GPIO_nPOWER_IN_A, \ GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ GPIO_TONE_ALARM_IDLE, \ diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index 169d907c6a..cae28037a6 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -109,10 +109,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/mro/ctrl-zero-h7-oem/bootloader.cmake b/boards/mro/ctrl-zero-h7-oem/bootloader.cmake new file mode 100644 index 0000000000..d4543ee775 --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/bootloader.cmake @@ -0,0 +1,9 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR mro + MODEL ctrl-zero-h7-oem + LABEL bootloader + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m7 +) diff --git a/boards/px4/fmu-v5/irqmonitor.cmake b/boards/mro/ctrl-zero-h7-oem/default.cmake similarity index 83% rename from boards/px4/fmu-v5/irqmonitor.cmake rename to boards/mro/ctrl-zero-h7-oem/default.cmake index 84f51e730a..16e89bafca 100644 --- a/boards/px4/fmu-v5/irqmonitor.cmake +++ b/boards/mro/ctrl-zero-h7-oem/default.cmake @@ -1,23 +1,26 @@ px4_add_board( PLATFORM nuttx - VENDOR px4 - MODEL fmu-v5 - LABEL irqmonitor + VENDOR mro + MODEL ctrl-zero-h7-oem + LABEL default TOOLCHAIN arm-none-eabi ARCHITECTURE cortex-m7 ROMFSROOT px4fmu_common - IO px4_io-v2_default + BUILD_BOOTLOADER TESTING UAVCAN_INTERFACES 2 SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS3 + TEL1:/dev/ttyS0 + TEL2:/dev/ttyS1 + GPS1:/dev/ttyS2 + #RC:/dev/ttyS3 + TEL3:/dev/ttyS4 + #FRSKY:/dev/ttyS5 DRIVERS adc/board_adc - barometer # all available barometer drivers + #barometer # all available barometer drivers + barometer/dps310 batt_smbus camera_capture camera_trigger @@ -25,34 +28,28 @@ px4_add_board( distance_sensor # all available distance sensor drivers dshot gps - #heater #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/adis16477 - imu/adis16497 - imu/bosch/bmi055 + imu/bosch/bmi088 imu/invensense/icm20602 - imu/invensense/icm20689 - #imu/mpu6000 # legacy icm20602/icm20689 driver + imu/invensense/icm20948 irlock lights/blinkm lights/rgbled lights/rgbled_ncp5623c - lights/rgbled_pwm magnetometer # all available magnetometer drivers + mkblctrl optical_flow # all available optical flow drivers #osd pca9685 power_monitor/ina226 #protocol_splitter - pwm_input + #pwm_input pwm_out_sim pwm_out - px4io rc_input roboclaw rpm - safety_button + #safety_button TODO tap_esc telemetry # all available telemetry drivers test_ppm @@ -66,6 +63,7 @@ px4_add_board( commander dataman ekf2 + esc_battery events flight_mode_manager fw_att_control @@ -94,6 +92,7 @@ px4_add_board( dmesg dumpfile esc_calib + gpio hardfault_log i2cdetect led_control @@ -118,6 +117,7 @@ px4_add_board( ver work_queue EXAMPLES + fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/mro/ctrl-zero-h7-oem/firmware.prototype b/boards/mro/ctrl-zero-h7-oem/firmware.prototype new file mode 100644 index 0000000000..cb21149851 --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1024, + "magic": "mRo-ctrl-zero-h7-oem", + "description": "Firmware for the mRo-ctrl-zero-h7-oem board", + "image": "", + "build_time": 0, + "summary": "mRo-ctrl-zero-h7-oem", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1966080, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/mro/ctrl-zero-h7-oem/init/rc.board_defaults b/boards/mro/ctrl-zero-h7-oem/init/rc.board_defaults new file mode 100644 index 0000000000..98136ea19e --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/init/rc.board_defaults @@ -0,0 +1,42 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +# +# Bootloader upgrade +# +set BL_FILE /etc/extras/mro_ctrl-zero-h7_bootloader.bin +if [ -f $BL_FILE ] +then + if param compare SYS_BL_UPDATE 1 + then + param set SYS_BL_UPDATE 0 + param save + echo "BL update..." + bl_update $BL_FILE + echo "BL update done" + reboot + fi +fi +unset BL_FILE + + +param set-default BAT_V_DIV 10.1 +param set-default BAT1_V_DIV 10.1 + +param set-default BAT_A_PER_V 24 +param set-default BAT1_A_PER_V 24 + +# Multi-EKF +param set-default EKF2_MULTI_IMU 3 +param set-default SENS_IMU_MODE 0 +param set-default EKF2_MULTI_MAG 3 +param set-default SENS_MAG_MODE 0 + +param set-default UAVCAN_ENABLE 2 + + +set LOGGER_BUF 64 + +safety_button start diff --git a/boards/mro/ctrl-zero-h7-oem/init/rc.board_mavlink b/boards/mro/ctrl-zero-h7-oem/init/rc.board_mavlink new file mode 100644 index 0000000000..08fed10e71 --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/init/rc.board_mavlink @@ -0,0 +1,7 @@ +#!/bin/sh +# +# Board specific MAVLink startup script. +#------------------------------------------------------------------------------ + +# Start MAVLink on the USB port +mavlink start -d /dev/ttyACM0 diff --git a/boards/mro/ctrl-zero-h7-oem/init/rc.board_sensors b/boards/mro/ctrl-zero-h7-oem/init/rc.board_sensors new file mode 100644 index 0000000000..111e3eb044 --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/init/rc.board_sensors @@ -0,0 +1,18 @@ +#!/bin/sh +# +# board specific sensors init +#------------------------------------------------------------------------------ +board_adc start + +# Internal ICM-20602 +icm20602 -s -b 1 -R 8 start + +# Internal SPI bus BMI088 accel & gyro +bmi088 -A -s -b 5 -R 8 start +bmi088 -G -s -b 5 -R 8 start + +# Internal ICM-20948 (with magnetometer) +icm20948 -s -b 1 -R 8 -M start + +# Interal DPS310 (barometer) +dps310 -s -b 2 start diff --git a/boards/mro/ctrl-zero-h7-oem/nuttx-config/bootloader/defconfig b/boards/mro/ctrl-zero-h7-oem/nuttx-config/bootloader/defconfig new file mode 100644 index 0000000000..2f526b433c --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/nuttx-config/bootloader/defconfig @@ -0,0 +1,95 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DEV_CONSOLE is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_SPI_EXCHANGE is not set +# CONFIG_STM32H7_SYSCFG is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_PRODUCTID=0x1023 +CONFIG_CDCACM_PRODUCTSTR="PX4 BL mRo ControlZeroH7 OEM" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="mRo" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_PTHREAD=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIB_BOARDCTL=y +CONFIG_MAX_TASKS=8 +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MM_REGIONS=3 +CONFIG_NFILE_DESCRIPTORS=5 +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SPI=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_USART3=y +CONFIG_SYSTEMTICK_HOOK=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGINT_CHAR=0x03 +CONFIG_TTY_SIGSTP=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="bootloader_main" diff --git a/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h b/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h new file mode 100644 index 0000000000..3d4a5c880f --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h @@ -0,0 +1,278 @@ +/************************************************************************************ + * nuttx-config/include/board.h + * + * Copyright (C) 2020 Gregory Nutt. All rights reserved. + * Authors: David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ +#pragma once + +#include "board_dma_map.h" + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/* Clocking *************************************************************************/ +/* The board provides the following clock sources: + * + * X1: 24 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 16 MHz RC factory-trimmed internal oscillator + * HSE: 24 MHz crystal for HSE + */ +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 0 + +/* Main PLL Configuration. + * + * PLL source is HSE = 24,000,000 + * + * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 1 <= PLLM <= 63 + * 4 <= PLLN <= 512 + * 150 MHz <= PLL_VCOL <= 420MHz + * 192 MHz <= PLL_VCOH <= 836MHz + * + * SYSCLK = PLL_VCO / PLLP + * CPUCLK = SYSCLK / D1CPRE + * Subject to + * + * PLLP1 = {2, 4, 6, 8, ..., 128} + * PLLP2,3 = {2, 3, 4, ..., 128} + * CPUCLK <= 480 MHz + */ +#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE + +/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR + * + * PLL1_VCO = (24,000,000 / 2) * 80 = 960 MHz + * + * PLL1P = PLL1_VCO/2 = 960 MHz / 2 = 480 MHz + * PLL1Q = PLL1_VCO/4 = 960 MHz / 4 = 240 MHz + * PLL1R = PLL1_VCO/8 = 960 MHz / 8 = 120 MHz + */ +#define STM32_PLLCFG_PLL1CFG (RCC_PLLCFGR_PLL1VCOSEL_WIDE|RCC_PLLCFGR_PLL1RGE_4_8_MHZ|RCC_PLLCFGR_DIVP1EN|RCC_PLLCFGR_DIVQ1EN|RCC_PLLCFGR_DIVR1EN) +#define STM32_PLLCFG_PLL1M RCC_PLLCKSELR_DIVM1(2) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(80) +#define STM32_PLLCFG_PLL1P RCC_PLL1DIVR_P1(2) +#define STM32_PLLCFG_PLL1Q RCC_PLL1DIVR_Q1(4) +#define STM32_PLLCFG_PLL1R RCC_PLL1DIVR_R1(8) + +#define STM32_VCO1_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 80) +#define STM32_PLL1P_FREQUENCY (STM32_VCO1_FREQUENCY / 2) +#define STM32_PLL1Q_FREQUENCY (STM32_VCO1_FREQUENCY / 4) +#define STM32_PLL1R_FREQUENCY (STM32_VCO1_FREQUENCY / 8) + +/* PLL2 */ +#define STM32_PLLCFG_PLL2CFG (RCC_PLLCFGR_PLL2VCOSEL_WIDE|RCC_PLLCFGR_PLL2RGE_4_8_MHZ|RCC_PLLCFGR_DIVP2EN|RCC_PLLCFGR_DIVQ2EN|RCC_PLLCFGR_DIVR2EN) +#define STM32_PLLCFG_PLL2M RCC_PLLCKSELR_DIVM2(4) +#define STM32_PLLCFG_PLL2N RCC_PLL2DIVR_N2(32) +#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(2) +#define STM32_PLLCFG_PLL2Q RCC_PLL2DIVR_Q2(2) +#define STM32_PLLCFG_PLL2R RCC_PLL2DIVR_R2(2) + +#define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 32) +#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 2) + +/* PLL3 */ +#define STM32_PLLCFG_PLL3CFG (RCC_PLLCFGR_PLL3VCOSEL_WIDE|RCC_PLLCFGR_PLL3RGE_4_8_MHZ|RCC_PLLCFGR_DIVQ3EN) +#define STM32_PLLCFG_PLL3M RCC_PLLCKSELR_DIVM3(4) +#define STM32_PLLCFG_PLL3N RCC_PLL3DIVR_N3(32) +#define STM32_PLLCFG_PLL3P RCC_PLL3DIVR_P3(2) +#define STM32_PLLCFG_PLL3Q RCC_PLL3DIVR_Q3(4) +#define STM32_PLLCFG_PLL3R RCC_PLL3DIVR_R3(2) + +#define STM32_VCO3_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 32) +#define STM32_PLL3P_FREQUENCY (STM32_VCO3_FREQUENCY / 2) +#define STM32_PLL3Q_FREQUENCY (STM32_VCO3_FREQUENCY / 4) +#define STM32_PLL3R_FREQUENCY (STM32_VCO3_FREQUENCY / 2) + +/* SYSCLK = PLL1P = 480MHz + * CPUCLK = SYSCLK / 1 = 480 MHz + */ +#define STM32_RCC_D1CFGR_D1CPRE (RCC_D1CFGR_D1CPRE_SYSCLK) +#define STM32_SYSCLK_FREQUENCY (STM32_PLL1P_FREQUENCY) +#define STM32_CPUCLK_FREQUENCY (STM32_SYSCLK_FREQUENCY / 1) + +/* Configure Clock Assignments */ + +/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max) + * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240 + */ +#define STM32_RCC_D1CFGR_HPRE RCC_D1CFGR_HPRE_SYSCLKd2 /* HCLK = SYSCLK / 2 */ +#define STM32_ACLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* ACLK in D1, HCLK3 in D1 */ +#define STM32_HCLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* HCLK in D2, HCLK4 in D3 */ +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */ +#define STM32_RCC_D2CFGR_D2PPRE1 RCC_D2CFGR_D2PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB2 clock (PCLK2) is HCLK/2 (120 MHz) */ +#define STM32_RCC_D2CFGR_D2PPRE2 RCC_D2CFGR_D2PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB3 clock (PCLK3) is HCLK/2 (120 MHz) */ +#define STM32_RCC_D1CFGR_D1PPRE RCC_D1CFGR_D1PPRE_HCLKd2 /* PCLK3 = HCLK / 2 */ +#define STM32_PCLK3_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB4 clock (PCLK4) is HCLK/4 (120 MHz) */ +#define STM32_RCC_D3CFGR_D3PPRE RCC_D3CFGR_D3PPRE_HCLKd2 /* PCLK4 = HCLK / 2 */ +#define STM32_PCLK4_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timer clock frequencies */ + +/* Timers driven from APB1 will be twice PCLK1 */ +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timers driven from APB2 will be twice PCLK2 */ +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM15_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM16_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM17_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Kernel Clock Configuration + * Note: look at Table 54 in ST Manual + */ +#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI /* I2C123 clock source */ +#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 /* SPI123 clock source */ +#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 /* SPI45 clock source */ +#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 /* SPI6 clock source */ +#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */ +#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ +#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ + +#define STM32_FDCANCLK STM32_HSE_FREQUENCY + +/* FLASH wait states */ +#define BOARD_FLASH_WAITSTATES 2 + +/* SDMMC definitions ********************************************************/ +/* Init 400kHz, freq = PLL1Q/(2*div) div = PLL1Q/(2*freq) */ +#define STM32_SDMMC_INIT_CLKDIV (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) + +/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div = PLL1Q/(2*freq) + * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s + */ +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_MMCXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_MMCXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_SDXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_SDXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +#define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE + + +/* UART/USART */ +#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */ +#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 /* PD4 */ +#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */ + +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 /* PD12 */ +#define GPIO_USART3_CTS GPIO_USART3_CTS_NSS_2 /* PD11 */ + +#define GPIO_UART4_RX GPIO_UART4_RX_1 /* PA1 */ +#define GPIO_UART4_TX GPIO_UART4_TX_1 /* PA0 */ + +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ +#define GPIO_USART6_TX GPIO_USART6_TX_2 /* PG14 */ + +#define GPIO_UART7_RX GPIO_UART7_RX_1 /* PE7 */ +#define GPIO_UART7_TX GPIO_UART7_TX_1 /* PE8 */ + +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ + + +/* CAN */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 /* PD0 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 /* PD1 */ +#define GPIO_CAN2_RX GPIO_CAN2_RX_1 /* PB12 */ +#define GPIO_CAN2_TX GPIO_CAN2_TX_1 /* PB13 */ + + +/* SPI */ +#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) + +#define GPIO_SPI1_SCK ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5 */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 /* PA7 */ + +#define GPIO_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_3) /* PB10 */ +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 /* PB14 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 /* PB15 */ + +#define GPIO_SPI5_SCK ADJ_SLEW_RATE(GPIO_SPI5_SCK_1) /* PF7 */ +#define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */ +#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_1 /* PF9 */ + + +/* I2C */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */ + +#define GPIO_I2C3_SCL GPIO_I2C3_SCL_2 /* PH7 */ +#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */ + +#define GPIO_I2C4_SCL GPIO_I2C4_SCL_2 /* PF14 */ +#define GPIO_I2C4_SDA GPIO_I2C4_SDA_2 /* PF15 */ diff --git a/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board_dma_map.h b/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board_dma_map.h new file mode 100644 index 0000000000..10011ed371 --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board_dma_map.h @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// DMAMUX1 +#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* DMA1:37 */ +#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* DMA1:38 */ + +#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_1 /* DMA1:71 */ +#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_1 /* DMA1:72 */ + +#define DMAMAP_SPI5_RX DMAMAP_DMA12_SPI5RX_0 /* DMA1:83 */ +#define DMAMAP_SPI5_TX DMAMAP_DMA12_SPI5TX_0 /* DMA1:84 */ diff --git a/boards/holybro/durandal-v1/nuttx-config/stackcheck/defconfig b/boards/mro/ctrl-zero-h7-oem/nuttx-config/nsh/defconfig similarity index 93% rename from boards/holybro/durandal-v1/nuttx-config/stackcheck/defconfig rename to boards/mro/ctrl-zero-h7-oem/nuttx-config/nsh/defconfig index 0c9a38350f..6fd0a5591c 100644 --- a/boards/holybro/durandal-v1/nuttx-config/stackcheck/defconfig +++ b/boards/mro/ctrl-zero-h7-oem/nuttx-config/nsh/defconfig @@ -36,7 +36,6 @@ CONFIG_ARMV7M_DCACHE=y CONFIG_ARMV7M_DTCM=y CONFIG_ARMV7M_ICACHE=y CONFIG_ARMV7M_MEMCPY=y -CONFIG_ARMV7M_STACKCHECK=y CONFIG_ARMV7M_USEBASEPRI=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_CRASHDUMP=y @@ -45,12 +44,12 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y -CONFIG_CDCACM_PRODUCTID=0x004b -CONFIG_CDCACM_PRODUCTSTR="PX4 DurandalV1" +CONFIG_CDCACM_PRODUCTID=0x1024 +CONFIG_CDCACM_PRODUCTSTR="mRoControlZeroH7 OEM" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 -CONFIG_CDCACM_VENDORID=0x3162 -CONFIG_CDCACM_VENDORSTR="Holybro" +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="mRo" CONFIG_CLOCK_MONOTONIC=y CONFIG_DEBUG_FULLOPT=y CONFIG_DEBUG_HARDFAULT_ALERT=y @@ -60,6 +59,7 @@ CONFIG_DEFAULT_SMALL=y CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 +CONFIG_DISABLE_MQUEUE=y CONFIG_EXPERIMENTAL=y CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y @@ -127,6 +127,8 @@ CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y @@ -164,7 +166,6 @@ CONFIG_STM32H7_DTCMEXCLUDE=y CONFIG_STM32H7_DTCM_PROCFS=y CONFIG_STM32H7_FLOWCONTROL_BROKEN=y CONFIG_STM32H7_I2C1=y -CONFIG_STM32H7_I2C2=y CONFIG_STM32H7_I2C3=y CONFIG_STM32H7_I2C4=y CONFIG_STM32H7_I2C_DYNTIMEO=y @@ -172,6 +173,7 @@ CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 CONFIG_STM32H7_OTGFS=y CONFIG_STM32H7_PROGMEM=y CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_HSECLOCK=y CONFIG_STM32H7_RTC_MAGIC_REG=1 CONFIG_STM32H7_SAVE_CRASHDUMP=y CONFIG_STM32H7_SDMMC1=y @@ -181,20 +183,19 @@ CONFIG_STM32H7_SPI1=y CONFIG_STM32H7_SPI1_DMA=y CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 CONFIG_STM32H7_SPI2=y -CONFIG_STM32H7_SPI4=y CONFIG_STM32H7_SPI5=y -CONFIG_STM32H7_SPI6=y -CONFIG_STM32H7_SPI6_DMA=y -CONFIG_STM32H7_SPI6_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI5_DMA=y +CONFIG_STM32H7_SPI5_DMA_BUFFER=1024 CONFIG_STM32H7_SPI_DMA=y CONFIG_STM32H7_SPI_DMATHRESHOLD=8 CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM2=y CONFIG_STM32H7_TIM3=y CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM8=y CONFIG_STM32H7_UART4=y CONFIG_STM32H7_UART7=y CONFIG_STM32H7_UART8=y -CONFIG_STM32H7_USART1=y CONFIG_STM32H7_USART2=y CONFIG_STM32H7_USART3=y CONFIG_STM32H7_USART6=y @@ -212,14 +213,11 @@ CONFIG_UART4_RXBUFSIZE=600 CONFIG_UART4_TXBUFSIZE=1500 CONFIG_UART7_BAUD=57600 CONFIG_UART7_RXBUFSIZE=600 -CONFIG_UART7_SERIAL_CONSOLE=y CONFIG_UART7_TXBUFSIZE=1500 CONFIG_UART8_BAUD=57600 CONFIG_UART8_RXBUFSIZE=600 CONFIG_UART8_TXBUFSIZE=1500 -CONFIG_USART1_BAUD=57600 -CONFIG_USART1_RXBUFSIZE=600 -CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_UART8_SERIAL_CONSOLE=y CONFIG_USART2_BAUD=57600 CONFIG_USART2_IFLOWCONTROL=y CONFIG_USART2_OFLOWCONTROL=y @@ -239,4 +237,3 @@ CONFIG_USBDEV_MAXPOWER=500 CONFIG_USEC_PER_TICK=1000 CONFIG_USERMAIN_STACKSIZE=2944 CONFIG_USER_ENTRYPOINT="nsh_main" -CONFIG_WATCHDOG=y diff --git a/boards/mro/ctrl-zero-h7-oem/nuttx-config/scripts/bootloader_script.ld b/boards/mro/ctrl-zero-h7-oem/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 0000000000..3fb4cc1f33 --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,221 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2020 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The board uses an STM32H743II has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * There's a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 128K + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > AXI_SRAM AT > FLASH + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } + + .ramfunc : { + _sramfuncs = .; + *(.ramfunc .ramfunc.*) + . = ALIGN(4); + _eramfuncs = .; + } > ITCM_RAM AT > FLASH + + _framfuncs = LOADADDR(.ramfunc); +} diff --git a/boards/mro/ctrl-zero-h7-oem/nuttx-config/scripts/script.ld b/boards/mro/ctrl-zero-h7-oem/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..c63b08f931 --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/nuttx-config/scripts/script.ld @@ -0,0 +1,229 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2021 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The board uses an STM32H743XIH6 and has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * There's a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1920K + + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > AXI_SRAM AT > FLASH + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Emit the the D3 power domain section for locating BDMA data */ + .SRAM4 (NOLOAD) : + { + } > SRAM4 + + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } + + .ramfunc : { + _sramfuncs = .; + *(.ramfunc .ramfunc.*) + . = ALIGN(4); + _eramfuncs = .; + } > ITCM_RAM AT > FLASH + + _framfuncs = LOADADDR(.ramfunc); +} diff --git a/boards/mro/ctrl-zero-h7-oem/src/CMakeLists.txt b/boards/mro/ctrl-zero-h7-oem/src/CMakeLists.txt new file mode 100644 index 0000000000..8e734c6fca --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/src/CMakeLists.txt @@ -0,0 +1,64 @@ +############################################################################ +# +# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_library(drivers_board + bootloader_main.c + usb.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader) + +else() + add_library(drivers_board + i2c.cpp + init.c + led.c + spi.cpp + timer_config.cpp + usb.c + ) + + target_link_libraries(drivers_board + PRIVATE + arch_spi + drivers__led + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/mro/ctrl-zero-h7-oem/src/board_config.h b/boards/mro/ctrl-zero-h7-oem/src/board_config.h new file mode 100644 index 0000000000..6538ff0d8a --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/src/board_config.h @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * Board internal definitions + */ + +#pragma once + +#include +#include +#include +#include + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ +#define GPIO_nLED_RED /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) +#define GPIO_nLED_GREEN /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) +#define GPIO_nLED_BLUE /* PB3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_STATE_LED LED_BLUE + +/* ADC channels */ +#define PX4_ADC_GPIO \ + /* PA2 */ GPIO_ADC12_INP14, \ + /* PA3 */ GPIO_ADC12_INP15, \ + /* PA4 */ GPIO_ADC12_INP18, \ + /* PC1 */ GPIO_ADC123_INP11 + +/* Define Channel numbers must match above GPIO pins */ +#define ADC_BATTERY_VOLTAGE_CHANNEL 14 /* PA2 BATT_VOLT_SENS */ +#define ADC_BATTERY_CURRENT_CHANNEL 15 /* PA3 BATT_CURRENT_SENS */ +#define ADC_SCALED_V5_CHANNEL 18 /* PA4 VDD_5V_SENS */ +#define ADC_RC_RSSI_CHANNEL 11 /* PC1 */ + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL) | \ + (1 << ADC_RC_RSSI_CHANNEL)) + +/* HW has to large of R termination on ADC todo:change when HW value is chosen */ +#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) + +/* CAN Silence: Silent mode control */ +#define GPIO_CAN1_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) +#define GPIO_CAN2_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) + +/* PWM */ +#define DIRECT_PWM_OUTPUT_CHANNELS 8 +#define DIRECT_INPUT_TIMER_CHANNELS 8 + +/* Power supply control and monitoring GPIOs */ +#define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) + +#define GPIO_VDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ +#define BOARD_NUMBER_BRICKS 1 + +#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) + +/* Define True logic Power Control in arch agnostic form */ +#define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true)) +#define READ_VDD_3V3_SPEKTRUM_POWER_EN() (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0) + +/* Tone alarm output */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* PA15 TIM2_CH1 */ + +#define GPIO_BUZZER_1 /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2 + +/* USB OTG FS */ +#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 3 /* use timer3 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +#define HRT_PPM_CHANNEL /* T3C3 */ 3 /* use capture/compare channel 3 */ +#define GPIO_PPM_IN /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1 + +/* RC Serial port */ +#define RC_SERIAL_PORT "/dev/ttyS3" + +#define GPIO_RSSI_IN /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) + +/* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */ +#define GPIO_SAFETY_SWITCH_IN /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */ +#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */ + +/* Power switch controls ******************************************************/ +#define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true) + +/* + * Board has a separate RC_IN + * + * GPIO PPM_IN on PB0 T3CH3 + * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7 + * Inversion is possible in the UART and can drive GPIO_PPM_IN as an output + */ +#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT) +#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */ +#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true)) + +/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) + * this board support the ADC system_power interface, and therefore + * provides the true logic GPIO BOARD_ADC_xxxx macros. + */ +#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) +#define BOARD_ADC_USB_VALID BOARD_ADC_USB_CONNECTED +#define BOARD_ADC_SERVO_VALID (1) /* never powers off the Servo rail */ +#define BOARD_ADC_BRICK_VALID (px4_arch_gpioread(GPIO_VDD_BRICK1_VALID)) + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ +#define BOARD_HAS_ON_RESET 1 + +#define BOARD_HAS_STATIC_MANIFEST 1 + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + +#define BOARD_NUM_IO_TIMERS 3 + +#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN2_TX, \ + GPIO_CAN2_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_CAN2_SILENT_S0, \ + GPIO_nPOWER_IN_A, \ + GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_SAFETY_SWITCH_IN, \ + GPIO_OTGFS_VBUS, \ + } + +__BEGIN_DECLS +#ifndef __ASSEMBLY__ + +extern void stm32_spiinitialize(void); +extern void board_peripheral_reset(int ms); + +#include +#endif /* __ASSEMBLY__ */ +__END_DECLS diff --git a/boards/mro/ctrl-zero-h7-oem/src/bootloader_main.c b/boards/mro/ctrl-zero-h7-oem/src/bootloader_main.c new file mode 100644 index 0000000000..0a481dcc19 --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/src/bootloader_main.c @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include +#include "arm_internal.h" +#include + +extern int sercon_main(int c, char **argv); + +__EXPORT void board_on_reset(int status) {} + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure USB interfaces */ + stm32_configgpio(GPIO_OTGFS_VBUS); +} + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + return 0; +} + +void board_late_initialize(void) +{ + px4_platform_console_init(); + sercon_main(0, NULL); +} + +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/mro/ctrl-zero-h7-oem/src/hw_config.h b/boards/mro/ctrl-zero-h7-oem/src/hw_config.h new file mode 100644 index 0000000000..8fdaec9e7e --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/src/hw_config.h @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +/**************************************************************************** + * 10-8--2016: + * To simplify the ripple effect on the tools, we will be using + * /dev/serial/by-id/PX4 to locate PX4 devices. Therefore + * moving forward all Bootloaders must contain the prefix "PX4 BL " + * in the USBDEVICESTRING + * This Change will be made in an upcoming BL release + ****************************************************************************/ +/* + * Define usage to configure a bootloader + * + * + * Constant example Usage + * APP_LOAD_ADDRESS 0x08004000 - The address in Linker Script, where the app fw is org-ed + * BOOTLOADER_DELAY 5000 - Ms to wait while under USB pwr or bootloader request + * BOARD_FMUV2 + * INTERFACE_USB 1 - (Optional) Scan and use the USB interface for bootloading + * INTERFACE_USART 1 - (Optional) Scan and use the Serial interface for bootloading + * USBDEVICESTRING "PX4 BL FMU v2.x" - USB id string + * USBPRODUCTID 0x0011 - PID Should match defconfig + * BOOT_DELAY_ADDRESS 0x000001a0 - (Optional) From the linker script from Linker Script to get a custom + * delay provided by an APP FW + * BOARD_TYPE 9 - Must match .prototype boad_id + * _FLASH_KBYTES (*(uint16_t *)0x1fff7a22) - Run time flash size detection + * BOARD_FLASH_SECTORS ((_FLASH_KBYTES == 0x400) ? 11 : 23) - Run time determine the physical last sector + * BOARD_FLASH_SECTORS 11 - Hard coded zero based last sector + * BOARD_FLASH_SIZE (_FLASH_KBYTES*1024)- Total Flash size of device, determined at run time. + * (1024 * 1024) - Hard coded Total Flash of device - The bootloader and app reserved will be deducted + * programmatically + * + * BOARD_FIRST_FLASH_SECTOR_TO_ERASE 2 - Optional sectors index in the flash_sectors table (F4 only), to begin erasing. + * This is to allow sectors to be reserved for app fw usage. That will NOT be erased + * during a FW upgrade. + * The default is 0, and selects the first sector to be erased, as the 0th entry in the + * flash_sectors table. Which is the second physical sector of FLASH in the device. + * The first physical sector of FLASH is used by the bootloader, and is not defined + * in the table. + * + * APP_RESERVATION_SIZE (BOARD_FIRST_FLASH_SECTOR_TO_ERASE * 16 * 1024) - Number of bytes reserved by the APP FW. This number plus + * BOOTLOADER_RESERVATION_SIZE will be deducted from + * BOARD_FLASH_SIZE to determine the size of the App FW + * and hence the address space of FLASH to erase and program. + * USBMFGSTRING "PX4 AP" - Optional USB MFG string (default is '3D Robotics' if not defined.) + * SERIAL_BREAK_DETECT_DISABLED - Optional prevent break selection on Serial port from entering or staying in BL + * + * * Other defines are somewhat self explanatory. + */ + +/* Boot device selection list*/ +#define USB0_DEV 0x01 +#define SERIAL0_DEV 0x02 +#define SERIAL1_DEV 0x04 + +#define APP_LOAD_ADDRESS 0x08020000 +#define BOOTLOADER_DELAY 5000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" +#define BOARD_VBUS MK_GPIO_INPUT(GPIO_OTGFS_VBUS) + +//#define USE_VBUS_PULL_DOWN +#define INTERFACE_USART 1 +#define INTERFACE_USART_CONFIG "/dev/ttyS0,115200" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 1024 +#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) +#define BOARD_FLASH_SECTORS (15) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 1024) + +#define OSC_FREQ 24 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_RED +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_RED +#define BOARD_LED_ON 0 +#define BOARD_LED_OFF 1 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 12 +#endif + +#if !defined(APP_RESERVATION_SIZE) +# define APP_RESERVATION_SIZE 0 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1 +#endif + +#if !defined(USB_DATA_ALIGN) +# define USB_DATA_ALIGN +#endif + +#ifndef BOOT_DEVICES_SELECTION +# define BOOT_DEVICES_SELECTION USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#ifndef BOOT_DEVICES_FILTER_ONUSB +# define BOOT_DEVICES_FILTER_ONUSB USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif diff --git a/boards/mro/ctrl-zero-h7-oem/src/i2c.cpp b/boards/mro/ctrl-zero-h7-oem/src/i2c.cpp new file mode 100644 index 0000000000..de6742ac58 --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/src/i2c.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusInternal(3), + initI2CBusExternal(4), +}; diff --git a/boards/mro/ctrl-zero-h7-oem/src/init.c b/boards/mro/ctrl-zero-h7-oem/src/init.c new file mode 100644 index 0000000000..0c8f4cf157 --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/src/init.c @@ -0,0 +1,202 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file init.c + * + * board-specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +#include "board_config.h" + +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include + +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + bool last = READ_VDD_3V3_SPEKTRUM_POWER_EN(); + /* Keep Spektum on to discharge rail*/ + VDD_3V3_SPEKTRUM_POWER_EN(false); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + syslog(LOG_DEBUG, "reset done, %d ms\n", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + VDD_3V3_SPEKTRUM_POWER_EN(last); +} + +/************************************************************************************ + * Name: board_on_reset + * + * Description: + * Optionally provided function called on entry to board_system_reset + * It should perform any house keeping prior to the rest. + * + * status - 1 if resetting to boot loader + * 0 if just resetting + * + ************************************************************************************/ +__EXPORT void board_on_reset(int status) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ +__EXPORT void stm32_boardinitialize(void) +{ + /* Reset PWM first thing */ + board_on_reset(-1); + + /* configure pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + board_control_spi_sensors_power_configgpio(); + + /* configure LEDs */ + board_autoled_initialize(); +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ +__EXPORT int board_app_initialize(uintptr_t arg) +{ + /* Power on Interfaces */ + board_control_spi_sensors_power(true, 0xffff); + VDD_3V3_SPEKTRUM_POWER_EN(true); + + px4_platform_init(); + + stm32_spiinitialize(); + + /* configure the DMA allocator */ + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_on(LED_GREEN); // Indicate Power. + led_off(LED_BLUE); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + +#ifdef CONFIG_MMCSD + /* Mount the SDIO-based MMC/SD block driver */ + /* First, get an instance of the SDIO interface */ + struct sdio_dev_s *sdio_dev = sdio_initialize(0); // SDIO_SLOTNO 0 Only one slot + + if (!sdio_dev) { + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", 0); + return ERROR; + } + + if (mmcsd_slotinitialize(0, sdio_dev) != OK) { + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver\n"); + return ERROR; + } + + /* Assume that the SD card is inserted. What choice do we have? */ + sdio_mediachange(sdio_dev, true); +#endif /* CONFIG_MMCSD */ + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + return OK; +} diff --git a/boards/mro/ctrl-zero-h7-oem/src/led.c b/boards/mro/ctrl-zero-h7-oem/src/led.c new file mode 100644 index 0000000000..0e7beb3a9e --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/src/led.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file led.c + * + * LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +static uint32_t g_ledmap[] = { + GPIO_nLED_BLUE, // Indexed by LED_BLUE + GPIO_nLED_RED, // Indexed by LED_RED + GPIO_nLED_GREEN, // Indexed by LED_GREEN +}; + +__EXPORT void led_init(void) +{ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + if (g_ledmap[l] != 0) { + stm32_configgpio(g_ledmap[l]); + } + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + if (g_ledmap[led] != 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } +} + +static bool phy_get_led(int led) +{ + /* If Low it is on */ + if (g_ledmap[led] != 0) { + return !stm32_gpioread(g_ledmap[led]); + } + + return false; +} + +__EXPORT void led_on(int led) +{ + phy_set_led(led, true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(led, !phy_get_led(led)); +} diff --git a/boards/mro/ctrl-zero-h7-oem/src/spi.cpp b/boards/mro/ctrl-zero-h7-oem/src/spi.cpp new file mode 100644 index 0000000000..4a4c3502bb --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/src/spi.cpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}), + initSPIDevice(DRV_BARO_DEVTYPE_DPS310, SPI::CS{GPIO::PortD, GPIO::Pin7}), + }), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/mro/ctrl-zero-h7-oem/src/timer_config.cpp b/boards/mro/ctrl-zero-h7-oem/src/timer_config.cpp new file mode 100644 index 0000000000..e238804914 --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/src/timer_config.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer1, DMA{DMA::Index1}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1}), + initIOTimer(Timer::Timer8, DMA{DMA::Index1}), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel1}, {GPIO::PortI, GPIO::Pin5}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel2}, {GPIO::PortI, GPIO::Pin6}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/mro/ctrl-zero-h7-oem/src/usb.c b/boards/mro/ctrl-zero-h7-oem/src/usb.c new file mode 100644 index 0000000000..e60a93bb59 --- /dev/null +++ b/boards/mro/ctrl-zero-h7-oem/src/usb.c @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file usb.c + * + * Board-specific USB functions. + */ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32H7_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); +#endif +} + + + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/boards/mro/ctrl-zero-h7/bootloader.cmake b/boards/mro/ctrl-zero-h7/bootloader.cmake new file mode 100644 index 0000000000..0679706aa2 --- /dev/null +++ b/boards/mro/ctrl-zero-h7/bootloader.cmake @@ -0,0 +1,9 @@ + +px4_add_board( + PLATFORM nuttx + VENDOR mro + MODEL ctrl-zero-h7 + LABEL bootloader + TOOLCHAIN arm-none-eabi + ARCHITECTURE cortex-m7 +) diff --git a/boards/px4/fmu-v5/critmonitor.cmake b/boards/mro/ctrl-zero-h7/default.cmake similarity index 82% rename from boards/px4/fmu-v5/critmonitor.cmake rename to boards/mro/ctrl-zero-h7/default.cmake index 48d0368bbd..0f6c9ae959 100644 --- a/boards/px4/fmu-v5/critmonitor.cmake +++ b/boards/mro/ctrl-zero-h7/default.cmake @@ -1,23 +1,26 @@ px4_add_board( PLATFORM nuttx - VENDOR px4 - MODEL fmu-v5 - LABEL critmonitor + VENDOR mro + MODEL ctrl-zero-h7 + LABEL default TOOLCHAIN arm-none-eabi ARCHITECTURE cortex-m7 ROMFSROOT px4fmu_common - IO px4_io-v2_default + BUILD_BOOTLOADER TESTING - UAVCAN_INTERFACES 2 + UAVCAN_INTERFACES 1 SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS3 + TEL1:/dev/ttyS0 + TEL2:/dev/ttyS1 + GPS1:/dev/ttyS2 + #RC:/dev/ttyS3 + TEL3:/dev/ttyS4 + #FRSKY:/dev/ttyS5 DRIVERS adc/board_adc - barometer # all available barometer drivers + #barometer # all available barometer drivers + barometer/dps310 batt_smbus camera_capture camera_trigger @@ -25,33 +28,28 @@ px4_add_board( distance_sensor # all available distance sensor drivers dshot gps - #heater #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/adis16477 - imu/adis16497 - imu/bosch/bmi055 + imu/bosch/bmi088 imu/invensense/icm20602 - imu/invensense/icm20689 - #imu/mpu6000 # legacy icm20602/icm20689 driver + imu/invensense/icm20948 irlock lights/blinkm lights/rgbled lights/rgbled_ncp5623c - lights/rgbled_pwm magnetometer # all available magnetometer drivers + mkblctrl optical_flow # all available optical flow drivers #osd pca9685 power_monitor/ina226 #protocol_splitter - pwm_input + #pwm_input pwm_out_sim pwm_out - px4io rc_input roboclaw - safety_button + rpm + #safety_button TODO tap_esc telemetry # all available telemetry drivers test_ppm @@ -65,6 +63,7 @@ px4_add_board( commander dataman ekf2 + esc_battery events flight_mode_manager fw_att_control @@ -93,6 +92,7 @@ px4_add_board( dmesg dumpfile esc_calib + gpio hardfault_log i2cdetect led_control @@ -117,6 +117,7 @@ px4_add_board( ver work_queue EXAMPLES + fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/mro/ctrl-zero-h7/firmware.prototype b/boards/mro/ctrl-zero-h7/firmware.prototype new file mode 100644 index 0000000000..4afdcf7ef2 --- /dev/null +++ b/boards/mro/ctrl-zero-h7/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1024, + "magic": "mRo-ctrl-zero-h7", + "description": "Firmware for the mRo-ctrl-zero-h7 board", + "image": "", + "build_time": 0, + "summary": "mRo-ctrl-zero-h7", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1966080, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/mro/ctrl-zero-h7/init/rc.board_defaults b/boards/mro/ctrl-zero-h7/init/rc.board_defaults new file mode 100644 index 0000000000..98136ea19e --- /dev/null +++ b/boards/mro/ctrl-zero-h7/init/rc.board_defaults @@ -0,0 +1,42 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +# +# Bootloader upgrade +# +set BL_FILE /etc/extras/mro_ctrl-zero-h7_bootloader.bin +if [ -f $BL_FILE ] +then + if param compare SYS_BL_UPDATE 1 + then + param set SYS_BL_UPDATE 0 + param save + echo "BL update..." + bl_update $BL_FILE + echo "BL update done" + reboot + fi +fi +unset BL_FILE + + +param set-default BAT_V_DIV 10.1 +param set-default BAT1_V_DIV 10.1 + +param set-default BAT_A_PER_V 24 +param set-default BAT1_A_PER_V 24 + +# Multi-EKF +param set-default EKF2_MULTI_IMU 3 +param set-default SENS_IMU_MODE 0 +param set-default EKF2_MULTI_MAG 3 +param set-default SENS_MAG_MODE 0 + +param set-default UAVCAN_ENABLE 2 + + +set LOGGER_BUF 64 + +safety_button start diff --git a/boards/mro/ctrl-zero-h7/init/rc.board_mavlink b/boards/mro/ctrl-zero-h7/init/rc.board_mavlink new file mode 100644 index 0000000000..08fed10e71 --- /dev/null +++ b/boards/mro/ctrl-zero-h7/init/rc.board_mavlink @@ -0,0 +1,7 @@ +#!/bin/sh +# +# Board specific MAVLink startup script. +#------------------------------------------------------------------------------ + +# Start MAVLink on the USB port +mavlink start -d /dev/ttyACM0 diff --git a/boards/mro/ctrl-zero-h7/init/rc.board_sensors b/boards/mro/ctrl-zero-h7/init/rc.board_sensors new file mode 100644 index 0000000000..111e3eb044 --- /dev/null +++ b/boards/mro/ctrl-zero-h7/init/rc.board_sensors @@ -0,0 +1,18 @@ +#!/bin/sh +# +# board specific sensors init +#------------------------------------------------------------------------------ +board_adc start + +# Internal ICM-20602 +icm20602 -s -b 1 -R 8 start + +# Internal SPI bus BMI088 accel & gyro +bmi088 -A -s -b 5 -R 8 start +bmi088 -G -s -b 5 -R 8 start + +# Internal ICM-20948 (with magnetometer) +icm20948 -s -b 1 -R 8 -M start + +# Interal DPS310 (barometer) +dps310 -s -b 2 start diff --git a/boards/mro/ctrl-zero-h7/nuttx-config/bootloader/defconfig b/boards/mro/ctrl-zero-h7/nuttx-config/bootloader/defconfig new file mode 100644 index 0000000000..6eb96642e4 --- /dev/null +++ b/boards/mro/ctrl-zero-h7/nuttx-config/bootloader/defconfig @@ -0,0 +1,95 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DEV_CONSOLE is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_SPI_EXCHANGE is not set +# CONFIG_STM32H7_SYSCFG is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_PRODUCTID=0x1023 +CONFIG_CDCACM_PRODUCTSTR="PX4 BL mRo ControlZeroH7" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="mRo" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_PTHREAD=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIB_BOARDCTL=y +CONFIG_MAX_TASKS=8 +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MM_REGIONS=3 +CONFIG_NFILE_DESCRIPTORS=5 +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SPI=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_USART3=y +CONFIG_SYSTEMTICK_HOOK=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGINT_CHAR=0x03 +CONFIG_TTY_SIGSTP=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="bootloader_main" diff --git a/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h b/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h new file mode 100644 index 0000000000..7dbde2d7d4 --- /dev/null +++ b/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h @@ -0,0 +1,270 @@ +/************************************************************************************ + * nuttx-config/include/board.h + * + * Copyright (C) 2020 Gregory Nutt. All rights reserved. + * Authors: David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ +#pragma once + +#include "board_dma_map.h" + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/* Clocking *************************************************************************/ +/* The board provides the following clock sources: + * + * X1: 24 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 16 MHz RC factory-trimmed internal oscillator + * HSE: 24 MHz crystal for HSE + */ +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 0 + +/* Main PLL Configuration. + * + * PLL source is HSE = 24,000,000 + * + * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 1 <= PLLM <= 63 + * 4 <= PLLN <= 512 + * 150 MHz <= PLL_VCOL <= 420MHz + * 192 MHz <= PLL_VCOH <= 836MHz + * + * SYSCLK = PLL_VCO / PLLP + * CPUCLK = SYSCLK / D1CPRE + * Subject to + * + * PLLP1 = {2, 4, 6, 8, ..., 128} + * PLLP2,3 = {2, 3, 4, ..., 128} + * CPUCLK <= 480 MHz + */ +#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE + +/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR + * + * PLL1_VCO = (24,000,000 / 2) * 80 = 960 MHz + * + * PLL1P = PLL1_VCO/2 = 960 MHz / 2 = 480 MHz + * PLL1Q = PLL1_VCO/4 = 960 MHz / 4 = 240 MHz + * PLL1R = PLL1_VCO/8 = 960 MHz / 8 = 120 MHz + */ +#define STM32_PLLCFG_PLL1CFG (RCC_PLLCFGR_PLL1VCOSEL_WIDE|RCC_PLLCFGR_PLL1RGE_4_8_MHZ|RCC_PLLCFGR_DIVP1EN|RCC_PLLCFGR_DIVQ1EN|RCC_PLLCFGR_DIVR1EN) +#define STM32_PLLCFG_PLL1M RCC_PLLCKSELR_DIVM1(2) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(80) +#define STM32_PLLCFG_PLL1P RCC_PLL1DIVR_P1(2) +#define STM32_PLLCFG_PLL1Q RCC_PLL1DIVR_Q1(4) +#define STM32_PLLCFG_PLL1R RCC_PLL1DIVR_R1(8) + +#define STM32_VCO1_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 80) +#define STM32_PLL1P_FREQUENCY (STM32_VCO1_FREQUENCY / 2) +#define STM32_PLL1Q_FREQUENCY (STM32_VCO1_FREQUENCY / 4) +#define STM32_PLL1R_FREQUENCY (STM32_VCO1_FREQUENCY / 8) + +/* PLL2 */ +#define STM32_PLLCFG_PLL2CFG (RCC_PLLCFGR_PLL2VCOSEL_WIDE|RCC_PLLCFGR_PLL2RGE_4_8_MHZ|RCC_PLLCFGR_DIVP2EN|RCC_PLLCFGR_DIVQ2EN|RCC_PLLCFGR_DIVR2EN) +#define STM32_PLLCFG_PLL2M RCC_PLLCKSELR_DIVM2(4) +#define STM32_PLLCFG_PLL2N RCC_PLL2DIVR_N2(32) +#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(2) +#define STM32_PLLCFG_PLL2Q RCC_PLL2DIVR_Q2(2) +#define STM32_PLLCFG_PLL2R RCC_PLL2DIVR_R2(2) + +#define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 32) +#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 2) + +/* PLL3 */ +#define STM32_PLLCFG_PLL3CFG (RCC_PLLCFGR_PLL3VCOSEL_WIDE|RCC_PLLCFGR_PLL3RGE_4_8_MHZ|RCC_PLLCFGR_DIVQ3EN) +#define STM32_PLLCFG_PLL3M RCC_PLLCKSELR_DIVM3(4) +#define STM32_PLLCFG_PLL3N RCC_PLL3DIVR_N3(32) +#define STM32_PLLCFG_PLL3P RCC_PLL3DIVR_P3(2) +#define STM32_PLLCFG_PLL3Q RCC_PLL3DIVR_Q3(4) +#define STM32_PLLCFG_PLL3R RCC_PLL3DIVR_R3(2) + +#define STM32_VCO3_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 32) +#define STM32_PLL3P_FREQUENCY (STM32_VCO3_FREQUENCY / 2) +#define STM32_PLL3Q_FREQUENCY (STM32_VCO3_FREQUENCY / 4) +#define STM32_PLL3R_FREQUENCY (STM32_VCO3_FREQUENCY / 2) + +/* SYSCLK = PLL1P = 480MHz + * CPUCLK = SYSCLK / 1 = 480 MHz + */ +#define STM32_RCC_D1CFGR_D1CPRE (RCC_D1CFGR_D1CPRE_SYSCLK) +#define STM32_SYSCLK_FREQUENCY (STM32_PLL1P_FREQUENCY) +#define STM32_CPUCLK_FREQUENCY (STM32_SYSCLK_FREQUENCY / 1) + +/* Configure Clock Assignments */ + +/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max) + * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240 + */ +#define STM32_RCC_D1CFGR_HPRE RCC_D1CFGR_HPRE_SYSCLKd2 /* HCLK = SYSCLK / 2 */ +#define STM32_ACLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* ACLK in D1, HCLK3 in D1 */ +#define STM32_HCLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* HCLK in D2, HCLK4 in D3 */ +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */ +#define STM32_RCC_D2CFGR_D2PPRE1 RCC_D2CFGR_D2PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB2 clock (PCLK2) is HCLK/2 (120 MHz) */ +#define STM32_RCC_D2CFGR_D2PPRE2 RCC_D2CFGR_D2PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB3 clock (PCLK3) is HCLK/2 (120 MHz) */ +#define STM32_RCC_D1CFGR_D1PPRE RCC_D1CFGR_D1PPRE_HCLKd2 /* PCLK3 = HCLK / 2 */ +#define STM32_PCLK3_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB4 clock (PCLK4) is HCLK/4 (120 MHz) */ +#define STM32_RCC_D3CFGR_D3PPRE RCC_D3CFGR_D3PPRE_HCLKd2 /* PCLK4 = HCLK / 2 */ +#define STM32_PCLK4_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timer clock frequencies */ + +/* Timers driven from APB1 will be twice PCLK1 */ +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timers driven from APB2 will be twice PCLK2 */ +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM15_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM16_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM17_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Kernel Clock Configuration + * Note: look at Table 54 in ST Manual + */ +#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI /* I2C123 clock source */ +#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 /* SPI123 clock source */ +#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 /* SPI45 clock source */ +#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 /* SPI6 clock source */ +#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */ +#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ +#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ + +#define STM32_FDCANCLK STM32_HSE_FREQUENCY + +/* FLASH wait states */ +#define BOARD_FLASH_WAITSTATES 2 + +/* SDMMC definitions ********************************************************/ +/* Init 400kHz, freq = PLL1Q/(2*div) div = PLL1Q/(2*freq) */ +#define STM32_SDMMC_INIT_CLKDIV (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) + +/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div = PLL1Q/(2*freq) + * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s + */ +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_MMCXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_MMCXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_SDXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_SDXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +#define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE + + +/* UART/USART */ +#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */ +#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 /* PD4 */ +#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */ + +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 /* PD12 */ +#define GPIO_USART3_CTS GPIO_USART3_CTS_NSS_2 /* PD11 */ + +#define GPIO_UART4_RX GPIO_UART4_RX_1 /* PA1 */ +#define GPIO_UART4_TX GPIO_UART4_TX_1 /* PA0 */ + +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ +#define GPIO_USART6_TX GPIO_USART6_TX_2 /* PG14 */ + +#define GPIO_UART7_RX GPIO_UART7_RX_1 /* PE7 */ +#define GPIO_UART7_TX GPIO_UART7_TX_1 /* PE8 */ + +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ + + +/* CAN */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 /* PD0 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 /* PD1 */ + + +/* SPI */ +#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) + +#define GPIO_SPI1_SCK ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5 */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 /* PA7 */ + +#define GPIO_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_3) /* PB10 */ +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 /* PB14 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 /* PB15 */ + +#define GPIO_SPI5_SCK ADJ_SLEW_RATE(GPIO_SPI5_SCK_1) /* PF7 */ +#define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */ +#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_1 /* PF9 */ + + +/* I2C */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */ diff --git a/boards/mro/ctrl-zero-h7/nuttx-config/include/board_dma_map.h b/boards/mro/ctrl-zero-h7/nuttx-config/include/board_dma_map.h new file mode 100644 index 0000000000..10011ed371 --- /dev/null +++ b/boards/mro/ctrl-zero-h7/nuttx-config/include/board_dma_map.h @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// DMAMUX1 +#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* DMA1:37 */ +#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* DMA1:38 */ + +#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_1 /* DMA1:71 */ +#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_1 /* DMA1:72 */ + +#define DMAMAP_SPI5_RX DMAMAP_DMA12_SPI5RX_0 /* DMA1:83 */ +#define DMAMAP_SPI5_TX DMAMAP_DMA12_SPI5TX_0 /* DMA1:84 */ diff --git a/boards/px4/fmu-v6u/nuttx-config/stackcheck/defconfig b/boards/mro/ctrl-zero-h7/nuttx-config/nsh/defconfig similarity index 73% rename from boards/px4/fmu-v6u/nuttx-config/stackcheck/defconfig rename to boards/mro/ctrl-zero-h7/nuttx-config/nsh/defconfig index 91ed16e974..e266eb4b70 100644 --- a/boards/px4/fmu-v6u/nuttx-config/stackcheck/defconfig +++ b/boards/mro/ctrl-zero-h7/nuttx-config/nsh/defconfig @@ -13,9 +13,9 @@ # CONFIG_MMCSD_SPI is not set # CONFIG_NSH_DISABLEBG is not set # CONFIG_NSH_DISABLESCRIPT is not set -# CONFIG_NSH_DISABLE_ARP is not set # CONFIG_NSH_DISABLE_DF is not set # CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set # CONFIG_NSH_DISABLE_GET is not set # CONFIG_NSH_DISABLE_ITEF is not set # CONFIG_NSH_DISABLE_LOOPS is not set @@ -27,7 +27,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" CONFIG_ARCH_CHIP="stm32h7" -CONFIG_ARCH_CHIP_STM32H743ZI=y +CONFIG_ARCH_CHIP_STM32H743II=y CONFIG_ARCH_CHIP_STM32H7=y CONFIG_ARCH_INTERRUPTSTACK=512 CONFIG_ARCH_STACKDUMP=y @@ -36,21 +36,20 @@ CONFIG_ARMV7M_DCACHE=y CONFIG_ARMV7M_DTCM=y CONFIG_ARMV7M_ICACHE=y CONFIG_ARMV7M_MEMCPY=y -CONFIG_ARMV7M_STACKCHECK=y CONFIG_ARMV7M_USEBASEPRI=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_CRASHDUMP=y -CONFIG_BOARD_LOOPSPERMSEC=95751 +CONFIG_BOARD_LOOPSPERMSEC=95150 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y CONFIG_CDCACM=y -CONFIG_CDCACM_PRODUCTID=0x0036 -CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v6U.x" +CONFIG_CDCACM_PRODUCTID=0x1024 +CONFIG_CDCACM_PRODUCTSTR="mRoControlZeroH7" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 -CONFIG_CDCACM_VENDORID=0x1B8C -CONFIG_CDCACM_VENDORSTR="Gumstix" +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="mRo" CONFIG_CLOCK_MONOTONIC=y CONFIG_DEBUG_FULLOPT=y CONFIG_DEBUG_HARDFAULT_ALERT=y @@ -60,7 +59,7 @@ CONFIG_DEFAULT_SMALL=y CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 -CONFIG_ETH0_PHY_LAN8742A=y +CONFIG_DISABLE_MQUEUE=y CONFIG_EXPERIMENTAL=y CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y @@ -82,8 +81,6 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 -CONFIG_IOB_NBUFFERS=24 -CONFIG_IOB_THROTTLE=0 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y @@ -99,28 +96,7 @@ CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_PROGMEM=y CONFIG_MTD_RAMTRON=y -CONFIG_NET=y -CONFIG_NETDB_DNSCLIENT=y -CONFIG_NETDB_DNSCLIENT_ENTRIES=8 -CONFIG_NETDB_DNSSERVER_NOADDR=y -CONFIG_NETDEV_PHY_IOCTL=y -CONFIG_NETINIT_DRIPADDR=0XC0A800FE -CONFIG_NETINIT_IPADDR=0XC0A8007B -CONFIG_NETINIT_THREAD=y -CONFIG_NETINIT_THREAD_PRIORITY=49 -CONFIG_NETUTILS_TELNETD=y -CONFIG_NET_ARP_IPIN=y -CONFIG_NET_ARP_SEND=y -CONFIG_NET_BROADCAST=y -CONFIG_NET_ICMP=y -CONFIG_NET_ICMP_SOCKET=y -CONFIG_NET_SOCKOPTS=y -CONFIG_NET_SOLINGER=y -CONFIG_NET_TCP=y -CONFIG_NET_TCPBACKLOG=y -CONFIG_NET_TCP_WRITE_BUFFERS=y -CONFIG_NET_UDP=y -CONFIG_NET_UDP_CHECKSUMS=y +CONFIG_NAME_MAX=40 CONFIG_NFILE_DESCRIPTORS=12 CONFIG_NSH_ARCHINIT=y CONFIG_NSH_ARCHROMFS=y @@ -128,8 +104,11 @@ CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_DISABLE_IFCONFIG=y +CONFIG_NSH_DISABLE_IFUPDOWN=y CONFIG_NSH_DISABLE_MB=y CONFIG_NSH_DISABLE_MH=y +CONFIG_NSH_DISABLE_TELNETD=y CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 @@ -137,8 +116,6 @@ CONFIG_NSH_QUOTE=y CONFIG_NSH_ROMFSETC=y CONFIG_NSH_ROMFSSECTSIZE=128 CONFIG_NSH_STRERROR=y -CONFIG_NSH_TELNET=y -CONFIG_NSH_TELNET_LOGIN=y CONFIG_NSH_VARS=y CONFIG_PIPES=y CONFIG_PREALLOC_TIMERS=50 @@ -150,6 +127,8 @@ CONFIG_RAMTRON_WRITEWAIT=y CONFIG_RAM_SIZE=245760 CONFIG_RAM_START=0x20010000 CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y CONFIG_RTC_DATETIME=y CONFIG_SCHED_ATEXIT=y CONFIG_SCHED_HPWORK=y @@ -162,7 +141,7 @@ CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKSTACKSIZE=1632 CONFIG_SCHED_WAITPID=y CONFIG_SDCLONE_DISABLE=y -CONFIG_SDMMC2_SDIO_PULLUP=y +CONFIG_SDMMC1_SDIO_MODE=y CONFIG_SEM_NNESTPRIO=8 CONFIG_SEM_PREALLOCHOLDERS=0 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y @@ -177,7 +156,6 @@ CONFIG_START_DAY=30 CONFIG_START_MONTH=11 CONFIG_STDIO_BUFFER_SIZE=256 CONFIG_STM32H7_ADC1=y -CONFIG_STM32H7_ADC3=y CONFIG_STM32H7_BBSRAM=y CONFIG_STM32H7_BBSRAM_FILES=5 CONFIG_STM32H7_BKPSRAM=y @@ -186,52 +164,36 @@ CONFIG_STM32H7_DMA2=y CONFIG_STM32H7_DMACAPABLE=y CONFIG_STM32H7_DTCMEXCLUDE=y CONFIG_STM32H7_DTCM_PROCFS=y -CONFIG_STM32H7_ETHMAC=y -CONFIG_STM32H7_FLASH_OVERRIDE_I=y CONFIG_STM32H7_FLOWCONTROL_BROKEN=y CONFIG_STM32H7_I2C1=y -CONFIG_STM32H7_I2C2=y -CONFIG_STM32H7_I2C3=y -CONFIG_STM32H7_I2C4=y CONFIG_STM32H7_I2C_DYNTIMEO=y CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 CONFIG_STM32H7_OTGFS=y -CONFIG_STM32H7_PHYSR=31 -CONFIG_STM32H7_PHYSR_100MBPS=0x8 -CONFIG_STM32H7_PHYSR_FULLDUPLEX=0x10 -CONFIG_STM32H7_PHYSR_MODE=0x10 -CONFIG_STM32H7_PHYSR_SPEED=0x8 CONFIG_STM32H7_PROGMEM=y CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_HSECLOCK=y CONFIG_STM32H7_RTC_MAGIC_REG=1 CONFIG_STM32H7_SAVE_CRASHDUMP=y -CONFIG_STM32H7_SDMMC2=y +CONFIG_STM32H7_SDMMC1=y CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y CONFIG_STM32H7_SPI1=y CONFIG_STM32H7_SPI1_DMA=y CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 CONFIG_STM32H7_SPI2=y -CONFIG_STM32H7_SPI2_DMA=y -CONFIG_STM32H7_SPI2_DMA_BUFFER=4096 -CONFIG_STM32H7_SPI3=y -CONFIG_STM32H7_SPI3_DMA=y -CONFIG_STM32H7_SPI3_DMA_BUFFER=1024 CONFIG_STM32H7_SPI5=y -CONFIG_STM32H7_SPI6=y -CONFIG_STM32H7_SPI6_DMA=y -CONFIG_STM32H7_SPI6_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI5_DMA=y +CONFIG_STM32H7_SPI5_DMA_BUFFER=1024 CONFIG_STM32H7_SPI_DMA=y CONFIG_STM32H7_SPI_DMATHRESHOLD=8 -CONFIG_STM32H7_TIM12=y CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM2=y +CONFIG_STM32H7_TIM3=y CONFIG_STM32H7_TIM4=y -CONFIG_STM32H7_TIM5=y +CONFIG_STM32H7_TIM8=y CONFIG_STM32H7_UART4=y -CONFIG_STM32H7_UART5=y CONFIG_STM32H7_UART7=y CONFIG_STM32H7_UART8=y -CONFIG_STM32H7_USART1=y CONFIG_STM32H7_USART2=y CONFIG_STM32H7_USART3=y CONFIG_STM32H7_USART6=y @@ -241,33 +203,29 @@ CONFIG_STM32H7_USART_SINGLEWIRE=y CONFIG_STM32H7_USART_SWAP=y CONFIG_SYSTEM_CDCACM=y CONFIG_SYSTEM_NSH=y -CONFIG_SYSTEM_PING=y CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGSTP=y CONFIG_UART4_BAUD=57600 CONFIG_UART4_RXBUFSIZE=600 CONFIG_UART4_TXBUFSIZE=1500 -CONFIG_UART5_IFLOWCONTROL=y -CONFIG_UART5_OFLOWCONTROL=y CONFIG_UART7_BAUD=57600 -CONFIG_UART7_IFLOWCONTROL=y -CONFIG_UART7_OFLOWCONTROL=y CONFIG_UART7_RXBUFSIZE=600 -CONFIG_UART7_TXBUFSIZE=3000 +CONFIG_UART7_TXBUFSIZE=1500 CONFIG_UART8_BAUD=57600 CONFIG_UART8_RXBUFSIZE=600 CONFIG_UART8_TXBUFSIZE=1500 -CONFIG_USART1_BAUD=57600 -CONFIG_USART1_RXBUFSIZE=600 -CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_UART8_SERIAL_CONSOLE=y CONFIG_USART2_BAUD=57600 CONFIG_USART2_IFLOWCONTROL=y CONFIG_USART2_OFLOWCONTROL=y CONFIG_USART2_RXBUFSIZE=600 -CONFIG_USART2_TXBUFSIZE=3000 +CONFIG_USART2_TXBUFSIZE=1500 CONFIG_USART3_BAUD=57600 -CONFIG_USART3_RXBUFSIZE=180 -CONFIG_USART3_SERIAL_CONSOLE=y -CONFIG_USART3_TXBUFSIZE=1500 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=3000 CONFIG_USART6_BAUD=57600 CONFIG_USART6_RXBUFSIZE=600 CONFIG_USART6_TXBUFSIZE=1500 @@ -277,4 +235,3 @@ CONFIG_USBDEV_MAXPOWER=500 CONFIG_USEC_PER_TICK=1000 CONFIG_USERMAIN_STACKSIZE=2944 CONFIG_USER_ENTRYPOINT="nsh_main" -CONFIG_WATCHDOG=y diff --git a/boards/mro/ctrl-zero-h7/nuttx-config/scripts/bootloader_script.ld b/boards/mro/ctrl-zero-h7/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 0000000000..3fb4cc1f33 --- /dev/null +++ b/boards/mro/ctrl-zero-h7/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,221 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2020 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The board uses an STM32H743II has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * There's a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 128K + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > AXI_SRAM AT > FLASH + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } + + .ramfunc : { + _sramfuncs = .; + *(.ramfunc .ramfunc.*) + . = ALIGN(4); + _eramfuncs = .; + } > ITCM_RAM AT > FLASH + + _framfuncs = LOADADDR(.ramfunc); +} diff --git a/boards/mro/ctrl-zero-h7/nuttx-config/scripts/script.ld b/boards/mro/ctrl-zero-h7/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..c63b08f931 --- /dev/null +++ b/boards/mro/ctrl-zero-h7/nuttx-config/scripts/script.ld @@ -0,0 +1,229 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2021 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The board uses an STM32H743XIH6 and has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * There's a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1920K + + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > AXI_SRAM AT > FLASH + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Emit the the D3 power domain section for locating BDMA data */ + .SRAM4 (NOLOAD) : + { + } > SRAM4 + + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } + + .ramfunc : { + _sramfuncs = .; + *(.ramfunc .ramfunc.*) + . = ALIGN(4); + _eramfuncs = .; + } > ITCM_RAM AT > FLASH + + _framfuncs = LOADADDR(.ramfunc); +} diff --git a/boards/mro/ctrl-zero-h7/src/CMakeLists.txt b/boards/mro/ctrl-zero-h7/src/CMakeLists.txt new file mode 100644 index 0000000000..8e734c6fca --- /dev/null +++ b/boards/mro/ctrl-zero-h7/src/CMakeLists.txt @@ -0,0 +1,64 @@ +############################################################################ +# +# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_library(drivers_board + bootloader_main.c + usb.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader) + +else() + add_library(drivers_board + i2c.cpp + init.c + led.c + spi.cpp + timer_config.cpp + usb.c + ) + + target_link_libraries(drivers_board + PRIVATE + arch_spi + drivers__led + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/mro/ctrl-zero-h7/src/board_config.h b/boards/mro/ctrl-zero-h7/src/board_config.h new file mode 100644 index 0000000000..c38b6796aa --- /dev/null +++ b/boards/mro/ctrl-zero-h7/src/board_config.h @@ -0,0 +1,186 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * Board internal definitions + */ + +#pragma once + +#include +#include +#include +#include + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ +#define GPIO_nLED_RED /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) +#define GPIO_nLED_GREEN /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) +#define GPIO_nLED_BLUE /* PB3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_STATE_LED LED_BLUE + +/* ADC channels */ +#define PX4_ADC_GPIO \ + /* PA2 */ GPIO_ADC12_INP14, \ + /* PA3 */ GPIO_ADC12_INP15, \ + /* PA4 */ GPIO_ADC12_INP18, \ + /* PC1 */ GPIO_ADC123_INP11 + +/* Define Channel numbers must match above GPIO pins */ +#define ADC_BATTERY_VOLTAGE_CHANNEL 14 /* PA2 BATT_VOLT_SENS */ +#define ADC_BATTERY_CURRENT_CHANNEL 15 /* PA3 BATT_CURRENT_SENS */ +#define ADC_SCALED_V5_CHANNEL 18 /* PA4 VDD_5V_SENS */ +#define ADC_RC_RSSI_CHANNEL 11 /* PC1 */ + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL) | \ + (1 << ADC_RC_RSSI_CHANNEL)) + +/* HW has to large of R termination on ADC todo:change when HW value is chosen */ +#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) + +/* CAN Silence: Silent mode control */ +#define GPIO_CAN1_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) + +/* PWM */ +#define DIRECT_PWM_OUTPUT_CHANNELS 8 +#define DIRECT_INPUT_TIMER_CHANNELS 8 + +/* Power supply control and monitoring GPIOs */ +#define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) + +#define GPIO_VDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ +#define BOARD_NUMBER_BRICKS 1 + +#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) + +/* Define True logic Power Control in arch agnostic form */ +#define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true)) +#define READ_VDD_3V3_SPEKTRUM_POWER_EN() (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0) + +/* Tone alarm output */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* PA15 TIM2_CH1 */ + +#define GPIO_BUZZER_1 /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2 + +/* USB OTG FS */ +#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 3 /* use timer3 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +#define HRT_PPM_CHANNEL /* T3C3 */ 3 /* use capture/compare channel 3 */ +#define GPIO_PPM_IN /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1 + +/* RC Serial port */ +#define RC_SERIAL_PORT "/dev/ttyS3" + +#define GPIO_RSSI_IN /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) + +/* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */ +#define GPIO_SAFETY_SWITCH_IN /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */ +#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */ + +/* Power switch controls ******************************************************/ +#define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true) + +/* + * Board has a separate RC_IN + * + * GPIO PPM_IN on PB0 T3CH3 + * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7 + * Inversion is possible in the UART and can drive GPIO_PPM_IN as an output + */ +#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT) +#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */ +#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true)) + +/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) + * this board support the ADC system_power interface, and therefore + * provides the true logic GPIO BOARD_ADC_xxxx macros. + */ +#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) +#define BOARD_ADC_USB_VALID BOARD_ADC_USB_CONNECTED +#define BOARD_ADC_SERVO_VALID (1) /* never powers off the Servo rail */ +#define BOARD_ADC_BRICK_VALID (px4_arch_gpioread(GPIO_VDD_BRICK1_VALID)) + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ +#define BOARD_HAS_ON_RESET 1 + +#define BOARD_HAS_STATIC_MANIFEST 1 + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + +#define BOARD_NUM_IO_TIMERS 3 + +#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_nPOWER_IN_A, \ + GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_SAFETY_SWITCH_IN, \ + GPIO_OTGFS_VBUS, \ + } + +__BEGIN_DECLS +#ifndef __ASSEMBLY__ + +extern void stm32_spiinitialize(void); +extern void board_peripheral_reset(int ms); + +#include +#endif /* __ASSEMBLY__ */ +__END_DECLS diff --git a/boards/mro/ctrl-zero-h7/src/bootloader_main.c b/boards/mro/ctrl-zero-h7/src/bootloader_main.c new file mode 100644 index 0000000000..0a481dcc19 --- /dev/null +++ b/boards/mro/ctrl-zero-h7/src/bootloader_main.c @@ -0,0 +1,76 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include +#include "arm_internal.h" +#include + +extern int sercon_main(int c, char **argv); + +__EXPORT void board_on_reset(int status) {} + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure USB interfaces */ + stm32_configgpio(GPIO_OTGFS_VBUS); +} + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + return 0; +} + +void board_late_initialize(void) +{ + px4_platform_console_init(); + sercon_main(0, NULL); +} + +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/mro/ctrl-zero-h7/src/hw_config.h b/boards/mro/ctrl-zero-h7/src/hw_config.h new file mode 100644 index 0000000000..8fdaec9e7e --- /dev/null +++ b/boards/mro/ctrl-zero-h7/src/hw_config.h @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +/**************************************************************************** + * 10-8--2016: + * To simplify the ripple effect on the tools, we will be using + * /dev/serial/by-id/PX4 to locate PX4 devices. Therefore + * moving forward all Bootloaders must contain the prefix "PX4 BL " + * in the USBDEVICESTRING + * This Change will be made in an upcoming BL release + ****************************************************************************/ +/* + * Define usage to configure a bootloader + * + * + * Constant example Usage + * APP_LOAD_ADDRESS 0x08004000 - The address in Linker Script, where the app fw is org-ed + * BOOTLOADER_DELAY 5000 - Ms to wait while under USB pwr or bootloader request + * BOARD_FMUV2 + * INTERFACE_USB 1 - (Optional) Scan and use the USB interface for bootloading + * INTERFACE_USART 1 - (Optional) Scan and use the Serial interface for bootloading + * USBDEVICESTRING "PX4 BL FMU v2.x" - USB id string + * USBPRODUCTID 0x0011 - PID Should match defconfig + * BOOT_DELAY_ADDRESS 0x000001a0 - (Optional) From the linker script from Linker Script to get a custom + * delay provided by an APP FW + * BOARD_TYPE 9 - Must match .prototype boad_id + * _FLASH_KBYTES (*(uint16_t *)0x1fff7a22) - Run time flash size detection + * BOARD_FLASH_SECTORS ((_FLASH_KBYTES == 0x400) ? 11 : 23) - Run time determine the physical last sector + * BOARD_FLASH_SECTORS 11 - Hard coded zero based last sector + * BOARD_FLASH_SIZE (_FLASH_KBYTES*1024)- Total Flash size of device, determined at run time. + * (1024 * 1024) - Hard coded Total Flash of device - The bootloader and app reserved will be deducted + * programmatically + * + * BOARD_FIRST_FLASH_SECTOR_TO_ERASE 2 - Optional sectors index in the flash_sectors table (F4 only), to begin erasing. + * This is to allow sectors to be reserved for app fw usage. That will NOT be erased + * during a FW upgrade. + * The default is 0, and selects the first sector to be erased, as the 0th entry in the + * flash_sectors table. Which is the second physical sector of FLASH in the device. + * The first physical sector of FLASH is used by the bootloader, and is not defined + * in the table. + * + * APP_RESERVATION_SIZE (BOARD_FIRST_FLASH_SECTOR_TO_ERASE * 16 * 1024) - Number of bytes reserved by the APP FW. This number plus + * BOOTLOADER_RESERVATION_SIZE will be deducted from + * BOARD_FLASH_SIZE to determine the size of the App FW + * and hence the address space of FLASH to erase and program. + * USBMFGSTRING "PX4 AP" - Optional USB MFG string (default is '3D Robotics' if not defined.) + * SERIAL_BREAK_DETECT_DISABLED - Optional prevent break selection on Serial port from entering or staying in BL + * + * * Other defines are somewhat self explanatory. + */ + +/* Boot device selection list*/ +#define USB0_DEV 0x01 +#define SERIAL0_DEV 0x02 +#define SERIAL1_DEV 0x04 + +#define APP_LOAD_ADDRESS 0x08020000 +#define BOOTLOADER_DELAY 5000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" +#define BOARD_VBUS MK_GPIO_INPUT(GPIO_OTGFS_VBUS) + +//#define USE_VBUS_PULL_DOWN +#define INTERFACE_USART 1 +#define INTERFACE_USART_CONFIG "/dev/ttyS0,115200" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 1024 +#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) +#define BOARD_FLASH_SECTORS (15) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 1024) + +#define OSC_FREQ 24 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_RED +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_RED +#define BOARD_LED_ON 0 +#define BOARD_LED_OFF 1 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 12 +#endif + +#if !defined(APP_RESERVATION_SIZE) +# define APP_RESERVATION_SIZE 0 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1 +#endif + +#if !defined(USB_DATA_ALIGN) +# define USB_DATA_ALIGN +#endif + +#ifndef BOOT_DEVICES_SELECTION +# define BOOT_DEVICES_SELECTION USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#ifndef BOOT_DEVICES_FILTER_ONUSB +# define BOOT_DEVICES_FILTER_ONUSB USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif diff --git a/boards/mro/ctrl-zero-h7/src/i2c.cpp b/boards/mro/ctrl-zero-h7/src/i2c.cpp new file mode 100644 index 0000000000..e9f26f9a9e --- /dev/null +++ b/boards/mro/ctrl-zero-h7/src/i2c.cpp @@ -0,0 +1,38 @@ +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), +}; diff --git a/boards/mro/ctrl-zero-h7/src/init.c b/boards/mro/ctrl-zero-h7/src/init.c new file mode 100644 index 0000000000..0c8f4cf157 --- /dev/null +++ b/boards/mro/ctrl-zero-h7/src/init.c @@ -0,0 +1,202 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file init.c + * + * board-specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +#include "board_config.h" + +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include + +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + bool last = READ_VDD_3V3_SPEKTRUM_POWER_EN(); + /* Keep Spektum on to discharge rail*/ + VDD_3V3_SPEKTRUM_POWER_EN(false); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + syslog(LOG_DEBUG, "reset done, %d ms\n", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + VDD_3V3_SPEKTRUM_POWER_EN(last); +} + +/************************************************************************************ + * Name: board_on_reset + * + * Description: + * Optionally provided function called on entry to board_system_reset + * It should perform any house keeping prior to the rest. + * + * status - 1 if resetting to boot loader + * 0 if just resetting + * + ************************************************************************************/ +__EXPORT void board_on_reset(int status) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ +__EXPORT void stm32_boardinitialize(void) +{ + /* Reset PWM first thing */ + board_on_reset(-1); + + /* configure pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + board_control_spi_sensors_power_configgpio(); + + /* configure LEDs */ + board_autoled_initialize(); +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ +__EXPORT int board_app_initialize(uintptr_t arg) +{ + /* Power on Interfaces */ + board_control_spi_sensors_power(true, 0xffff); + VDD_3V3_SPEKTRUM_POWER_EN(true); + + px4_platform_init(); + + stm32_spiinitialize(); + + /* configure the DMA allocator */ + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_on(LED_GREEN); // Indicate Power. + led_off(LED_BLUE); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + +#ifdef CONFIG_MMCSD + /* Mount the SDIO-based MMC/SD block driver */ + /* First, get an instance of the SDIO interface */ + struct sdio_dev_s *sdio_dev = sdio_initialize(0); // SDIO_SLOTNO 0 Only one slot + + if (!sdio_dev) { + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", 0); + return ERROR; + } + + if (mmcsd_slotinitialize(0, sdio_dev) != OK) { + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver\n"); + return ERROR; + } + + /* Assume that the SD card is inserted. What choice do we have? */ + sdio_mediachange(sdio_dev, true); +#endif /* CONFIG_MMCSD */ + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + return OK; +} diff --git a/boards/mro/ctrl-zero-h7/src/led.c b/boards/mro/ctrl-zero-h7/src/led.c new file mode 100644 index 0000000000..0e7beb3a9e --- /dev/null +++ b/boards/mro/ctrl-zero-h7/src/led.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file led.c + * + * LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +static uint32_t g_ledmap[] = { + GPIO_nLED_BLUE, // Indexed by LED_BLUE + GPIO_nLED_RED, // Indexed by LED_RED + GPIO_nLED_GREEN, // Indexed by LED_GREEN +}; + +__EXPORT void led_init(void) +{ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + if (g_ledmap[l] != 0) { + stm32_configgpio(g_ledmap[l]); + } + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + if (g_ledmap[led] != 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } +} + +static bool phy_get_led(int led) +{ + /* If Low it is on */ + if (g_ledmap[led] != 0) { + return !stm32_gpioread(g_ledmap[led]); + } + + return false; +} + +__EXPORT void led_on(int led) +{ + phy_set_led(led, true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(led, !phy_get_led(led)); +} diff --git a/boards/mro/ctrl-zero-h7/src/spi.cpp b/boards/mro/ctrl-zero-h7/src/spi.cpp new file mode 100644 index 0000000000..4a4c3502bb --- /dev/null +++ b/boards/mro/ctrl-zero-h7/src/spi.cpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}), + initSPIDevice(DRV_BARO_DEVTYPE_DPS310, SPI::CS{GPIO::PortD, GPIO::Pin7}), + }), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/mro/ctrl-zero-h7/src/timer_config.cpp b/boards/mro/ctrl-zero-h7/src/timer_config.cpp new file mode 100644 index 0000000000..e238804914 --- /dev/null +++ b/boards/mro/ctrl-zero-h7/src/timer_config.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer1, DMA{DMA::Index1}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1}), + initIOTimer(Timer::Timer8, DMA{DMA::Index1}), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel1}, {GPIO::PortI, GPIO::Pin5}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel2}, {GPIO::PortI, GPIO::Pin6}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/mro/ctrl-zero-h7/src/usb.c b/boards/mro/ctrl-zero-h7/src/usb.c new file mode 100644 index 0000000000..e60a93bb59 --- /dev/null +++ b/boards/mro/ctrl-zero-h7/src/usb.c @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file usb.c + * + * Board-specific USB functions. + */ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32H7_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); +#endif +} + + + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/boards/mro/pixracerpro/default.cmake b/boards/mro/pixracerpro/default.cmake index 0bd854ff26..f65c1b6c82 100644 --- a/boards/mro/pixracerpro/default.cmake +++ b/boards/mro/pixracerpro/default.cmake @@ -110,10 +110,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/mro/pixracerpro/nuttx-config/include/board.h b/boards/mro/pixracerpro/nuttx-config/include/board.h index 15bee730b3..9cda26cb73 100644 --- a/boards/mro/pixracerpro/nuttx-config/include/board.h +++ b/boards/mro/pixracerpro/nuttx-config/include/board.h @@ -237,8 +237,8 @@ #define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */ #define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */ -#define GPIO_USART6_TX GPIO_USART6_TX_2 /* PG14 */ -#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ +//#define GPIO_USART6_TX GPIO_USART6_TX_2 /* PG14 */ +//#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ #define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ #define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */ @@ -262,7 +262,7 @@ #define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ #define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 /* PA7 */ -#define GPIO_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_2) /* PB10 */ +#define GPIO_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_3) /* PB10 */ #define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 /* PB14 */ #define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 /* PB15 */ diff --git a/boards/mro/pixracerpro/nuttx-config/nsh/defconfig b/boards/mro/pixracerpro/nuttx-config/nsh/defconfig index 74a31bd189..d020ae771b 100644 --- a/boards/mro/pixracerpro/nuttx-config/nsh/defconfig +++ b/boards/mro/pixracerpro/nuttx-config/nsh/defconfig @@ -198,7 +198,6 @@ CONFIG_STM32H7_UART8=y CONFIG_STM32H7_USART1=y CONFIG_STM32H7_USART2=y CONFIG_STM32H7_USART3=y -CONFIG_STM32H7_USART6=y CONFIG_STM32H7_USART_BREAKS=y CONFIG_STM32H7_USART_INVERT=y CONFIG_STM32H7_USART_SINGLEWIRE=y @@ -228,9 +227,6 @@ CONFIG_USART3_IFLOWCONTROL=y CONFIG_USART3_OFLOWCONTROL=y CONFIG_USART3_RXBUFSIZE=600 CONFIG_USART3_TXBUFSIZE=3000 -CONFIG_USART6_BAUD=57600 -CONFIG_USART6_RXBUFSIZE=600 -CONFIG_USART6_TXBUFSIZE=1500 CONFIG_USBDEV=y CONFIG_USBDEV_BUSPOWERED=y CONFIG_USBDEV_MAXPOWER=500 diff --git a/boards/mro/x21-777/default.cmake b/boards/mro/x21-777/default.cmake index 3f091aa63e..b375d33cf0 100644 --- a/boards/mro/x21-777/default.cmake +++ b/boards/mro/x21-777/default.cmake @@ -111,10 +111,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/mro/x21/default.cmake b/boards/mro/x21/default.cmake index baf3ff0123..3206bc224d 100644 --- a/boards/mro/x21/default.cmake +++ b/boards/mro/x21/default.cmake @@ -107,10 +107,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/nxp/fmuk66-e/default.cmake b/boards/nxp/fmuk66-e/default.cmake index 51cbe7180b..4f0a2faf86 100644 --- a/boards/nxp/fmuk66-e/default.cmake +++ b/boards/nxp/fmuk66-e/default.cmake @@ -106,10 +106,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/nxp/fmuk66-e/socketcan.cmake b/boards/nxp/fmuk66-e/socketcan.cmake index db21a3a0b7..5c8758adfb 100644 --- a/boards/nxp/fmuk66-e/socketcan.cmake +++ b/boards/nxp/fmuk66-e/socketcan.cmake @@ -105,10 +105,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index dccf51ad58..3c5ab8ad67 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -106,10 +106,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/nxp/fmuk66-v3/rtps.cmake b/boards/nxp/fmuk66-v3/rtps.cmake index ebf7f7beeb..47dd4d9b4b 100644 --- a/boards/nxp/fmuk66-v3/rtps.cmake +++ b/boards/nxp/fmuk66-v3/rtps.cmake @@ -106,10 +106,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/nxp/fmuk66-v3/socketcan.cmake b/boards/nxp/fmuk66-v3/socketcan.cmake index 0aee173d69..396da8efe8 100644 --- a/boards/nxp/fmuk66-v3/socketcan.cmake +++ b/boards/nxp/fmuk66-v3/socketcan.cmake @@ -105,10 +105,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/nxp/fmurt1062-v1/default.cmake b/boards/nxp/fmurt1062-v1/default.cmake index 23dc2d9989..0dd342b85b 100644 --- a/boards/nxp/fmurt1062-v1/default.cmake +++ b/boards/nxp/fmurt1062-v1/default.cmake @@ -73,7 +73,7 @@ px4_add_board( sensors sih temperature_compensation - vmount + #vmount SYSTEMCMDS # bl_update dmesg @@ -98,6 +98,7 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake index 676c7c6cf8..a5a98cfb36 100644 --- a/boards/omnibus/f4sd/default.cmake +++ b/boards/omnibus/f4sd/default.cmake @@ -93,10 +93,12 @@ px4_add_board( top #topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + #fake_gps #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello #hwtest # Hardware test diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index f65075961f..82fbc75c50 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -96,7 +96,7 @@ px4_add_board( sensors #sih #temperature_compensation - vmount + #vmount #vtol_att_control SYSTEMCMDS bl_update @@ -123,10 +123,12 @@ px4_add_board( top #topic_listener tune_control + #uorb #usb_connected #ver #work_queue EXAMPLES + #fake_gps #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello #hwtest # Hardware test diff --git a/boards/px4/fmu-v2/fixedwing.cmake b/boards/px4/fmu-v2/fixedwing.cmake index 5222e88a1d..d0656e3880 100644 --- a/boards/px4/fmu-v2/fixedwing.cmake +++ b/boards/px4/fmu-v2/fixedwing.cmake @@ -82,6 +82,7 @@ px4_add_board( top #topic_listener tune_control + #uorb #usb_connected ver #work_queue diff --git a/boards/px4/fmu-v2/lpe.cmake b/boards/px4/fmu-v2/lpe.cmake deleted file mode 100644 index a21c45816d..0000000000 --- a/boards/px4/fmu-v2/lpe.cmake +++ /dev/null @@ -1,121 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v2 - LABEL lpe - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - BOOTLOADER ${PX4_SOURCE_DIR}/ROMFS/px4fmu_common/extras/px4fmuv3_bl.bin - IO px4_io-v2_default - #TESTING - CONSTRAINED_FLASH - #UAVCAN_INTERFACES 2 - - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS6 - - DRIVERS - adc/board_adc - #barometer # all available barometer drivers - barometer/ms5611 - #batt_smbus - camera_capture - camera_trigger - #differential_pressure # all available differential pressure drivers - #differential_pressure/ms4525 - distance_sensor # all available distance sensor drivers - gps - #heater - #imu/analog_devices/adis16448 - #imu # all available imu drivers - imu/l3gd20 - imu/lsm303d - imu/invensense/mpu6000 - #iridiumsbd - irlock - #lights/blinkm - lights/rgbled - #magnetometer # all available magnetometer drivers - magnetometer/hmc5883 - #optical_flow # all available optical flow drivers - optical_flow/px4flow - #pca9685 - #protocol_splitter - #pwm_input - pwm_out_sim - pwm_out - px4io - #tap_esc - #telemetry # all available telemetry drivers - #test_ppm - tone_alarm - #uavcan - - MODULES - attitude_estimator_q - camera_feedback - commander - dataman - #ekf2 - events - flight_mode_manager - #fw_att_control - #fw_pos_control_l1 - #rover_pos_control - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_rate_control - mc_pos_control - navigator - battery_status - rc_update - sensors - temperature_compensation - vmount - #vtol_att_control - #airspeed_selector - - SYSTEMCMDS - bl_update - #dumpfile - #esc_calib - hardfault_log - i2cdetect - #led_control - mft - mixer - #motor_ramp - motor_test - mtd - #nshterm - param - perf - pwm - reboot - #sd_bench - #tests # tests and test runner - top - #topic_listener - tune_control - ver - work_queue - - EXAMPLES - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - ) diff --git a/boards/px4/fmu-v2/multicopter.cmake b/boards/px4/fmu-v2/multicopter.cmake index c3982cd205..7d99cd65f3 100644 --- a/boards/px4/fmu-v2/multicopter.cmake +++ b/boards/px4/fmu-v2/multicopter.cmake @@ -59,7 +59,7 @@ px4_add_board( sensors #sih #temperature_compensation - vmount + #vmount SYSTEMCMDS #bl_update #dumpfile @@ -81,6 +81,7 @@ px4_add_board( top #topic_listener tune_control + #uorb #usb_connected ver #work_queue diff --git a/boards/px4/fmu-v2/rover.cmake b/boards/px4/fmu-v2/rover.cmake index 537db8dadc..1955913e26 100644 --- a/boards/px4/fmu-v2/rover.cmake +++ b/boards/px4/fmu-v2/rover.cmake @@ -52,7 +52,7 @@ px4_add_board( rc_update sensors temperature_compensation - vmount + #vmount SYSTEMCMDS bl_update @@ -75,8 +75,8 @@ px4_add_board( top #topic_listener tune_control + uorb usb_connected ver work_queue - ) diff --git a/boards/px4/fmu-v2/test.cmake b/boards/px4/fmu-v2/test.cmake index 732f116d41..78c1430cbc 100644 --- a/boards/px4/fmu-v2/test.cmake +++ b/boards/px4/fmu-v2/test.cmake @@ -116,10 +116,12 @@ px4_add_board( top #topic_listener tune_control + #uorb #usb_connected ver #work_queue EXAMPLES + #fake_gps #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello #hwtest # Hardware test diff --git a/boards/px4/fmu-v3/ctrlalloc.cmake b/boards/px4/fmu-v3/ctrlalloc.cmake deleted file mode 100644 index 726900aaf6..0000000000 --- a/boards/px4/fmu-v3/ctrlalloc.cmake +++ /dev/null @@ -1,138 +0,0 @@ - -# FMUv3 is FMUv2 with access to the full 2MB flash - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v3 - LABEL ctrlalloc - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - IO px4_io-v2_default - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS6 - DRIVERS - adc/board_adc - adc/ads1115 - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/adis16477 - imu/adis16497 - imu/l3gd20 - imu/lsm303d - imu/invensense/icm20608g - imu/invensense/icm20948 - imu/invensense/mpu6000 - imu/invensense/mpu9250 - irlock - lights/blinkm - lights/rgbled - lights/rgbled_ncp5623c - magnetometer # all available magnetometer drivers - #optical_flow # all available optical flow drivers - optical_flow/px4flow - #osd - pca9685 - pca9685_pwm_out - #power_monitor/ina226 - #protocol_splitter - pwm_input - pwm_out_sim - pwm_out - px4io - roboclaw - tap_esc - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - angular_velocity_controller - attitude_estimator_q - battery_status - camera_feedback - commander - control_allocator - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - vmount - vtol_att_control - SYSTEMCMDS - bl_update - #dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - system_time - tests # tests and test runner - top - topic_listener - tune_control - usb_connected - ver - work_queue - EXAMPLES - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index 6891046e53..ab565a9bce 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -120,10 +120,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/px4/fmu-v3/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v3/nuttx-config/stackcheck/defconfig deleted file mode 100644 index ec3e2a4edb..0000000000 --- a/boards/px4/fmu-v3/nuttx-config/stackcheck/defconfig +++ /dev/null @@ -1,238 +0,0 @@ -# -# This file is autogenerated: PLEASE DO NOT EDIT IT. -# -# You can use "make menuconfig" to make any modifications to the installed .config file. -# You can then do "make savedefconfig" to generate a new defconfig file that includes your -# modifications. -# -# CONFIG_DISABLE_ENVIRON is not set -# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set -# CONFIG_MMCSD_HAVE_CARDDETECT is not set -# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set -# CONFIG_MMCSD_MMCSUPPORT is not set -# CONFIG_MMCSD_SPI is not set -# CONFIG_NSH_DISABLEBG is not set -# CONFIG_NSH_DISABLESCRIPT is not set -# CONFIG_NSH_DISABLE_DF is not set -# CONFIG_NSH_DISABLE_EXEC is not set -# CONFIG_NSH_DISABLE_EXIT is not set -# CONFIG_NSH_DISABLE_GET is not set -# CONFIG_NSH_DISABLE_ITEF is not set -# CONFIG_NSH_DISABLE_LOOPS is not set -# CONFIG_NSH_DISABLE_SEMICOLON is not set -# CONFIG_NSH_DISABLE_TIME is not set -# CONFIG_STM32_CCMEXCLUDE is not set -CONFIG_ARCH="arm" -CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" -CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y -CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" -CONFIG_ARCH_CHIP="stm32" -CONFIG_ARCH_CHIP_STM32=y -CONFIG_ARCH_CHIP_STM32F427V=y -CONFIG_ARCH_INTERRUPTSTACK=512 -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARMV7M_MEMCPY=y -CONFIG_ARMV7M_STACKCHECK=y -CONFIG_ARMV7M_USEBASEPRI=y -CONFIG_BOARDCTL_RESET=y -CONFIG_BOARD_CRASHDUMP=y -CONFIG_BOARD_LOOPSPERMSEC=16717 -CONFIG_BOARD_RESET_ON_ASSERT=2 -CONFIG_BUILTIN=y -CONFIG_C99_BOOL8=y -CONFIG_CDCACM=y -CONFIG_CDCACM_PRODUCTID=0x0011 -CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v2.x" -CONFIG_CDCACM_RXBUFSIZE=600 -CONFIG_CDCACM_TXBUFSIZE=2000 -CONFIG_CDCACM_VENDORID=0x26ac -CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CLOCK_MONOTONIC=y -CONFIG_DEBUG_FULLOPT=y -CONFIG_DEBUG_HARDFAULT_ALERT=y -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEFAULT_SMALL=y -CONFIG_DEV_FIFO_SIZE=0 -CONFIG_DEV_PIPE_MAXSIZE=1024 -CONFIG_DEV_PIPE_SIZE=70 -CONFIG_DISABLE_MQUEUE=y -CONFIG_FAT_DMAMEMORY=y -CONFIG_FAT_LCNAMES=y -CONFIG_FAT_LFN=y -CONFIG_FAT_LFN_ALIAS_HASH=y -CONFIG_FDCLONE_STDIO=y -CONFIG_FS_BINFS=y -CONFIG_FS_CROMFS=y -CONFIG_FS_FAT=y -CONFIG_FS_FATTIME=y -CONFIG_FS_PROCFS=y -CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y -CONFIG_FS_PROCFS_REGISTER=y -CONFIG_FS_ROMFS=y -CONFIG_GRAN=y -CONFIG_GRAN_INTR=y -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_I2C=y -CONFIG_I2C_RESET=y -CONFIG_IDLETHREAD_STACKSIZE=750 -CONFIG_LIBC_FLOATINGPOINT=y -CONFIG_LIBC_LONG_LONG=y -CONFIG_LIBC_STRERROR=y -CONFIG_MEMSET_64BIT=y -CONFIG_MEMSET_OPTSPEED=y -CONFIG_MMCSD=y -CONFIG_MMCSD_SDIO=y -CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y -CONFIG_MM_REGIONS=2 -CONFIG_MTD=y -CONFIG_MTD_BYTE_WRITE=y -CONFIG_MTD_PARTITION=y -CONFIG_MTD_RAMTRON=y -CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 -CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y -CONFIG_NSH_ARGCAT=y -CONFIG_NSH_BUILTIN_APPS=y -CONFIG_NSH_CMDPARMS=y -CONFIG_NSH_CROMFSETC=y -CONFIG_NSH_DISABLE_IFCONFIG=y -CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_TELNETD=y -CONFIG_NSH_LINELEN=128 -CONFIG_NSH_MAXARGUMENTS=15 -CONFIG_NSH_NESTDEPTH=8 -CONFIG_NSH_QUOTE=y -CONFIG_NSH_ROMFSETC=y -CONFIG_NSH_ROMFSSECTSIZE=128 -CONFIG_NSH_STRERROR=y -CONFIG_NSH_VARS=y -CONFIG_PIPES=y -CONFIG_PREALLOC_TIMERS=50 -CONFIG_PRIORITY_INHERITANCE=y -CONFIG_PTHREAD_MUTEX_ROBUST=y -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y -CONFIG_RAM_SIZE=262144 -CONFIG_RAM_START=0x20000000 -CONFIG_RAW_BINARY=y -CONFIG_RTC_DATETIME=y -CONFIG_SCHED_ATEXIT=y -CONFIG_SCHED_HPWORK=y -CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1280 -CONFIG_SCHED_INSTRUMENTATION=y -CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y -CONFIG_SCHED_LPWORK=y -CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1632 -CONFIG_SCHED_WAITPID=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SDIO_BLOCKSETUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y -CONFIG_SERIAL_TERMIOS=y -CONFIG_SIG_DEFAULT=y -CONFIG_SIG_SIGALRM_ACTION=y -CONFIG_SIG_SIGUSR1_ACTION=y -CONFIG_SIG_SIGUSR2_ACTION=y -CONFIG_SIG_SIGWORK=4 -CONFIG_STACK_COLORATION=y -CONFIG_START_DAY=30 -CONFIG_START_MONTH=11 -CONFIG_STDIO_BUFFER_SIZE=32 -CONFIG_STM32_ADC1=y -CONFIG_STM32_BBSRAM=y -CONFIG_STM32_BBSRAM_FILES=5 -CONFIG_STM32_BKPSRAM=y -CONFIG_STM32_CCMDATARAM=y -CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y -CONFIG_STM32_DMA1=y -CONFIG_STM32_DMA2=y -CONFIG_STM32_FLASH_CONFIG_I=y -CONFIG_STM32_FLOWCONTROL_BROKEN=y -CONFIG_STM32_I2C1=y -CONFIG_STM32_I2C2=y -CONFIG_STM32_I2CTIMEOMS=10 -CONFIG_STM32_I2CTIMEOTICKS=10 -CONFIG_STM32_JTAG_SW_ENABLE=y -CONFIG_STM32_OTGFS=y -CONFIG_STM32_PWR=y -CONFIG_STM32_RTC=y -CONFIG_STM32_RTC_HSECLOCK=y -CONFIG_STM32_RTC_MAGIC=0xfacefeee -CONFIG_STM32_RTC_MAGIC_REG=1 -CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef -CONFIG_STM32_SAVE_CRASHDUMP=y -CONFIG_STM32_SDIO=y -CONFIG_STM32_SDIO_CARD=y -CONFIG_STM32_SERIALBRK_BSDCOMPAT=y -CONFIG_STM32_SERIAL_DISABLE_REORDERING=y -CONFIG_STM32_SPI1=y -CONFIG_STM32_SPI1_DMA=y -CONFIG_STM32_SPI1_DMA_BUFFER=1024 -CONFIG_STM32_SPI2=y -CONFIG_STM32_SPI4=y -CONFIG_STM32_SPI4_DMA=y -CONFIG_STM32_SPI4_DMA_BUFFER=1024 -CONFIG_STM32_SPI_DMA=y -CONFIG_STM32_SPI_DMATHRESHOLD=32 -CONFIG_STM32_TIM10=y -CONFIG_STM32_TIM11=y -CONFIG_STM32_TIM3=y -CONFIG_STM32_TIM9=y -CONFIG_STM32_UART4=y -CONFIG_STM32_UART7=y -CONFIG_STM32_UART8=y -CONFIG_STM32_USART1=y -CONFIG_STM32_USART2=y -CONFIG_STM32_USART3=y -CONFIG_STM32_USART6=y -CONFIG_STM32_USART_BREAKS=y -CONFIG_STM32_USART_SINGLEWIRE=y -CONFIG_STM32_WWDG=y -CONFIG_SYSTEM_CDCACM=y -CONFIG_SYSTEM_NSH=y -CONFIG_TASK_NAME_SIZE=24 -CONFIG_UART4_BAUD=57600 -CONFIG_UART4_RXBUFSIZE=300 -CONFIG_UART4_RXDMA=y -CONFIG_UART4_TXBUFSIZE=300 -CONFIG_UART7_BAUD=57600 -CONFIG_UART7_RXBUFSIZE=300 -CONFIG_UART7_RXDMA=y -CONFIG_UART7_SERIAL_CONSOLE=y -CONFIG_UART7_TXBUFSIZE=300 -CONFIG_UART8_BAUD=57600 -CONFIG_UART8_RXBUFSIZE=300 -CONFIG_UART8_TXBUFSIZE=300 -CONFIG_USART1_RXBUFSIZE=128 -CONFIG_USART1_TXBUFSIZE=32 -CONFIG_USART2_BAUD=57600 -CONFIG_USART2_IFLOWCONTROL=y -CONFIG_USART2_OFLOWCONTROL=y -CONFIG_USART2_RXBUFSIZE=600 -CONFIG_USART2_RXDMA=y -CONFIG_USART2_TXBUFSIZE=1100 -CONFIG_USART3_BAUD=57600 -CONFIG_USART3_IFLOWCONTROL=y -CONFIG_USART3_OFLOWCONTROL=y -CONFIG_USART3_RXBUFSIZE=300 -CONFIG_USART3_RXDMA=y -CONFIG_USART3_TXBUFSIZE=600 -CONFIG_USART6_BAUD=57600 -CONFIG_USART6_RXBUFSIZE=300 -CONFIG_USART6_RXDMA=y -CONFIG_USART6_TXBUFSIZE=300 -CONFIG_USBDEV=y -CONFIG_USBDEV_BUSPOWERED=y -CONFIG_USBDEV_MAXPOWER=500 -CONFIG_USEC_PER_TICK=1000 -CONFIG_USERMAIN_STACKSIZE=2944 -CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake deleted file mode 100644 index cc2ad9ee8e..0000000000 --- a/boards/px4/fmu-v3/rtps.cmake +++ /dev/null @@ -1,132 +0,0 @@ - -# FMUv3 is FMUv2 with access to the full 2MB flash - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v3 - LABEL rtps - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - IO px4_io-v2_default - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS6 - DRIVERS - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/adis16477 - imu/adis16497 - imu/l3gd20 - imu/lsm303d - imu/invensense/icm20608g - imu/invensense/icm20948 - imu/invensense/mpu6000 - imu/invensense/mpu9250 - irlock - lights/blinkm - lights/rgbled - lights/rgbled_ncp5623c - magnetometer # all available magnetometer drivers - #optical_flow # all available optical flow drivers - optical_flow/px4flow - #osd - pca9685 - #power_monitor/ina226 - protocol_splitter - pwm_input - pwm_out_sim - pwm_out - px4io - roboclaw - tap_esc - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - vmount - vtol_att_control - SYSTEMCMDS - bl_update - #dmesg - dumpfile - esc_calib - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - system_time - tests # tests and test runner - top - topic_listener - tune_control - usb_connected - ver - work_queue - EXAMPLES - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake deleted file mode 100644 index cba93b5f94..0000000000 --- a/boards/px4/fmu-v3/stackcheck.cmake +++ /dev/null @@ -1,128 +0,0 @@ - -# FMUv3 is FMUv2 with access to the full 2MB flash - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v3 - LABEL stackcheck - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - IO px4_io-v2_default - TESTING - #UAVCAN_INTERFACES 2 - - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL4:/dev/ttyS3 - - DRIVERS - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - gps - #heater - imu/analog_devices/adis16448 - #imu # all available imu drivers - imu/l3gd20 - imu/lsm303d - imu/invensense/icm20608g - imu/invensense/mpu6000 - imu/invensense/mpu9250 - irlock - lights/blinkm - lights/rgbled - lights/rgbled_ncp5623c - magnetometer # all available magnetometer drivers - #optical_flow # all available optical flow drivers - optical_flow/px4flow - pca9685 - protocol_splitter - pwm_input - pwm_out_sim - pwm_out - px4io - roboclaw - tap_esc - telemetry # all available telemetry drivers - test_ppm - tone_alarm - #uavcan - - MODULES - attitude_estimator_q - camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - rover_pos_control - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_rate_control - mc_pos_control - navigator - battery_status - rc_update - sensors - sih - temperature_compensation - vmount - vtol_att_control - airspeed_selector - - SYSTEMCMDS - bl_update - dumpfile - esc_calib - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - system_time - tests # tests and test runner - top - topic_listener - tune_control - usb_connected - ver - work_queue - - EXAMPLES - #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - #hello - #hwtest # Hardware test - #matlab_csv_serial - #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - #rover_steering_control # Rover example app - #uuv_example_app - - ) diff --git a/boards/px4/fmu-v4/ctrlalloc.cmake b/boards/px4/fmu-v4/ctrlalloc.cmake deleted file mode 100644 index d94052facf..0000000000 --- a/boards/px4/fmu-v4/ctrlalloc.cmake +++ /dev/null @@ -1,138 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v4 - LABEL ctrlalloc - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - TESTING - UAVCAN_INTERFACES 1 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - WIFI:/dev/ttyS0 - - DRIVERS - adc/board_adc - adc/ads1115 - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/adis16477 - imu/adis16497 - imu/invensense/icm20602 - imu/invensense/icm20608g - imu/invensense/icm40609d - imu/invensense/mpu6500 - imu/invensense/mpu9250 - irlock - lights/blinkm - lights/rgbled - lights/rgbled_ncp5623c - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - #osd - pca9685 - pca9685_pwm_out - #protocol_splitter - pwm_input - pwm_out_sim - pwm_out - rc_input - roboclaw - safety_button - tap_esc - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - angular_velocity_controller - attitude_estimator_q - battery_status - camera_feedback - commander - control_allocator - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - #dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - system_time - tests # tests and test runner - top - topic_listener - tune_control - usb_connected - ver - work_queue - EXAMPLES - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - gyro_fft - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 6c88eb2c8e..d8f30be794 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -118,10 +118,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fake_gyro fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control diff --git a/boards/px4/fmu-v4/nuttx-config/optimized/defconfig b/boards/px4/fmu-v4/nuttx-config/optimized/defconfig deleted file mode 100644 index 3a6c78bb6b..0000000000 --- a/boards/px4/fmu-v4/nuttx-config/optimized/defconfig +++ /dev/null @@ -1,236 +0,0 @@ -# -# This file is autogenerated: PLEASE DO NOT EDIT IT. -# -# You can use "make menuconfig" to make any modifications to the installed .config file. -# You can then do "make savedefconfig" to generate a new defconfig file that includes your -# modifications. -# -# CONFIG_DISABLE_ENVIRON is not set -# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set -# CONFIG_MMCSD_HAVE_CARDDETECT is not set -# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set -# CONFIG_MMCSD_MMCSUPPORT is not set -# CONFIG_MMCSD_SPI is not set -# CONFIG_NSH_DISABLEBG is not set -# CONFIG_NSH_DISABLESCRIPT is not set -# CONFIG_NSH_DISABLE_DF is not set -# CONFIG_NSH_DISABLE_EXEC is not set -# CONFIG_NSH_DISABLE_EXIT is not set -# CONFIG_NSH_DISABLE_GET is not set -# CONFIG_NSH_DISABLE_ITEF is not set -# CONFIG_NSH_DISABLE_LOOPS is not set -# CONFIG_NSH_DISABLE_SEMICOLON is not set -# CONFIG_NSH_DISABLE_TIME is not set -# CONFIG_STM32_CCMEXCLUDE is not set -CONFIG_ARCH="arm" -CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" -CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y -CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" -CONFIG_ARCH_CHIP="stm32" -CONFIG_ARCH_CHIP_STM32=y -CONFIG_ARCH_CHIP_STM32F427V=y -CONFIG_ARCH_INTERRUPTSTACK=512 -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARMV7M_MEMCPY=y -CONFIG_ARMV7M_USEBASEPRI=y -CONFIG_BOARDCTL_RESET=y -CONFIG_BOARD_CRASHDUMP=y -CONFIG_BOARD_LOOPSPERMSEC=16717 -CONFIG_BOARD_RESET_ON_ASSERT=2 -CONFIG_BUILTIN=y -CONFIG_C99_BOOL8=y -CONFIG_CDCACM=y -CONFIG_CDCACM_PRODUCTID=0x0012 -CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v4.x" -CONFIG_CDCACM_RXBUFSIZE=600 -CONFIG_CDCACM_TXBUFSIZE=2000 -CONFIG_CDCACM_VENDORID=0x26ac -CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CLOCK_MONOTONIC=y -CONFIG_DEBUG_CUSTOMOPT=y -CONFIG_DEBUG_HARDFAULT_ALERT=y -CONFIG_DEBUG_OPTLEVEL="-O3" -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEFAULT_SMALL=y -CONFIG_DEV_FIFO_SIZE=0 -CONFIG_DEV_PIPE_MAXSIZE=1024 -CONFIG_DEV_PIPE_SIZE=70 -CONFIG_DISABLE_MQUEUE=y -CONFIG_FAT_DMAMEMORY=y -CONFIG_FAT_LCNAMES=y -CONFIG_FAT_LFN=y -CONFIG_FAT_LFN_ALIAS_HASH=y -CONFIG_FDCLONE_STDIO=y -CONFIG_FS_BINFS=y -CONFIG_FS_CROMFS=y -CONFIG_FS_FAT=y -CONFIG_FS_FATTIME=y -CONFIG_FS_PROCFS=y -CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y -CONFIG_FS_PROCFS_REGISTER=y -CONFIG_FS_ROMFS=y -CONFIG_GRAN=y -CONFIG_GRAN_INTR=y -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_I2C=y -CONFIG_I2C_RESET=y -CONFIG_IDLETHREAD_STACKSIZE=750 -CONFIG_LIBC_FLOATINGPOINT=y -CONFIG_LIBC_LONG_LONG=y -CONFIG_LIBC_STRERROR=y -CONFIG_MEMSET_64BIT=y -CONFIG_MEMSET_OPTSPEED=y -CONFIG_MMCSD=y -CONFIG_MMCSD_SDIO=y -CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y -CONFIG_MM_REGIONS=2 -CONFIG_MTD=y -CONFIG_MTD_BYTE_WRITE=y -CONFIG_MTD_PARTITION=y -CONFIG_MTD_RAMTRON=y -CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 -CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y -CONFIG_NSH_ARGCAT=y -CONFIG_NSH_BUILTIN_APPS=y -CONFIG_NSH_CMDPARMS=y -CONFIG_NSH_CROMFSETC=y -CONFIG_NSH_DISABLE_IFCONFIG=y -CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_TELNETD=y -CONFIG_NSH_LINELEN=128 -CONFIG_NSH_MAXARGUMENTS=15 -CONFIG_NSH_NESTDEPTH=8 -CONFIG_NSH_QUOTE=y -CONFIG_NSH_ROMFSETC=y -CONFIG_NSH_ROMFSSECTSIZE=128 -CONFIG_NSH_STRERROR=y -CONFIG_NSH_VARS=y -CONFIG_PIPES=y -CONFIG_PREALLOC_TIMERS=50 -CONFIG_PRIORITY_INHERITANCE=y -CONFIG_PTHREAD_MUTEX_ROBUST=y -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y -CONFIG_RAM_SIZE=262144 -CONFIG_RAM_START=0x20000000 -CONFIG_RAW_BINARY=y -CONFIG_RTC_DATETIME=y -CONFIG_SCHED_ATEXIT=y -CONFIG_SCHED_HPWORK=y -CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1280 -CONFIG_SCHED_INSTRUMENTATION=y -CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y -CONFIG_SCHED_LPWORK=y -CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1632 -CONFIG_SCHED_WAITPID=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SDIO_BLOCKSETUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y -CONFIG_SERIAL_TERMIOS=y -CONFIG_SIG_DEFAULT=y -CONFIG_SIG_SIGALRM_ACTION=y -CONFIG_SIG_SIGUSR1_ACTION=y -CONFIG_SIG_SIGUSR2_ACTION=y -CONFIG_SIG_SIGWORK=4 -CONFIG_STACK_COLORATION=y -CONFIG_START_DAY=30 -CONFIG_START_MONTH=11 -CONFIG_STDIO_BUFFER_SIZE=32 -CONFIG_STM32_ADC1=y -CONFIG_STM32_BBSRAM=y -CONFIG_STM32_BBSRAM_FILES=5 -CONFIG_STM32_BKPSRAM=y -CONFIG_STM32_CCMDATARAM=y -CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y -CONFIG_STM32_DMA1=y -CONFIG_STM32_DMA2=y -CONFIG_STM32_FLASH_CONFIG_I=y -CONFIG_STM32_FLOWCONTROL_BROKEN=y -CONFIG_STM32_I2C1=y -CONFIG_STM32_I2CTIMEOMS=10 -CONFIG_STM32_I2CTIMEOTICKS=10 -CONFIG_STM32_JTAG_SW_ENABLE=y -CONFIG_STM32_OTGFS=y -CONFIG_STM32_PWR=y -CONFIG_STM32_RTC=y -CONFIG_STM32_RTC_HSECLOCK=y -CONFIG_STM32_RTC_MAGIC=0xfacefeee -CONFIG_STM32_RTC_MAGIC_REG=1 -CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef -CONFIG_STM32_SAVE_CRASHDUMP=y -CONFIG_STM32_SDIO=y -CONFIG_STM32_SDIO_CARD=y -CONFIG_STM32_SERIALBRK_BSDCOMPAT=y -CONFIG_STM32_SERIAL_DISABLE_REORDERING=y -CONFIG_STM32_SPI1=y -CONFIG_STM32_SPI1_DMA=y -CONFIG_STM32_SPI1_DMA_BUFFER=1024 -CONFIG_STM32_SPI2=y -CONFIG_STM32_SPI4=y -CONFIG_STM32_SPI_DMA=y -CONFIG_STM32_SPI_DMATHRESHOLD=8 -CONFIG_STM32_TIM10=y -CONFIG_STM32_TIM11=y -CONFIG_STM32_TIM8=y -CONFIG_STM32_TIM9=y -CONFIG_STM32_UART4=y -CONFIG_STM32_UART7=y -CONFIG_STM32_UART8=y -CONFIG_STM32_USART1=y -CONFIG_STM32_USART2=y -CONFIG_STM32_USART3=y -CONFIG_STM32_USART6=y -CONFIG_STM32_USART_BREAKS=y -CONFIG_STM32_USART_SINGLEWIRE=y -CONFIG_STM32_WWDG=y -CONFIG_SYSTEM_CDCACM=y -CONFIG_SYSTEM_NSH=y -CONFIG_TASK_NAME_SIZE=24 -CONFIG_UART4_BAUD=57600 -CONFIG_UART4_RXBUFSIZE=300 -CONFIG_UART4_RXDMA=y -CONFIG_UART4_TXBUFSIZE=300 -CONFIG_UART7_BAUD=57600 -CONFIG_UART7_RXBUFSIZE=300 -CONFIG_UART7_RXDMA=y -CONFIG_UART7_SERIAL_CONSOLE=y -CONFIG_UART7_TXBUFSIZE=300 -CONFIG_UART8_BAUD=57600 -CONFIG_UART8_RXBUFSIZE=300 -CONFIG_UART8_TXBUFSIZE=300 -CONFIG_USART1_RXBUFSIZE=600 -CONFIG_USART1_RXDMA=y -CONFIG_USART1_TXBUFSIZE=2500 -CONFIG_USART2_BAUD=57600 -CONFIG_USART2_IFLOWCONTROL=y -CONFIG_USART2_OFLOWCONTROL=y -CONFIG_USART2_RXBUFSIZE=600 -CONFIG_USART2_RXDMA=y -CONFIG_USART2_TXBUFSIZE=1100 -CONFIG_USART3_BAUD=57600 -CONFIG_USART3_IFLOWCONTROL=y -CONFIG_USART3_OFLOWCONTROL=y -CONFIG_USART3_RXBUFSIZE=1200 -CONFIG_USART3_RXDMA=y -CONFIG_USART3_TXBUFSIZE=900 -CONFIG_USART6_BAUD=57600 -CONFIG_USART6_RXBUFSIZE=300 -CONFIG_USART6_RXDMA=y -CONFIG_USART6_TXBUFSIZE=300 -CONFIG_USBDEV=y -CONFIG_USBDEV_BUSPOWERED=y -CONFIG_USBDEV_MAXPOWER=500 -CONFIG_USEC_PER_TICK=1000 -CONFIG_USERMAIN_STACKSIZE=2944 -CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v4/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v4/nuttx-config/stackcheck/defconfig deleted file mode 100644 index fa0d897891..0000000000 --- a/boards/px4/fmu-v4/nuttx-config/stackcheck/defconfig +++ /dev/null @@ -1,236 +0,0 @@ -# -# This file is autogenerated: PLEASE DO NOT EDIT IT. -# -# You can use "make menuconfig" to make any modifications to the installed .config file. -# You can then do "make savedefconfig" to generate a new defconfig file that includes your -# modifications. -# -# CONFIG_DISABLE_ENVIRON is not set -# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set -# CONFIG_MMCSD_HAVE_CARDDETECT is not set -# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set -# CONFIG_MMCSD_MMCSUPPORT is not set -# CONFIG_MMCSD_SPI is not set -# CONFIG_NSH_DISABLEBG is not set -# CONFIG_NSH_DISABLESCRIPT is not set -# CONFIG_NSH_DISABLE_DF is not set -# CONFIG_NSH_DISABLE_EXEC is not set -# CONFIG_NSH_DISABLE_EXIT is not set -# CONFIG_NSH_DISABLE_GET is not set -# CONFIG_NSH_DISABLE_ITEF is not set -# CONFIG_NSH_DISABLE_LOOPS is not set -# CONFIG_NSH_DISABLE_SEMICOLON is not set -# CONFIG_NSH_DISABLE_TIME is not set -# CONFIG_STM32_CCMEXCLUDE is not set -CONFIG_ARCH="arm" -CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" -CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y -CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" -CONFIG_ARCH_CHIP="stm32" -CONFIG_ARCH_CHIP_STM32=y -CONFIG_ARCH_CHIP_STM32F427V=y -CONFIG_ARCH_INTERRUPTSTACK=512 -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARMV7M_MEMCPY=y -CONFIG_ARMV7M_STACKCHECK=y -CONFIG_ARMV7M_USEBASEPRI=y -CONFIG_BOARDCTL_RESET=y -CONFIG_BOARD_CRASHDUMP=y -CONFIG_BOARD_LOOPSPERMSEC=16717 -CONFIG_BOARD_RESET_ON_ASSERT=2 -CONFIG_BUILTIN=y -CONFIG_C99_BOOL8=y -CONFIG_CDCACM=y -CONFIG_CDCACM_PRODUCTID=0x0012 -CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v4.x" -CONFIG_CDCACM_RXBUFSIZE=600 -CONFIG_CDCACM_TXBUFSIZE=2000 -CONFIG_CDCACM_VENDORID=0x26ac -CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CLOCK_MONOTONIC=y -CONFIG_DEBUG_FULLOPT=y -CONFIG_DEBUG_HARDFAULT_ALERT=y -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEFAULT_SMALL=y -CONFIG_DEV_FIFO_SIZE=0 -CONFIG_DEV_PIPE_MAXSIZE=1024 -CONFIG_DEV_PIPE_SIZE=70 -CONFIG_DISABLE_MQUEUE=y -CONFIG_FAT_DMAMEMORY=y -CONFIG_FAT_LCNAMES=y -CONFIG_FAT_LFN=y -CONFIG_FAT_LFN_ALIAS_HASH=y -CONFIG_FDCLONE_STDIO=y -CONFIG_FS_BINFS=y -CONFIG_FS_CROMFS=y -CONFIG_FS_FAT=y -CONFIG_FS_FATTIME=y -CONFIG_FS_PROCFS=y -CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y -CONFIG_FS_PROCFS_REGISTER=y -CONFIG_FS_ROMFS=y -CONFIG_GRAN=y -CONFIG_GRAN_INTR=y -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_I2C=y -CONFIG_I2C_RESET=y -CONFIG_IDLETHREAD_STACKSIZE=750 -CONFIG_LIBC_FLOATINGPOINT=y -CONFIG_LIBC_LONG_LONG=y -CONFIG_LIBC_STRERROR=y -CONFIG_MEMSET_64BIT=y -CONFIG_MEMSET_OPTSPEED=y -CONFIG_MMCSD=y -CONFIG_MMCSD_SDIO=y -CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y -CONFIG_MM_REGIONS=2 -CONFIG_MTD=y -CONFIG_MTD_BYTE_WRITE=y -CONFIG_MTD_PARTITION=y -CONFIG_MTD_RAMTRON=y -CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 -CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y -CONFIG_NSH_ARGCAT=y -CONFIG_NSH_BUILTIN_APPS=y -CONFIG_NSH_CMDPARMS=y -CONFIG_NSH_CROMFSETC=y -CONFIG_NSH_DISABLE_IFCONFIG=y -CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_TELNETD=y -CONFIG_NSH_LINELEN=128 -CONFIG_NSH_MAXARGUMENTS=15 -CONFIG_NSH_NESTDEPTH=8 -CONFIG_NSH_QUOTE=y -CONFIG_NSH_ROMFSETC=y -CONFIG_NSH_ROMFSSECTSIZE=128 -CONFIG_NSH_STRERROR=y -CONFIG_NSH_VARS=y -CONFIG_PIPES=y -CONFIG_PREALLOC_TIMERS=50 -CONFIG_PRIORITY_INHERITANCE=y -CONFIG_PTHREAD_MUTEX_ROBUST=y -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y -CONFIG_RAM_SIZE=262144 -CONFIG_RAM_START=0x20000000 -CONFIG_RAW_BINARY=y -CONFIG_RTC_DATETIME=y -CONFIG_SCHED_ATEXIT=y -CONFIG_SCHED_HPWORK=y -CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1280 -CONFIG_SCHED_INSTRUMENTATION=y -CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y -CONFIG_SCHED_LPWORK=y -CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1632 -CONFIG_SCHED_WAITPID=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SDIO_BLOCKSETUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y -CONFIG_SERIAL_TERMIOS=y -CONFIG_SIG_DEFAULT=y -CONFIG_SIG_SIGALRM_ACTION=y -CONFIG_SIG_SIGUSR1_ACTION=y -CONFIG_SIG_SIGUSR2_ACTION=y -CONFIG_SIG_SIGWORK=4 -CONFIG_STACK_COLORATION=y -CONFIG_START_DAY=30 -CONFIG_START_MONTH=11 -CONFIG_STDIO_BUFFER_SIZE=32 -CONFIG_STM32_ADC1=y -CONFIG_STM32_BBSRAM=y -CONFIG_STM32_BBSRAM_FILES=5 -CONFIG_STM32_BKPSRAM=y -CONFIG_STM32_CCMDATARAM=y -CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y -CONFIG_STM32_DMA1=y -CONFIG_STM32_DMA2=y -CONFIG_STM32_FLASH_CONFIG_I=y -CONFIG_STM32_FLOWCONTROL_BROKEN=y -CONFIG_STM32_I2C1=y -CONFIG_STM32_I2CTIMEOMS=10 -CONFIG_STM32_I2CTIMEOTICKS=10 -CONFIG_STM32_JTAG_SW_ENABLE=y -CONFIG_STM32_OTGFS=y -CONFIG_STM32_PWR=y -CONFIG_STM32_RTC=y -CONFIG_STM32_RTC_HSECLOCK=y -CONFIG_STM32_RTC_MAGIC=0xfacefeee -CONFIG_STM32_RTC_MAGIC_REG=1 -CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef -CONFIG_STM32_SAVE_CRASHDUMP=y -CONFIG_STM32_SDIO=y -CONFIG_STM32_SDIO_CARD=y -CONFIG_STM32_SERIALBRK_BSDCOMPAT=y -CONFIG_STM32_SERIAL_DISABLE_REORDERING=y -CONFIG_STM32_SPI1=y -CONFIG_STM32_SPI1_DMA=y -CONFIG_STM32_SPI1_DMA_BUFFER=1024 -CONFIG_STM32_SPI2=y -CONFIG_STM32_SPI4=y -CONFIG_STM32_SPI_DMA=y -CONFIG_STM32_SPI_DMATHRESHOLD=8 -CONFIG_STM32_TIM10=y -CONFIG_STM32_TIM11=y -CONFIG_STM32_TIM8=y -CONFIG_STM32_TIM9=y -CONFIG_STM32_UART4=y -CONFIG_STM32_UART7=y -CONFIG_STM32_UART8=y -CONFIG_STM32_USART1=y -CONFIG_STM32_USART2=y -CONFIG_STM32_USART3=y -CONFIG_STM32_USART6=y -CONFIG_STM32_USART_BREAKS=y -CONFIG_STM32_USART_SINGLEWIRE=y -CONFIG_STM32_WWDG=y -CONFIG_SYSTEM_CDCACM=y -CONFIG_SYSTEM_NSH=y -CONFIG_TASK_NAME_SIZE=24 -CONFIG_UART4_BAUD=57600 -CONFIG_UART4_RXBUFSIZE=300 -CONFIG_UART4_RXDMA=y -CONFIG_UART4_TXBUFSIZE=300 -CONFIG_UART7_BAUD=57600 -CONFIG_UART7_RXBUFSIZE=300 -CONFIG_UART7_RXDMA=y -CONFIG_UART7_SERIAL_CONSOLE=y -CONFIG_UART7_TXBUFSIZE=300 -CONFIG_UART8_BAUD=57600 -CONFIG_UART8_RXBUFSIZE=300 -CONFIG_UART8_TXBUFSIZE=300 -CONFIG_USART1_RXBUFSIZE=600 -CONFIG_USART1_RXDMA=y -CONFIG_USART1_TXBUFSIZE=2500 -CONFIG_USART2_BAUD=57600 -CONFIG_USART2_IFLOWCONTROL=y -CONFIG_USART2_OFLOWCONTROL=y -CONFIG_USART2_RXBUFSIZE=600 -CONFIG_USART2_RXDMA=y -CONFIG_USART2_TXBUFSIZE=1100 -CONFIG_USART3_BAUD=57600 -CONFIG_USART3_IFLOWCONTROL=y -CONFIG_USART3_OFLOWCONTROL=y -CONFIG_USART3_RXBUFSIZE=1200 -CONFIG_USART3_RXDMA=y -CONFIG_USART3_TXBUFSIZE=900 -CONFIG_USART6_BAUD=57600 -CONFIG_USART6_RXBUFSIZE=300 -CONFIG_USART6_RXDMA=y -CONFIG_USART6_TXBUFSIZE=300 -CONFIG_USBDEV=y -CONFIG_USBDEV_BUSPOWERED=y -CONFIG_USBDEV_MAXPOWER=500 -CONFIG_USEC_PER_TICK=1000 -CONFIG_USERMAIN_STACKSIZE=2944 -CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v4/nuttx-config/uavcanv1/defconfig b/boards/px4/fmu-v4/nuttx-config/uavcanv1/defconfig deleted file mode 100644 index 9695e004b2..0000000000 --- a/boards/px4/fmu-v4/nuttx-config/uavcanv1/defconfig +++ /dev/null @@ -1,239 +0,0 @@ -# -# This file is autogenerated: PLEASE DO NOT EDIT IT. -# -# You can use "make menuconfig" to make any modifications to the installed .config file. -# You can then do "make savedefconfig" to generate a new defconfig file that includes your -# modifications. -# -# CONFIG_DISABLE_ENVIRON is not set -# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set -# CONFIG_MMCSD_HAVE_CARDDETECT is not set -# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set -# CONFIG_MMCSD_MMCSUPPORT is not set -# CONFIG_MMCSD_SPI is not set -# CONFIG_NSH_DISABLEBG is not set -# CONFIG_NSH_DISABLESCRIPT is not set -# CONFIG_NSH_DISABLE_DF is not set -# CONFIG_NSH_DISABLE_EXEC is not set -# CONFIG_NSH_DISABLE_EXIT is not set -# CONFIG_NSH_DISABLE_GET is not set -# CONFIG_NSH_DISABLE_ITEF is not set -# CONFIG_NSH_DISABLE_LOOPS is not set -# CONFIG_NSH_DISABLE_SEMICOLON is not set -# CONFIG_NSH_DISABLE_TIME is not set -# CONFIG_STM32_CCMEXCLUDE is not set -CONFIG_ARCH="arm" -CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" -CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y -CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" -CONFIG_ARCH_CHIP="stm32" -CONFIG_ARCH_CHIP_STM32=y -CONFIG_ARCH_CHIP_STM32F427V=y -CONFIG_ARCH_INTERRUPTSTACK=512 -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARMV7M_LAZYFPU=y -CONFIG_ARMV7M_MEMCPY=y -CONFIG_ARMV7M_USEBASEPRI=y -CONFIG_BOARDCTL_RESET=y -CONFIG_BOARD_CRASHDUMP=y -CONFIG_BOARD_LOOPSPERMSEC=16717 -CONFIG_BOARD_RESET_ON_ASSERT=2 -CONFIG_BUILTIN=y -CONFIG_C99_BOOL8=y -CONFIG_CAN_EXTID=y -CONFIG_CDCACM=y -CONFIG_CDCACM_PRODUCTID=0x0012 -CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v4.x" -CONFIG_CDCACM_RXBUFSIZE=600 -CONFIG_CDCACM_TXBUFSIZE=2000 -CONFIG_CDCACM_VENDORID=0x26ac -CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CLOCK_MONOTONIC=y -CONFIG_DEBUG_FULLOPT=y -CONFIG_DEBUG_HARDFAULT_ALERT=y -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEFAULT_SMALL=y -CONFIG_DEV_FIFO_SIZE=0 -CONFIG_DEV_PIPE_MAXSIZE=1024 -CONFIG_DEV_PIPE_SIZE=70 -CONFIG_DISABLE_MQUEUE=y -CONFIG_FAT_DMAMEMORY=y -CONFIG_FAT_LCNAMES=y -CONFIG_FAT_LFN=y -CONFIG_FAT_LFN_ALIAS_HASH=y -CONFIG_FDCLONE_STDIO=y -CONFIG_FS_BINFS=y -CONFIG_FS_CROMFS=y -CONFIG_FS_FAT=y -CONFIG_FS_FATTIME=y -CONFIG_FS_PROCFS=y -CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y -CONFIG_FS_PROCFS_REGISTER=y -CONFIG_FS_ROMFS=y -CONFIG_GRAN=y -CONFIG_GRAN_INTR=y -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_I2C=y -CONFIG_I2C_RESET=y -CONFIG_IDLETHREAD_STACKSIZE=750 -CONFIG_LIBC_FLOATINGPOINT=y -CONFIG_LIBC_LONG_LONG=y -CONFIG_LIBC_STRERROR=y -CONFIG_MEMSET_64BIT=y -CONFIG_MEMSET_OPTSPEED=y -CONFIG_MMCSD=y -CONFIG_MMCSD_SDIO=y -CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y -CONFIG_MM_REGIONS=2 -CONFIG_MTD=y -CONFIG_MTD_BYTE_WRITE=y -CONFIG_MTD_PARTITION=y -CONFIG_MTD_RAMTRON=y -CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 -CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y -CONFIG_NSH_ARGCAT=y -CONFIG_NSH_BUILTIN_APPS=y -CONFIG_NSH_CMDPARMS=y -CONFIG_NSH_CROMFSETC=y -CONFIG_NSH_DISABLE_IFCONFIG=y -CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_TELNETD=y -CONFIG_NSH_LINELEN=128 -CONFIG_NSH_MAXARGUMENTS=15 -CONFIG_NSH_NESTDEPTH=8 -CONFIG_NSH_QUOTE=y -CONFIG_NSH_ROMFSETC=y -CONFIG_NSH_ROMFSSECTSIZE=128 -CONFIG_NSH_STRERROR=y -CONFIG_NSH_VARS=y -CONFIG_PIPES=y -CONFIG_PREALLOC_TIMERS=50 -CONFIG_PRIORITY_INHERITANCE=y -CONFIG_PTHREAD_MUTEX_ROBUST=y -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y -CONFIG_RAM_SIZE=262144 -CONFIG_RAM_START=0x20000000 -CONFIG_RAW_BINARY=y -CONFIG_RTC_DATETIME=y -CONFIG_SCHED_ATEXIT=y -CONFIG_SCHED_HPWORK=y -CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1280 -CONFIG_SCHED_INSTRUMENTATION=y -CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y -CONFIG_SCHED_LPWORK=y -CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1632 -CONFIG_SCHED_WAITPID=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SDIO_BLOCKSETUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y -CONFIG_SERIAL_TERMIOS=y -CONFIG_SIG_DEFAULT=y -CONFIG_SIG_SIGALRM_ACTION=y -CONFIG_SIG_SIGUSR1_ACTION=y -CONFIG_SIG_SIGUSR2_ACTION=y -CONFIG_SIG_SIGWORK=4 -CONFIG_STACK_COLORATION=y -CONFIG_START_DAY=30 -CONFIG_START_MONTH=11 -CONFIG_STDIO_BUFFER_SIZE=32 -CONFIG_STM32_ADC1=y -CONFIG_STM32_BBSRAM=y -CONFIG_STM32_BBSRAM_FILES=5 -CONFIG_STM32_BKPSRAM=y -CONFIG_STM32_CAN1=y -CONFIG_STM32_CAN1_BAUD=1000000 -CONFIG_STM32_CCMDATARAM=y -CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y -CONFIG_STM32_DMA1=y -CONFIG_STM32_DMA2=y -CONFIG_STM32_FLASH_CONFIG_I=y -CONFIG_STM32_FLOWCONTROL_BROKEN=y -CONFIG_STM32_I2C1=y -CONFIG_STM32_I2CTIMEOMS=10 -CONFIG_STM32_I2CTIMEOTICKS=10 -CONFIG_STM32_JTAG_SW_ENABLE=y -CONFIG_STM32_OTGFS=y -CONFIG_STM32_PWR=y -CONFIG_STM32_RTC=y -CONFIG_STM32_RTC_HSECLOCK=y -CONFIG_STM32_RTC_MAGIC=0xfacefeee -CONFIG_STM32_RTC_MAGIC_REG=1 -CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef -CONFIG_STM32_SAVE_CRASHDUMP=y -CONFIG_STM32_SDIO=y -CONFIG_STM32_SDIO_CARD=y -CONFIG_STM32_SERIALBRK_BSDCOMPAT=y -CONFIG_STM32_SERIAL_DISABLE_REORDERING=y -CONFIG_STM32_SPI1=y -CONFIG_STM32_SPI1_DMA=y -CONFIG_STM32_SPI1_DMA_BUFFER=1024 -CONFIG_STM32_SPI2=y -CONFIG_STM32_SPI4=y -CONFIG_STM32_SPI_DMA=y -CONFIG_STM32_SPI_DMATHRESHOLD=8 -CONFIG_STM32_TIM10=y -CONFIG_STM32_TIM11=y -CONFIG_STM32_TIM8=y -CONFIG_STM32_TIM9=y -CONFIG_STM32_UART4=y -CONFIG_STM32_UART7=y -CONFIG_STM32_UART8=y -CONFIG_STM32_USART1=y -CONFIG_STM32_USART2=y -CONFIG_STM32_USART3=y -CONFIG_STM32_USART6=y -CONFIG_STM32_USART_BREAKS=y -CONFIG_STM32_USART_SINGLEWIRE=y -CONFIG_STM32_WWDG=y -CONFIG_SYSTEM_CDCACM=y -CONFIG_SYSTEM_NSH=y -CONFIG_TASK_NAME_SIZE=24 -CONFIG_UART4_BAUD=57600 -CONFIG_UART4_RXBUFSIZE=300 -CONFIG_UART4_RXDMA=y -CONFIG_UART4_TXBUFSIZE=300 -CONFIG_UART7_BAUD=57600 -CONFIG_UART7_RXBUFSIZE=300 -CONFIG_UART7_RXDMA=y -CONFIG_UART7_SERIAL_CONSOLE=y -CONFIG_UART7_TXBUFSIZE=300 -CONFIG_UART8_BAUD=57600 -CONFIG_UART8_RXBUFSIZE=300 -CONFIG_UART8_TXBUFSIZE=300 -CONFIG_USART1_RXBUFSIZE=600 -CONFIG_USART1_RXDMA=y -CONFIG_USART1_TXBUFSIZE=2500 -CONFIG_USART2_BAUD=57600 -CONFIG_USART2_IFLOWCONTROL=y -CONFIG_USART2_OFLOWCONTROL=y -CONFIG_USART2_RXBUFSIZE=600 -CONFIG_USART2_RXDMA=y -CONFIG_USART2_TXBUFSIZE=1100 -CONFIG_USART3_BAUD=57600 -CONFIG_USART3_IFLOWCONTROL=y -CONFIG_USART3_OFLOWCONTROL=y -CONFIG_USART3_RXBUFSIZE=1200 -CONFIG_USART3_RXDMA=y -CONFIG_USART3_TXBUFSIZE=900 -CONFIG_USART6_BAUD=57600 -CONFIG_USART6_RXBUFSIZE=300 -CONFIG_USART6_RXDMA=y -CONFIG_USART6_TXBUFSIZE=300 -CONFIG_USBDEV=y -CONFIG_USBDEV_BUSPOWERED=y -CONFIG_USBDEV_MAXPOWER=500 -CONFIG_USEC_PER_TICK=1000 -CONFIG_USERMAIN_STACKSIZE=2944 -CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v4/optimized.cmake b/boards/px4/fmu-v4/optimized.cmake deleted file mode 100644 index 05bb456997..0000000000 --- a/boards/px4/fmu-v4/optimized.cmake +++ /dev/null @@ -1,118 +0,0 @@ - -# set Release for -O3 -set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Build type" FORCE) -add_compile_options(-Wno-error=array-bounds) - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v4 - LABEL optimized - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - TESTING - UAVCAN_INTERFACES 1 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - DRIVERS - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - #heater - #imu # all available imu drivers - #imu/analog_devices/adis16448 - #imu/adis16477 - #imu/adis16497 - imu/invensense/icm20602 - imu/invensense/icm20608g - #imu/invensense/icm40609d - #imu/invensense/mpu6500 - imu/invensense/mpu9250 - irlock - lights/blinkm - lights/rgbled - lights/rgbled_ncp5623c - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - #pwm_input - pwm_out_sim - pwm_out - rc_input - #roboclaw - safety_button - #tap_esc - #telemetry # all available telemetry drivers - test_ppm - tone_alarm - #uavcan - MODULES - airspeed_selector - #attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - land_detector - #landing_target_estimator - load_mon - #local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - #rover_pos_control - sensors - #sih - temperature_compensation - #uuv_att_control - #vmount - vtol_att_control - SYSTEMCMDS - bl_update - #dmesg - dumpfile - esc_calib - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - system_time - tests # tests and test runner - top - topic_listener - tune_control - usb_connected - ver - work_queue - ) diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake deleted file mode 100644 index d87230acab..0000000000 --- a/boards/px4/fmu-v4/rtps.cmake +++ /dev/null @@ -1,128 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v4 - LABEL rtps - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - TESTING - UAVCAN_INTERFACES 1 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - DRIVERS - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/adis16477 - imu/adis16497 - imu/invensense/icm20602 - imu/invensense/icm20608g - imu/invensense/icm40609d - imu/invensense/mpu6500 - imu/invensense/mpu9250 - irlock - lights/blinkm - lights/rgbled - lights/rgbled_ncp5623c - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - #osd - pca9685 - protocol_splitter - pwm_input - pwm_out_sim - pwm_out - rc_input - roboclaw - safety_button - tap_esc - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - #dmesg - dumpfile - esc_calib - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - system_time - tests # tests and test runner - top - topic_listener - tune_control - usb_connected - ver - work_queue - EXAMPLES - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/px4/fmu-v4/uavcanv1.cmake b/boards/px4/fmu-v4/uavcanv1.cmake deleted file mode 100644 index b4a9b29283..0000000000 --- a/boards/px4/fmu-v4/uavcanv1.cmake +++ /dev/null @@ -1,136 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v4 - LABEL uavcanv1 - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - ROMFSROOT px4fmu_common - TESTING - UAVCAN_INTERFACES 1 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - WIFI:/dev/ttyS0 - - DRIVERS - adc/board_adc - adc/ads1115 - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/adis16477 - imu/adis16497 - imu/invensense/icm20602 - imu/invensense/icm20608g - imu/invensense/icm40609d - imu/invensense/mpu6500 - imu/invensense/mpu9250 - irlock - lights/blinkm - lights/rgbled - lights/rgbled_ncp5623c - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - #osd - pca9685 - pca9685_pwm_out - #protocol_splitter - pwm_input - pwm_out_sim - pwm_out - rc_input - roboclaw - safety_button - tap_esc - telemetry # all available telemetry drivers - test_ppm - tone_alarm - #uavcan - uavcan_v1 - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - #dmesg - dumpfile - esc_calib - gpio - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - system_time - tests # tests and test runner - top - topic_listener - tune_control - usb_connected - ver - work_queue - EXAMPLES - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - gyro_fft - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index 3592562acd..cbb50d0808 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -114,10 +114,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake deleted file mode 100644 index 8167947868..0000000000 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ /dev/null @@ -1,127 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v4pro - LABEL rtps - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY - ROMFSROOT px4fmu_common - IO px4_io-v2_default - TESTING - UAVCAN_INTERFACES 2 - SERIAL_PORTS - GPS1:/dev/ttyS3 - TEL1:/dev/ttyS1 - TEL2:/dev/ttyS2 - TEL3:/dev/ttyS0 - TEL4:/dev/ttyS6 - DRIVERS - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - #dshot - gps - #heater - #imu # all available imu drivers - imu/invensense/icm20602 - imu/invensense/icm20608g - imu/invensense/mpu9250 - irlock - lights/blinkm - lights/rgbled - lights/rgbled_ncp5623c - magnetometer # all available magnetometer drivers - #optical_flow # all available optical flow drivers - optical_flow/px4flow - #osd - pca9685 - power_monitor/ina226 - protocol_splitter - pwm_input - pwm_out_sim - pwm_out - px4io - roboclaw - tap_esc - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - uuv_att_control - uuv_pos_control - vmount - vtol_att_control - SYSTEMCMDS - bl_update - #dmesg - dumpfile - esc_calib - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - system_time - tests # tests and test runner - top - topic_listener - tune_control - usb_connected - ver - work_queue - EXAMPLES - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/px4/fmu-v5/ctrlalloc.cmake b/boards/px4/fmu-v5/ctrlalloc.cmake index 0adf5cff0d..db5923eb3a 100644 --- a/boards/px4/fmu-v5/ctrlalloc.cmake +++ b/boards/px4/fmu-v5/ctrlalloc.cmake @@ -122,10 +122,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fake_gyro fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control diff --git a/boards/px4/fmu-v5/debug.cmake b/boards/px4/fmu-v5/debug.cmake index e157e46b59..e74ab83236 100644 --- a/boards/px4/fmu-v5/debug.cmake +++ b/boards/px4/fmu-v5/debug.cmake @@ -118,10 +118,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + #fake_gps #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 8eedc7b875..c2dcd651ab 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -120,10 +120,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fake_gyro fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index 9ff4e3286d..8c333bcb4b 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -87,6 +87,7 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index f966510aa2..ba85501855 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -99,6 +99,7 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue diff --git a/boards/px4/fmu-v5/nuttx-config/critmonitor/defconfig b/boards/px4/fmu-v5/nuttx-config/critmonitor/defconfig deleted file mode 100644 index e95158d3fd..0000000000 --- a/boards/px4/fmu-v5/nuttx-config/critmonitor/defconfig +++ /dev/null @@ -1,245 +0,0 @@ -# -# This file is autogenerated: PLEASE DO NOT EDIT IT. -# -# You can use "make menuconfig" to make any modifications to the installed .config file. -# You can then do "make savedefconfig" to generate a new defconfig file that includes your -# modifications. -# -# CONFIG_DISABLE_ENVIRON is not set -# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set -# CONFIG_FS_PROCFS_EXCLUDE_ENVIRON is not set -# CONFIG_MMCSD_HAVE_CARDDETECT is not set -# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set -# CONFIG_MMCSD_MMCSUPPORT is not set -# CONFIG_MMCSD_SPI is not set -# CONFIG_NSH_DISABLEBG is not set -# CONFIG_NSH_DISABLESCRIPT is not set -# CONFIG_NSH_DISABLE_DF is not set -# CONFIG_NSH_DISABLE_EXEC is not set -# CONFIG_NSH_DISABLE_EXIT is not set -# CONFIG_NSH_DISABLE_GET is not set -# CONFIG_NSH_DISABLE_ITEF is not set -# CONFIG_NSH_DISABLE_LOOPS is not set -# CONFIG_NSH_DISABLE_SEMICOLON is not set -# CONFIG_NSH_DISABLE_TIME is not set -CONFIG_ARCH="arm" -CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" -CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y -CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" -CONFIG_ARCH_CHIP="stm32f7" -CONFIG_ARCH_CHIP_STM32F765II=y -CONFIG_ARCH_CHIP_STM32F7=y -CONFIG_ARCH_INTERRUPTSTACK=512 -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARMV7M_BASEPRI_WAR=y -CONFIG_ARMV7M_DCACHE=y -CONFIG_ARMV7M_DTCM=y -CONFIG_ARMV7M_ICACHE=y -CONFIG_ARMV7M_MEMCPY=y -CONFIG_ARMV7M_USEBASEPRI=y -CONFIG_BOARDCTL_RESET=y -CONFIG_BOARD_CRASHDUMP=y -CONFIG_BOARD_LOOPSPERMSEC=22114 -CONFIG_BOARD_RESET_ON_ASSERT=2 -CONFIG_BUILTIN=y -CONFIG_C99_BOOL8=y -CONFIG_CDCACM=y -CONFIG_CDCACM_PRODUCTID=0x0032 -CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x" -CONFIG_CDCACM_RXBUFSIZE=600 -CONFIG_CDCACM_TXBUFSIZE=12000 -CONFIG_CDCACM_VENDORID=0x26ac -CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CLOCK_MONOTONIC=y -CONFIG_DEBUG_FULLOPT=y -CONFIG_DEBUG_HARDFAULT_ALERT=y -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEFAULT_SMALL=y -CONFIG_DEV_FIFO_SIZE=0 -CONFIG_DEV_PIPE_MAXSIZE=1024 -CONFIG_DEV_PIPE_SIZE=70 -CONFIG_DISABLE_MQUEUE=y -CONFIG_FAT_DMAMEMORY=y -CONFIG_FAT_LCNAMES=y -CONFIG_FAT_LFN=y -CONFIG_FAT_LFN_ALIAS_HASH=y -CONFIG_FDCLONE_STDIO=y -CONFIG_FS_BINFS=y -CONFIG_FS_CROMFS=y -CONFIG_FS_FAT=y -CONFIG_FS_FATTIME=y -CONFIG_FS_PROCFS=y -CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y -CONFIG_FS_PROCFS_REGISTER=y -CONFIG_FS_ROMFS=y -CONFIG_GRAN=y -CONFIG_GRAN_INTR=y -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_I2C=y -CONFIG_I2C_RESET=y -CONFIG_IDLETHREAD_STACKSIZE=750 -CONFIG_LIBC_FLOATINGPOINT=y -CONFIG_LIBC_LONG_LONG=y -CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 -CONFIG_MEMSET_64BIT=y -CONFIG_MEMSET_OPTSPEED=y -CONFIG_MMCSD=y -CONFIG_MMCSD_SDIO=y -CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y -CONFIG_MM_REGIONS=3 -CONFIG_MTD=y -CONFIG_MTD_BYTE_WRITE=y -CONFIG_MTD_PARTITION=y -CONFIG_MTD_RAMTRON=y -CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 -CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y -CONFIG_NSH_ARGCAT=y -CONFIG_NSH_BUILTIN_APPS=y -CONFIG_NSH_CMDPARMS=y -CONFIG_NSH_CROMFSETC=y -CONFIG_NSH_DISABLE_IFCONFIG=y -CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_TELNETD=y -CONFIG_NSH_LINELEN=128 -CONFIG_NSH_MAXARGUMENTS=15 -CONFIG_NSH_NESTDEPTH=8 -CONFIG_NSH_QUOTE=y -CONFIG_NSH_ROMFSETC=y -CONFIG_NSH_ROMFSSECTSIZE=128 -CONFIG_NSH_STRERROR=y -CONFIG_NSH_VARS=y -CONFIG_PIPES=y -CONFIG_PREALLOC_TIMERS=50 -CONFIG_PRIORITY_INHERITANCE=y -CONFIG_PTHREAD_MUTEX_ROBUST=y -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y -CONFIG_RAM_SIZE=245760 -CONFIG_RAM_START=0x20010000 -CONFIG_RAW_BINARY=y -CONFIG_RTC_DATETIME=y -CONFIG_SCHED_ATEXIT=y -CONFIG_SCHED_CRITMONITOR=y -CONFIG_SCHED_HPWORK=y -CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1280 -CONFIG_SCHED_INSTRUMENTATION=y -CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y -CONFIG_SCHED_LPWORK=y -CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1632 -CONFIG_SCHED_WAITPID=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SDMMC1_SDIO_MODE=y -CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y -CONFIG_SERIAL_TERMIOS=y -CONFIG_SIG_DEFAULT=y -CONFIG_SIG_SIGALRM_ACTION=y -CONFIG_SIG_SIGUSR1_ACTION=y -CONFIG_SIG_SIGUSR2_ACTION=y -CONFIG_SIG_SIGWORK=4 -CONFIG_STACK_COLORATION=y -CONFIG_START_DAY=30 -CONFIG_START_MONTH=11 -CONFIG_STDIO_BUFFER_SIZE=256 -CONFIG_STM32F7_ADC1=y -CONFIG_STM32F7_BBSRAM=y -CONFIG_STM32F7_BBSRAM_FILES=5 -CONFIG_STM32F7_BKPSRAM=y -CONFIG_STM32F7_DMA1=y -CONFIG_STM32F7_DMA2=y -CONFIG_STM32F7_DMACAPABLE=y -CONFIG_STM32F7_FLOWCONTROL_BROKEN=y -CONFIG_STM32F7_I2C1=y -CONFIG_STM32F7_I2C2=y -CONFIG_STM32F7_I2C3=y -CONFIG_STM32F7_I2C4=y -CONFIG_STM32F7_I2C_DYNTIMEO=y -CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 -CONFIG_STM32F7_OTGFS=y -CONFIG_STM32F7_PROGMEM=y -CONFIG_STM32F7_PWR=y -CONFIG_STM32F7_RTC=y -CONFIG_STM32F7_RTC_HSECLOCK=y -CONFIG_STM32F7_RTC_MAGIC_REG=1 -CONFIG_STM32F7_SAVE_CRASHDUMP=y -CONFIG_STM32F7_SDMMC1=y -CONFIG_STM32F7_SDMMC_DMA=y -CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y -CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y -CONFIG_STM32F7_SPI1=y -CONFIG_STM32F7_SPI1_DMA=y -CONFIG_STM32F7_SPI1_DMA_BUFFER=1024 -CONFIG_STM32F7_SPI2=y -CONFIG_STM32F7_SPI4=y -CONFIG_STM32F7_SPI5=y -CONFIG_STM32F7_SPI6=y -CONFIG_STM32F7_SPI_DMA=y -CONFIG_STM32F7_SPI_DMATHRESHOLD=8 -CONFIG_STM32F7_TIM10=y -CONFIG_STM32F7_TIM11=y -CONFIG_STM32F7_UART4=y -CONFIG_STM32F7_UART7=y -CONFIG_STM32F7_UART8=y -CONFIG_STM32F7_USART1=y -CONFIG_STM32F7_USART2=y -CONFIG_STM32F7_USART3=y -CONFIG_STM32F7_USART6=y -CONFIG_STM32F7_USART_BREAKS=y -CONFIG_STM32F7_USART_INVERT=y -CONFIG_STM32F7_USART_SINGLEWIRE=y -CONFIG_STM32F7_USART_SWAP=y -CONFIG_STM32F7_WWDG=y -CONFIG_SYSTEM_CDCACM=y -CONFIG_SYSTEM_CRITMONITOR=y -CONFIG_SYSTEM_NSH=y -CONFIG_TASK_NAME_SIZE=24 -CONFIG_UART4_BAUD=57600 -CONFIG_UART4_RXBUFSIZE=600 -CONFIG_UART4_RXDMA=y -CONFIG_UART4_TXBUFSIZE=1500 -CONFIG_UART7_BAUD=57600 -CONFIG_UART7_RXBUFSIZE=600 -CONFIG_UART7_SERIAL_CONSOLE=y -CONFIG_UART7_TXBUFSIZE=1500 -CONFIG_UART8_BAUD=57600 -CONFIG_UART8_RXBUFSIZE=600 -CONFIG_UART8_RXDMA=y -CONFIG_UART8_TXBUFSIZE=1500 -CONFIG_USART1_BAUD=57600 -CONFIG_USART1_RXBUFSIZE=600 -CONFIG_USART1_TXBUFSIZE=1500 -CONFIG_USART2_BAUD=57600 -CONFIG_USART2_IFLOWCONTROL=y -CONFIG_USART2_OFLOWCONTROL=y -CONFIG_USART2_RXBUFSIZE=600 -CONFIG_USART2_RXDMA=y -CONFIG_USART2_TXBUFSIZE=1500 -CONFIG_USART3_BAUD=57600 -CONFIG_USART3_IFLOWCONTROL=y -CONFIG_USART3_OFLOWCONTROL=y -CONFIG_USART3_RXBUFSIZE=600 -CONFIG_USART3_RXDMA=y -CONFIG_USART3_TXBUFSIZE=3000 -CONFIG_USART3_TXDMA=y -CONFIG_USART6_BAUD=57600 -CONFIG_USART6_RXBUFSIZE=600 -CONFIG_USART6_RXDMA=y -CONFIG_USART6_TXBUFSIZE=1500 -CONFIG_USBDEV=y -CONFIG_USBDEV_BUSPOWERED=y -CONFIG_USBDEV_MAXPOWER=500 -CONFIG_USEC_PER_TICK=1000 -CONFIG_USERMAIN_STACKSIZE=2944 -CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v5/nuttx-config/irqmonitor/defconfig b/boards/px4/fmu-v5/nuttx-config/irqmonitor/defconfig deleted file mode 100644 index f11048a173..0000000000 --- a/boards/px4/fmu-v5/nuttx-config/irqmonitor/defconfig +++ /dev/null @@ -1,244 +0,0 @@ -# -# This file is autogenerated: PLEASE DO NOT EDIT IT. -# -# You can use "make menuconfig" to make any modifications to the installed .config file. -# You can then do "make savedefconfig" to generate a new defconfig file that includes your -# modifications. -# -# CONFIG_DISABLE_ENVIRON is not set -# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set -# CONFIG_FS_PROCFS_EXCLUDE_ENVIRON is not set -# CONFIG_MMCSD_HAVE_CARDDETECT is not set -# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set -# CONFIG_MMCSD_MMCSUPPORT is not set -# CONFIG_MMCSD_SPI is not set -# CONFIG_NSH_DISABLEBG is not set -# CONFIG_NSH_DISABLESCRIPT is not set -# CONFIG_NSH_DISABLE_DF is not set -# CONFIG_NSH_DISABLE_EXEC is not set -# CONFIG_NSH_DISABLE_EXIT is not set -# CONFIG_NSH_DISABLE_GET is not set -# CONFIG_NSH_DISABLE_ITEF is not set -# CONFIG_NSH_DISABLE_LOOPS is not set -# CONFIG_NSH_DISABLE_SEMICOLON is not set -# CONFIG_NSH_DISABLE_TIME is not set -CONFIG_ARCH="arm" -CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" -CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y -CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" -CONFIG_ARCH_CHIP="stm32f7" -CONFIG_ARCH_CHIP_STM32F765II=y -CONFIG_ARCH_CHIP_STM32F7=y -CONFIG_ARCH_INTERRUPTSTACK=512 -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARMV7M_BASEPRI_WAR=y -CONFIG_ARMV7M_DCACHE=y -CONFIG_ARMV7M_DTCM=y -CONFIG_ARMV7M_ICACHE=y -CONFIG_ARMV7M_MEMCPY=y -CONFIG_ARMV7M_USEBASEPRI=y -CONFIG_BOARDCTL_RESET=y -CONFIG_BOARD_CRASHDUMP=y -CONFIG_BOARD_LOOPSPERMSEC=22114 -CONFIG_BOARD_RESET_ON_ASSERT=2 -CONFIG_BUILTIN=y -CONFIG_C99_BOOL8=y -CONFIG_CDCACM=y -CONFIG_CDCACM_PRODUCTID=0x0032 -CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x" -CONFIG_CDCACM_RXBUFSIZE=600 -CONFIG_CDCACM_TXBUFSIZE=12000 -CONFIG_CDCACM_VENDORID=0x26ac -CONFIG_CDCACM_VENDORSTR="3D Robotics" -CONFIG_CLOCK_MONOTONIC=y -CONFIG_DEBUG_FULLOPT=y -CONFIG_DEBUG_HARDFAULT_ALERT=y -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEFAULT_SMALL=y -CONFIG_DEV_FIFO_SIZE=0 -CONFIG_DEV_PIPE_MAXSIZE=1024 -CONFIG_DEV_PIPE_SIZE=70 -CONFIG_DISABLE_MQUEUE=y -CONFIG_FAT_DMAMEMORY=y -CONFIG_FAT_LCNAMES=y -CONFIG_FAT_LFN=y -CONFIG_FAT_LFN_ALIAS_HASH=y -CONFIG_FDCLONE_STDIO=y -CONFIG_FS_BINFS=y -CONFIG_FS_CROMFS=y -CONFIG_FS_FAT=y -CONFIG_FS_FATTIME=y -CONFIG_FS_PROCFS=y -CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y -CONFIG_FS_PROCFS_REGISTER=y -CONFIG_FS_ROMFS=y -CONFIG_GRAN=y -CONFIG_GRAN_INTR=y -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_I2C=y -CONFIG_I2C_RESET=y -CONFIG_IDLETHREAD_STACKSIZE=750 -CONFIG_LIBC_FLOATINGPOINT=y -CONFIG_LIBC_LONG_LONG=y -CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 -CONFIG_MEMSET_64BIT=y -CONFIG_MEMSET_OPTSPEED=y -CONFIG_MMCSD=y -CONFIG_MMCSD_SDIO=y -CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y -CONFIG_MM_REGIONS=3 -CONFIG_MTD=y -CONFIG_MTD_BYTE_WRITE=y -CONFIG_MTD_PARTITION=y -CONFIG_MTD_RAMTRON=y -CONFIG_NAME_MAX=40 -CONFIG_NFILE_DESCRIPTORS=12 -CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y -CONFIG_NSH_ARGCAT=y -CONFIG_NSH_BUILTIN_APPS=y -CONFIG_NSH_CMDPARMS=y -CONFIG_NSH_CROMFSETC=y -CONFIG_NSH_DISABLE_IFCONFIG=y -CONFIG_NSH_DISABLE_IFUPDOWN=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_DISABLE_TELNETD=y -CONFIG_NSH_LINELEN=128 -CONFIG_NSH_MAXARGUMENTS=15 -CONFIG_NSH_NESTDEPTH=8 -CONFIG_NSH_QUOTE=y -CONFIG_NSH_ROMFSETC=y -CONFIG_NSH_ROMFSSECTSIZE=128 -CONFIG_NSH_STRERROR=y -CONFIG_NSH_VARS=y -CONFIG_PIPES=y -CONFIG_PREALLOC_TIMERS=50 -CONFIG_PRIORITY_INHERITANCE=y -CONFIG_PTHREAD_MUTEX_ROBUST=y -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y -CONFIG_RAM_SIZE=245760 -CONFIG_RAM_START=0x20010000 -CONFIG_RAW_BINARY=y -CONFIG_RTC_DATETIME=y -CONFIG_SCHED_ATEXIT=y -CONFIG_SCHED_HPWORK=y -CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1280 -CONFIG_SCHED_INSTRUMENTATION=y -CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y -CONFIG_SCHED_IRQMONITOR=y -CONFIG_SCHED_LPWORK=y -CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1632 -CONFIG_SCHED_WAITPID=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SDMMC1_SDIO_MODE=y -CONFIG_SDMMC1_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y -CONFIG_SERIAL_TERMIOS=y -CONFIG_SIG_DEFAULT=y -CONFIG_SIG_SIGALRM_ACTION=y -CONFIG_SIG_SIGUSR1_ACTION=y -CONFIG_SIG_SIGUSR2_ACTION=y -CONFIG_SIG_SIGWORK=4 -CONFIG_STACK_COLORATION=y -CONFIG_START_DAY=30 -CONFIG_START_MONTH=11 -CONFIG_STDIO_BUFFER_SIZE=256 -CONFIG_STM32F7_ADC1=y -CONFIG_STM32F7_BBSRAM=y -CONFIG_STM32F7_BBSRAM_FILES=5 -CONFIG_STM32F7_BKPSRAM=y -CONFIG_STM32F7_DMA1=y -CONFIG_STM32F7_DMA2=y -CONFIG_STM32F7_DMACAPABLE=y -CONFIG_STM32F7_FLOWCONTROL_BROKEN=y -CONFIG_STM32F7_I2C1=y -CONFIG_STM32F7_I2C2=y -CONFIG_STM32F7_I2C3=y -CONFIG_STM32F7_I2C4=y -CONFIG_STM32F7_I2C_DYNTIMEO=y -CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 -CONFIG_STM32F7_OTGFS=y -CONFIG_STM32F7_PROGMEM=y -CONFIG_STM32F7_PWR=y -CONFIG_STM32F7_RTC=y -CONFIG_STM32F7_RTC_HSECLOCK=y -CONFIG_STM32F7_RTC_MAGIC_REG=1 -CONFIG_STM32F7_SAVE_CRASHDUMP=y -CONFIG_STM32F7_SDMMC1=y -CONFIG_STM32F7_SDMMC_DMA=y -CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y -CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y -CONFIG_STM32F7_SPI1=y -CONFIG_STM32F7_SPI1_DMA=y -CONFIG_STM32F7_SPI1_DMA_BUFFER=1024 -CONFIG_STM32F7_SPI2=y -CONFIG_STM32F7_SPI4=y -CONFIG_STM32F7_SPI5=y -CONFIG_STM32F7_SPI6=y -CONFIG_STM32F7_SPI_DMA=y -CONFIG_STM32F7_SPI_DMATHRESHOLD=8 -CONFIG_STM32F7_TIM10=y -CONFIG_STM32F7_TIM11=y -CONFIG_STM32F7_UART4=y -CONFIG_STM32F7_UART7=y -CONFIG_STM32F7_UART8=y -CONFIG_STM32F7_USART1=y -CONFIG_STM32F7_USART2=y -CONFIG_STM32F7_USART3=y -CONFIG_STM32F7_USART6=y -CONFIG_STM32F7_USART_BREAKS=y -CONFIG_STM32F7_USART_INVERT=y -CONFIG_STM32F7_USART_SINGLEWIRE=y -CONFIG_STM32F7_USART_SWAP=y -CONFIG_STM32F7_WWDG=y -CONFIG_SYSTEM_CDCACM=y -CONFIG_SYSTEM_NSH=y -CONFIG_TASK_NAME_SIZE=24 -CONFIG_UART4_BAUD=57600 -CONFIG_UART4_RXBUFSIZE=600 -CONFIG_UART4_RXDMA=y -CONFIG_UART4_TXBUFSIZE=1500 -CONFIG_UART7_BAUD=57600 -CONFIG_UART7_RXBUFSIZE=600 -CONFIG_UART7_SERIAL_CONSOLE=y -CONFIG_UART7_TXBUFSIZE=1500 -CONFIG_UART8_BAUD=57600 -CONFIG_UART8_RXBUFSIZE=600 -CONFIG_UART8_RXDMA=y -CONFIG_UART8_TXBUFSIZE=1500 -CONFIG_USART1_BAUD=57600 -CONFIG_USART1_RXBUFSIZE=600 -CONFIG_USART1_TXBUFSIZE=1500 -CONFIG_USART2_BAUD=57600 -CONFIG_USART2_IFLOWCONTROL=y -CONFIG_USART2_OFLOWCONTROL=y -CONFIG_USART2_RXBUFSIZE=600 -CONFIG_USART2_RXDMA=y -CONFIG_USART2_TXBUFSIZE=1500 -CONFIG_USART3_BAUD=57600 -CONFIG_USART3_IFLOWCONTROL=y -CONFIG_USART3_OFLOWCONTROL=y -CONFIG_USART3_RXBUFSIZE=600 -CONFIG_USART3_RXDMA=y -CONFIG_USART3_TXBUFSIZE=3000 -CONFIG_USART3_TXDMA=y -CONFIG_USART6_BAUD=57600 -CONFIG_USART6_RXBUFSIZE=600 -CONFIG_USART6_RXDMA=y -CONFIG_USART6_TXBUFSIZE=1500 -CONFIG_USBDEV=y -CONFIG_USBDEV_BUSPOWERED=y -CONFIG_USBDEV_MAXPOWER=500 -CONFIG_USEC_PER_TICK=1000 -CONFIG_USERMAIN_STACKSIZE=2944 -CONFIG_USER_ENTRYPOINT="nsh_main" diff --git a/boards/px4/fmu-v5/optimized.cmake b/boards/px4/fmu-v5/optimized.cmake index 46a2d2599c..c63b47973c 100644 --- a/boards/px4/fmu-v5/optimized.cmake +++ b/boards/px4/fmu-v5/optimized.cmake @@ -111,6 +111,7 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake index 94d03e80e9..331e5fc07c 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -86,6 +86,7 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index 615857d1b2..37c253aead 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -114,10 +114,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index d497763de4..387c142a51 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -118,10 +118,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + #fake_gps #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control #hello diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v5/uavcanv0periph.cmake similarity index 73% rename from boards/px4/fmu-v4/stackcheck.cmake rename to boards/px4/fmu-v5/uavcanv0periph.cmake index 45e7577208..2459f85ac6 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v5/uavcanv0periph.cmake @@ -2,20 +2,24 @@ px4_add_board( PLATFORM nuttx VENDOR px4 - MODEL fmu-v4 - LABEL stackcheck + MODEL fmu-v5 + LABEL uavcanv0periph TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m4 - CONSTRAINED_MEMORY + ARCHITECTURE cortex-m7 ROMFSROOT px4fmu_common - TESTING - #UAVCAN_INTERFACES 1 + IO px4_io-v2_default + #TESTING + UAVCAN_INTERFACES 2 + UAVCAN_PERIPHERALS + cuav_can-gps-v1_default SERIAL_PORTS - GPS1:/dev/ttyS3 + GPS1:/dev/ttyS0 TEL1:/dev/ttyS1 TEL2:/dev/ttyS2 + TEL4:/dev/ttyS3 DRIVERS adc/board_adc + #adc/ads1115 barometer # all available barometer drivers batt_smbus camera_capture @@ -24,36 +28,40 @@ px4_add_board( distance_sensor # all available distance sensor drivers dshot gps - heater + #heater #imu # all available imu drivers - #imu/analog_devices/adis16448 + #imu/adis16448 #imu/adis16477 #imu/adis16497 + imu/bosch/bmi055 imu/invensense/icm20602 - imu/invensense/icm20608g - #imu/invensense/icm40609d - #imu/invensense/mpu6500 - #imu/invensense/mpu9250 - irlock - lights/blinkm + imu/invensense/icm20689 + #imu/mpu6000 # legacy icm20602/icm20689 driver + #irlock + #lights/blinkm lights/rgbled lights/rgbled_ncp5623c + lights/rgbled_pwm magnetometer # all available magnetometer drivers optical_flow # all available optical flow drivers #osd pca9685 + #pca9685_pwm_out + power_monitor/ina226 #protocol_splitter - pwm_input + #pwm_input pwm_out_sim pwm_out + px4io rc_input #roboclaw + #rpm safety_button - tap_esc + #tap_esc telemetry # all available telemetry drivers - test_ppm + #test_ppm tone_alarm - #uavcan + uavcan MODULES airspeed_selector attitude_estimator_q @@ -62,6 +70,7 @@ px4_add_board( commander dataman ekf2 + #esc_battery events flight_mode_manager fw_att_control @@ -69,7 +78,7 @@ px4_add_board( land_detector landing_target_estimator load_mon - local_position_estimator + #local_position_estimator logger mavlink mc_att_control @@ -79,18 +88,20 @@ px4_add_board( #micrortps_bridge navigator rc_update - rover_pos_control + #rover_pos_control sensors - sih + #sih temperature_compensation #uuv_att_control + #uuv_pos_control vmount vtol_att_control SYSTEMCMDS - bl_update - #dmesg - dumpfile + #bl_update + dmesg + #dumpfile esc_calib + #gpio hardfault_log i2cdetect led_control @@ -107,15 +118,19 @@ px4_add_board( reflect sd_bench system_time - tests # tests and test runner + #tests # tests and test runner top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + #fake_gyro + #fake_magnetometer #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + #gyro_fft #hello #hwtest # Hardware test #matlab_csv_serial diff --git a/boards/px4/fmu-v5/uavcanv1.cmake b/boards/px4/fmu-v5/uavcanv1.cmake index b75b5353ff..765326d8f1 100644 --- a/boards/px4/fmu-v5/uavcanv1.cmake +++ b/boards/px4/fmu-v5/uavcanv1.cmake @@ -8,7 +8,7 @@ px4_add_board( ARCHITECTURE cortex-m7 ROMFSROOT px4fmu_common IO px4_io-v2_default - TESTING + #TESTING UAVCAN_INTERFACES 2 SERIAL_PORTS GPS1:/dev/ttyS0 @@ -57,7 +57,7 @@ px4_add_board( safety_button tap_esc telemetry # all available telemetry drivers - test_ppm + #test_ppm tone_alarm #uavcan # legacy v0 uavcan_v1 @@ -92,6 +92,7 @@ px4_add_board( sih temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS @@ -116,24 +117,26 @@ px4_add_board( reflect sd_bench system_time - tests # tests and test runner + #tests # tests and test runner top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES - fake_gyro - fake_magnetometer - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - gyro_fft - hello - hwtest # Hardware test + #fake_gps + #fake_gyro + #fake_magnetometer + #fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control + #gyro_fft + #hello + #hwtest # Hardware test #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item + #px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html + #px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html + #rover_steering_control # Rover example app + #uuv_example_app + #work_item ) diff --git a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake index e9bdbce90e..9c7ac206ac 100644 --- a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake +++ b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake @@ -113,11 +113,13 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue serial_test EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index d760d664df..58cc476cfb 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -117,10 +117,12 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/px4/fmu-v6u/default.cmake b/boards/px4/fmu-v6u/default.cmake index 4c839c807c..7e23ff4459 100644 --- a/boards/px4/fmu-v6u/default.cmake +++ b/boards/px4/fmu-v6u/default.cmake @@ -116,11 +116,13 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue serial_test EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/px4/fmu-v6u/stackcheck.cmake b/boards/px4/fmu-v6u/stackcheck.cmake deleted file mode 100644 index 4c839c807c..0000000000 --- a/boards/px4/fmu-v6u/stackcheck.cmake +++ /dev/null @@ -1,133 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v6u - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - BUILD_BOOTLOADER - TESTING - UAVCAN_INTERFACES 1 - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS6 - TEL2:/dev/ttyS4 - TEL3:/dev/ttyS1 - GPS2:/dev/ttyS7 - DRIVERS - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/adis16477 - imu/adis16497 - imu/bosch/bmi088 - imu/invensense/icm20649 - imu/invensense/icm20602 - imu/invensense/icm42605 - imu/invensense/icm42688p - irlock - lights/blinkm - lights/rgbled - lights/rgbled_ncp5623c - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - #osd - pca9685 - power_monitor/ina226 - #protocol_splitter -# pwm_input - Need to create arch/stm32 arch/stm32h7 arch/kinetis and reloacate -# all arch dependant code there - pwm_out_sim - pwm_out - rc_input - roboclaw - rpm - safety_button - tap_esc - telemetry # all available telemetry drivers - test_ppm - tone_alarm - uavcan - MODULES - airspeed_selector - attitude_estimator_q - battery_status - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - system_time - tests # tests and test runner - top - topic_listener - tune_control - usb_connected - ver - work_queue - serial_test - EXAMPLES - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/px4/fmu-v6x/default.cmake b/boards/px4/fmu-v6x/default.cmake index 0444cfb5d6..9afa6612de 100644 --- a/boards/px4/fmu-v6x/default.cmake +++ b/boards/px4/fmu-v6x/default.cmake @@ -114,11 +114,13 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue serial_test EXAMPLES + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello hwtest # Hardware test diff --git a/boards/px4/fmu-v6x/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v6x/nuttx-config/stackcheck/defconfig deleted file mode 100644 index 84cb348d0f..0000000000 --- a/boards/px4/fmu-v6x/nuttx-config/stackcheck/defconfig +++ /dev/null @@ -1,280 +0,0 @@ -# -# This file is autogenerated: PLEASE DO NOT EDIT IT. -# -# You can use "make menuconfig" to make any modifications to the installed .config file. -# You can then do "make savedefconfig" to generate a new defconfig file that includes your -# modifications. -# -# CONFIG_DISABLE_ENVIRON is not set -# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set -# CONFIG_MMCSD_HAVE_CARDDETECT is not set -# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set -# CONFIG_MMCSD_MMCSUPPORT is not set -# CONFIG_MMCSD_SPI is not set -# CONFIG_NSH_DISABLEBG is not set -# CONFIG_NSH_DISABLESCRIPT is not set -# CONFIG_NSH_DISABLE_ARP is not set -# CONFIG_NSH_DISABLE_DF is not set -# CONFIG_NSH_DISABLE_EXEC is not set -# CONFIG_NSH_DISABLE_GET is not set -# CONFIG_NSH_DISABLE_ITEF is not set -# CONFIG_NSH_DISABLE_LOOPS is not set -# CONFIG_NSH_DISABLE_SEMICOLON is not set -# CONFIG_NSH_DISABLE_TIME is not set -CONFIG_ARCH="arm" -CONFIG_ARCH_BOARD_CUSTOM=y -CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config" -CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y -CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" -CONFIG_ARCH_CHIP="stm32h7" -CONFIG_ARCH_CHIP_STM32H743ZI=y -CONFIG_ARCH_CHIP_STM32H7=y -CONFIG_ARCH_INTERRUPTSTACK=512 -CONFIG_ARCH_STACKDUMP=y -CONFIG_ARMV7M_BASEPRI_WAR=y -CONFIG_ARMV7M_DCACHE=y -CONFIG_ARMV7M_DTCM=y -CONFIG_ARMV7M_ICACHE=y -CONFIG_ARMV7M_MEMCPY=y -CONFIG_ARMV7M_STACKCHECK=y -CONFIG_ARMV7M_USEBASEPRI=y -CONFIG_BOARDCTL_RESET=y -CONFIG_BOARD_CRASHDUMP=y -CONFIG_BOARD_LOOPSPERMSEC=95751 -CONFIG_BOARD_RESET_ON_ASSERT=2 -CONFIG_BUILTIN=y -CONFIG_C99_BOOL8=y -CONFIG_CDCACM=y -CONFIG_CDCACM_PRODUCTID=0x0035 -CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v6X.x" -CONFIG_CDCACM_RXBUFSIZE=600 -CONFIG_CDCACM_TXBUFSIZE=12000 -CONFIG_CDCACM_VENDORID=0x3185 -CONFIG_CDCACM_VENDORSTR="Auterion" -CONFIG_CLOCK_MONOTONIC=y -CONFIG_DEBUG_FULLOPT=y -CONFIG_DEBUG_HARDFAULT_ALERT=y -CONFIG_DEBUG_MEMFAULT=y -CONFIG_DEBUG_SYMBOLS=y -CONFIG_DEFAULT_SMALL=y -CONFIG_DEV_FIFO_SIZE=0 -CONFIG_DEV_PIPE_MAXSIZE=1024 -CONFIG_DEV_PIPE_SIZE=70 -CONFIG_ETH0_PHY_LAN8742A=y -CONFIG_EXPERIMENTAL=y -CONFIG_FAT_DMAMEMORY=y -CONFIG_FAT_LCNAMES=y -CONFIG_FAT_LFN=y -CONFIG_FAT_LFN_ALIAS_HASH=y -CONFIG_FDCLONE_STDIO=y -CONFIG_FS_BINFS=y -CONFIG_FS_CROMFS=y -CONFIG_FS_FAT=y -CONFIG_FS_FATTIME=y -CONFIG_FS_PROCFS=y -CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y -CONFIG_FS_PROCFS_REGISTER=y -CONFIG_FS_ROMFS=y -CONFIG_GRAN=y -CONFIG_GRAN_INTR=y -CONFIG_HAVE_CXX=y -CONFIG_HAVE_CXXINITIALIZE=y -CONFIG_I2C=y -CONFIG_I2C_RESET=y -CONFIG_IDLETHREAD_STACKSIZE=750 -CONFIG_IOB_NBUFFERS=24 -CONFIG_IOB_THROTTLE=0 -CONFIG_LIBC_FLOATINGPOINT=y -CONFIG_LIBC_LONG_LONG=y -CONFIG_LIBC_STRERROR=y -CONFIG_MAX_TASKS=64 -CONFIG_MEMSET_64BIT=y -CONFIG_MEMSET_OPTSPEED=y -CONFIG_MMCSD=y -CONFIG_MMCSD_SDIO=y -CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y -CONFIG_MM_REGIONS=3 -CONFIG_MTD=y -CONFIG_MTD_BYTE_WRITE=y -CONFIG_MTD_PARTITION=y -CONFIG_MTD_PROGMEM=y -CONFIG_MTD_RAMTRON=y -CONFIG_NET=y -CONFIG_NETDB_DNSCLIENT=y -CONFIG_NETDB_DNSCLIENT_ENTRIES=8 -CONFIG_NETDB_DNSSERVER_NOADDR=y -CONFIG_NETDEV_PHY_IOCTL=y -CONFIG_NETINIT_DRIPADDR=0XC0A800FE -CONFIG_NETINIT_IPADDR=0XC0A8007B -CONFIG_NETINIT_THREAD=y -CONFIG_NETINIT_THREAD_PRIORITY=49 -CONFIG_NETUTILS_TELNETD=y -CONFIG_NET_ARP_IPIN=y -CONFIG_NET_ARP_SEND=y -CONFIG_NET_BROADCAST=y -CONFIG_NET_ICMP=y -CONFIG_NET_ICMP_SOCKET=y -CONFIG_NET_SOCKOPTS=y -CONFIG_NET_SOLINGER=y -CONFIG_NET_TCP=y -CONFIG_NET_TCPBACKLOG=y -CONFIG_NET_TCP_WRITE_BUFFERS=y -CONFIG_NET_UDP=y -CONFIG_NET_UDP_CHECKSUMS=y -CONFIG_NFILE_DESCRIPTORS=12 -CONFIG_NSH_ARCHINIT=y -CONFIG_NSH_ARCHROMFS=y -CONFIG_NSH_ARGCAT=y -CONFIG_NSH_BUILTIN_APPS=y -CONFIG_NSH_CMDPARMS=y -CONFIG_NSH_CROMFSETC=y -CONFIG_NSH_DISABLE_MB=y -CONFIG_NSH_DISABLE_MH=y -CONFIG_NSH_LINELEN=128 -CONFIG_NSH_MAXARGUMENTS=15 -CONFIG_NSH_NESTDEPTH=8 -CONFIG_NSH_QUOTE=y -CONFIG_NSH_ROMFSETC=y -CONFIG_NSH_ROMFSSECTSIZE=128 -CONFIG_NSH_STRERROR=y -CONFIG_NSH_TELNET=y -CONFIG_NSH_TELNET_LOGIN=y -CONFIG_NSH_VARS=y -CONFIG_PIPES=y -CONFIG_PREALLOC_TIMERS=50 -CONFIG_PRIORITY_INHERITANCE=y -CONFIG_PTHREAD_MUTEX_ROBUST=y -CONFIG_PTHREAD_STACK_MIN=512 -CONFIG_RAMTRON_SETSPEED=y -CONFIG_RAMTRON_WRITEWAIT=y -CONFIG_RAM_SIZE=245760 -CONFIG_RAM_START=0x20010000 -CONFIG_RAW_BINARY=y -CONFIG_RTC_DATETIME=y -CONFIG_SCHED_ATEXIT=y -CONFIG_SCHED_HPWORK=y -CONFIG_SCHED_HPWORKPRIORITY=249 -CONFIG_SCHED_HPWORKSTACKSIZE=1280 -CONFIG_SCHED_INSTRUMENTATION=y -CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y -CONFIG_SCHED_LPWORK=y -CONFIG_SCHED_LPWORKPRIORITY=50 -CONFIG_SCHED_LPWORKSTACKSIZE=1632 -CONFIG_SCHED_WAITPID=y -CONFIG_SDCLONE_DISABLE=y -CONFIG_SDMMC2_SDIO_PULLUP=y -CONFIG_SEM_NNESTPRIO=8 -CONFIG_SEM_PREALLOCHOLDERS=0 -CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y -CONFIG_SERIAL_TERMIOS=y -CONFIG_SIG_DEFAULT=y -CONFIG_SIG_SIGALRM_ACTION=y -CONFIG_SIG_SIGUSR1_ACTION=y -CONFIG_SIG_SIGUSR2_ACTION=y -CONFIG_SIG_SIGWORK=4 -CONFIG_STACK_COLORATION=y -CONFIG_START_DAY=30 -CONFIG_START_MONTH=11 -CONFIG_STDIO_BUFFER_SIZE=256 -CONFIG_STM32H7_ADC1=y -CONFIG_STM32H7_ADC3=y -CONFIG_STM32H7_BBSRAM=y -CONFIG_STM32H7_BBSRAM_FILES=5 -CONFIG_STM32H7_BKPSRAM=y -CONFIG_STM32H7_DMA1=y -CONFIG_STM32H7_DMA2=y -CONFIG_STM32H7_DMACAPABLE=y -CONFIG_STM32H7_DTCMEXCLUDE=y -CONFIG_STM32H7_DTCM_PROCFS=y -CONFIG_STM32H7_ETHMAC=y -CONFIG_STM32H7_FLASH_OVERRIDE_I=y -CONFIG_STM32H7_FLOWCONTROL_BROKEN=y -CONFIG_STM32H7_I2C1=y -CONFIG_STM32H7_I2C2=y -CONFIG_STM32H7_I2C3=y -CONFIG_STM32H7_I2C4=y -CONFIG_STM32H7_I2C_DYNTIMEO=y -CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 -CONFIG_STM32H7_OTGFS=y -CONFIG_STM32H7_PHYSR=31 -CONFIG_STM32H7_PHYSR_100MBPS=0x8 -CONFIG_STM32H7_PHYSR_FULLDUPLEX=0x10 -CONFIG_STM32H7_PHYSR_MODE=0x10 -CONFIG_STM32H7_PHYSR_SPEED=0x8 -CONFIG_STM32H7_PROGMEM=y -CONFIG_STM32H7_RTC=y -CONFIG_STM32H7_RTC_MAGIC_REG=1 -CONFIG_STM32H7_SAVE_CRASHDUMP=y -CONFIG_STM32H7_SDMMC2=y -CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y -CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y -CONFIG_STM32H7_SPI1=y -CONFIG_STM32H7_SPI1_DMA=y -CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 -CONFIG_STM32H7_SPI2=y -CONFIG_STM32H7_SPI2_DMA=y -CONFIG_STM32H7_SPI2_DMA_BUFFER=4096 -CONFIG_STM32H7_SPI3=y -CONFIG_STM32H7_SPI3_DMA=y -CONFIG_STM32H7_SPI3_DMA_BUFFER=1024 -CONFIG_STM32H7_SPI5=y -CONFIG_STM32H7_SPI6=y -CONFIG_STM32H7_SPI6_DMA=y -CONFIG_STM32H7_SPI6_DMA_BUFFER=1024 -CONFIG_STM32H7_SPI_DMA=y -CONFIG_STM32H7_SPI_DMATHRESHOLD=8 -CONFIG_STM32H7_TIM12=y -CONFIG_STM32H7_TIM1=y -CONFIG_STM32H7_TIM4=y -CONFIG_STM32H7_TIM5=y -CONFIG_STM32H7_UART4=y -CONFIG_STM32H7_UART5=y -CONFIG_STM32H7_UART7=y -CONFIG_STM32H7_UART8=y -CONFIG_STM32H7_USART1=y -CONFIG_STM32H7_USART2=y -CONFIG_STM32H7_USART3=y -CONFIG_STM32H7_USART6=y -CONFIG_STM32H7_USART_BREAKS=y -CONFIG_STM32H7_USART_INVERT=y -CONFIG_STM32H7_USART_SINGLEWIRE=y -CONFIG_STM32H7_USART_SWAP=y -CONFIG_SYSTEM_CDCACM=y -CONFIG_SYSTEM_NSH=y -CONFIG_SYSTEM_PING=y -CONFIG_TASK_NAME_SIZE=24 -CONFIG_UART4_BAUD=57600 -CONFIG_UART4_RXBUFSIZE=600 -CONFIG_UART4_TXBUFSIZE=1500 -CONFIG_UART5_IFLOWCONTROL=y -CONFIG_UART5_OFLOWCONTROL=y -CONFIG_UART7_BAUD=57600 -CONFIG_UART7_IFLOWCONTROL=y -CONFIG_UART7_OFLOWCONTROL=y -CONFIG_UART7_RXBUFSIZE=600 -CONFIG_UART7_TXBUFSIZE=3000 -CONFIG_UART8_BAUD=57600 -CONFIG_UART8_RXBUFSIZE=600 -CONFIG_UART8_TXBUFSIZE=1500 -CONFIG_USART1_BAUD=57600 -CONFIG_USART1_RXBUFSIZE=600 -CONFIG_USART1_TXBUFSIZE=1500 -CONFIG_USART2_BAUD=57600 -CONFIG_USART2_IFLOWCONTROL=y -CONFIG_USART2_OFLOWCONTROL=y -CONFIG_USART2_RXBUFSIZE=600 -CONFIG_USART2_TXBUFSIZE=3000 -CONFIG_USART3_BAUD=57600 -CONFIG_USART3_RXBUFSIZE=180 -CONFIG_USART3_SERIAL_CONSOLE=y -CONFIG_USART3_TXBUFSIZE=1500 -CONFIG_USART6_BAUD=57600 -CONFIG_USART6_RXBUFSIZE=600 -CONFIG_USART6_TXBUFSIZE=1500 -CONFIG_USBDEV=y -CONFIG_USBDEV_BUSPOWERED=y -CONFIG_USBDEV_MAXPOWER=500 -CONFIG_USEC_PER_TICK=1000 -CONFIG_USERMAIN_STACKSIZE=2944 -CONFIG_USER_ENTRYPOINT="nsh_main" -CONFIG_WATCHDOG=y diff --git a/boards/px4/fmu-v6x/stackcheck.cmake b/boards/px4/fmu-v6x/stackcheck.cmake deleted file mode 100644 index 0444cfb5d6..0000000000 --- a/boards/px4/fmu-v6x/stackcheck.cmake +++ /dev/null @@ -1,131 +0,0 @@ - -px4_add_board( - PLATFORM nuttx - VENDOR px4 - MODEL fmu-v6x - LABEL default - TOOLCHAIN arm-none-eabi - ARCHITECTURE cortex-m7 - ROMFSROOT px4fmu_common - BUILD_BOOTLOADER - IO px4_io-v2_default - TESTING -# UAVCAN_INTERFACES 2 - No H7 or FD can support in UAVCAN - SERIAL_PORTS - GPS1:/dev/ttyS0 - TEL1:/dev/ttyS6 - TEL2:/dev/ttyS4 - TEL3:/dev/ttyS1 - GPS2:/dev/ttyS7 - DRIVERS - adc/board_adc - barometer # all available barometer drivers - batt_smbus - camera_capture - camera_trigger - differential_pressure # all available differential pressure drivers - distance_sensor # all available distance sensor drivers - dshot - gps - heater - #imu # all available imu drivers - imu/analog_devices/adis16448 - imu/adis16477 - imu/adis16497 - imu/bosch/bmi088 - imu/invensense/icm20649 - irlock - lights/blinkm - lights/rgbled - lights/rgbled_ncp5623c - magnetometer # all available magnetometer drivers - optical_flow # all available optical flow drivers - #osd - pca9685 - power_monitor/ina226 - #protocol_splitter -# pwm_input - Need to create arch/stm32 arch/stm32h7 arch/kinetis and reloacate -# all arch dependant code there - pwm_out_sim - pwm_out - px4io - rc_input - roboclaw - rpm - safety_button - tap_esc - telemetry # all available telemetry drivers - test_ppm - tone_alarm -# uavcan - No H7 or FD can support in UAVCAN yet - MODULES - airspeed_selector - attitude_estimator_q - camera_feedback - commander - dataman - ekf2 - esc_battery - events - flight_mode_manager - fw_att_control - fw_pos_control_l1 - land_detector - landing_target_estimator - load_mon - local_position_estimator - logger - mavlink - mc_att_control - mc_hover_thrust_estimator - mc_pos_control - mc_rate_control - #micrortps_bridge - navigator - rc_update - rover_pos_control - sensors - sih - temperature_compensation - vmount - vtol_att_control - SYSTEMCMDS - bl_update - dmesg - dumpfile - esc_calib - hardfault_log - i2cdetect - led_control - mft - mixer - motor_ramp - motor_test - mtd - nshterm - param - perf - pwm - reboot - reflect - sd_bench - system_time - tests # tests and test runner - top - topic_listener - tune_control - usb_connected - ver - work_queue - serial_test - EXAMPLES - fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control - hello - hwtest # Hardware test - #matlab_csv_serial - px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html - px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html - rover_steering_control # Rover example app - uuv_example_app - work_item - ) diff --git a/boards/px4/raspberrypi/default.cmake b/boards/px4/raspberrypi/default.cmake index 7e64927351..36d812f838 100644 --- a/boards/px4/raspberrypi/default.cmake +++ b/boards/px4/raspberrypi/default.cmake @@ -79,9 +79,11 @@ px4_add_board( #top topic_listener tune_control + uorb ver work_queue EXAMPLES + fake_gps dyn_hello # dynamically loading modules example fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/px4/sitl/ctrlalloc.cmake b/boards/px4/sitl/ctrlalloc.cmake index 61c126f8fc..9922036c8e 100644 --- a/boards/px4/sitl/ctrlalloc.cmake +++ b/boards/px4/sitl/ctrlalloc.cmake @@ -82,10 +82,12 @@ px4_add_board( #top topic_listener tune_control + uorb ver work_queue EXAMPLES dyn_hello # dynamically loading modules example + fake_gps fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index cccb9d9409..217b262699 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -80,10 +80,12 @@ px4_add_board( #top topic_listener tune_control + uorb ver work_queue EXAMPLES dyn_hello # dynamically loading modules example + fake_gps fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/px4/sitl/nolockstep.cmake b/boards/px4/sitl/nolockstep.cmake index f797304951..3c45559194 100644 --- a/boards/px4/sitl/nolockstep.cmake +++ b/boards/px4/sitl/nolockstep.cmake @@ -80,10 +80,12 @@ px4_add_board( #top topic_listener tune_control + uorb ver work_queue EXAMPLES dyn_hello # dynamically loading modules example + fake_gps fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index cba4e8adfc..9f043c589f 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -79,10 +79,12 @@ px4_add_board( #top topic_listener tune_control + uorb ver work_queue EXAMPLES dyn_hello # dynamically loading modules example + fake_gps fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index 4c101974a4..5205f770df 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -78,10 +78,12 @@ px4_add_board( #top topic_listener tune_control + uorb ver work_queue EXAMPLES dyn_hello # dynamically loading modules example + fake_gps fake_magnetometer fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello diff --git a/boards/scumaker/pilotpi/arm64.cmake b/boards/scumaker/pilotpi/arm64.cmake index 9674354dd0..3684b2990f 100644 --- a/boards/scumaker/pilotpi/arm64.cmake +++ b/boards/scumaker/pilotpi/arm64.cmake @@ -79,10 +79,12 @@ px4_add_board( #top topic_listener tune_control + uorb ver work_queue EXAMPLES dyn_hello # dynamically loading modules example + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test diff --git a/boards/scumaker/pilotpi/default.cmake b/boards/scumaker/pilotpi/default.cmake index 853dbbf041..4dcd5bed6b 100644 --- a/boards/scumaker/pilotpi/default.cmake +++ b/boards/scumaker/pilotpi/default.cmake @@ -79,10 +79,12 @@ px4_add_board( #top topic_listener tune_control + uorb ver work_queue EXAMPLES dyn_hello # dynamically loading modules example + fake_gps fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control hello #hwtest # Hardware test diff --git a/boards/spracing/h7extreme/default.cmake b/boards/spracing/h7extreme/default.cmake index 9c7f0ada3a..e82de24a40 100644 --- a/boards/spracing/h7extreme/default.cmake +++ b/boards/spracing/h7extreme/default.cmake @@ -107,6 +107,7 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue diff --git a/boards/uvify/core/default.cmake b/boards/uvify/core/default.cmake index 4c7663d9b1..12d8879e1d 100644 --- a/boards/uvify/core/default.cmake +++ b/boards/uvify/core/default.cmake @@ -88,6 +88,7 @@ px4_add_board( top topic_listener tune_control + uorb usb_connected ver work_queue diff --git a/cmake/gtest/px4_add_gtest.cmake b/cmake/gtest/px4_add_gtest.cmake index 77b2b5c24f..5560fca062 100644 --- a/cmake/gtest/px4_add_gtest.cmake +++ b/cmake/gtest/px4_add_gtest.cmake @@ -92,7 +92,7 @@ function(px4_add_functional_gtest) target_link_libraries(${TESTNAME} ${LINKLIBS} gtest_functional_main px4_layer px4_platform - modules__uORB + uORB systemlib cdev px4_work_queue diff --git a/cmake/px4_add_board.cmake b/cmake/px4_add_board.cmake index df9a339c71..459b27b8fa 100644 --- a/cmake/px4_add_board.cmake +++ b/cmake/px4_add_board.cmake @@ -50,6 +50,7 @@ # [ IO ] # [ BOOTLOADER ] # [ UAVCAN_INTERFACES ] +# [ UAVCAN_PERIPHERALS ] # [ DRIVERS ] # [ MODULES ] # [ SYSTEMCMDS ] @@ -74,6 +75,7 @@ # IO : name of IO board to be built and included in the ROMFS (requires a valid ROMFSROOT) # BOOTLOADER : bootloader file to include for flashing via bl_update (currently NuttX only) # UAVCAN_INTERFACES : number of interfaces for UAVCAN +# UAVCAN_PERIPHERALS : list of UAVCAN peripheral firmware to build and embed # DRIVERS : list of drivers to build for this board (relative to src/drivers) # MODULES : list of modules to build for this board (relative to src/modules) # SYSTEMCMDS : list of system commands to build for this board (relative to src/systemcmds) @@ -154,6 +156,7 @@ function(px4_add_board) SYSTEMCMDS EXAMPLES SERIAL_PORTS + UAVCAN_PERIPHERALS EMBEDDED_METADATA OPTIONS BUILD_BOOTLOADER @@ -236,6 +239,10 @@ function(px4_add_board) if(IO) set(config_io_board ${IO} CACHE INTERNAL "IO" FORCE) endif() + + if(UAVCAN_PERIPHERALS) + set(config_uavcan_peripheral_firmware ${UAVCAN_PERIPHERALS} CACHE INTERNAL "UAVCAN peripheral firmware" FORCE) + endif() endif() if(UAVCAN_INTERFACES) diff --git a/cmake/px4_add_common_flags.cmake b/cmake/px4_add_common_flags.cmake index 699d976641..546cbfce30 100644 --- a/cmake/px4_add_common_flags.cmake +++ b/cmake/px4_add_common_flags.cmake @@ -180,7 +180,9 @@ function(px4_add_common_flags) ${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/src/px4/${PX4_CHIP_MANUFACTURER}/${PX4_CHIP}/include ${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/src/px4/common/include + ${PX4_SOURCE_DIR}/platforms/common ${PX4_SOURCE_DIR}/platforms/common/include + ${PX4_SOURCE_DIR}/src ${PX4_SOURCE_DIR}/src/include ${PX4_SOURCE_DIR}/src/lib diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0 index 4e25dfa47b..bf7df5c01d 160000 --- a/mavlink/include/mavlink/v2.0 +++ b/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 4e25dfa47b28adc177ea7badad837d8b84173712 +Subproject commit bf7df5c01d856f174c3b98e5b6527e777b173935 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 64cfd389d4..388b8ea70e 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -41,6 +41,7 @@ set(msg_files adc_report.msg airspeed.msg airspeed_validated.msg + airspeed_wind.msg battery_status.msg camera_capture.msg camera_trigger.msg @@ -66,8 +67,16 @@ set(msg_files follow_target.msg generator_status.msg geofence_result.msg + gimbal_device_attitude_status.msg + gimbal_device_information.msg + gimbal_device_set_attitude.msg + gimbal_manager_information.msg + gimbal_manager_set_attitude.msg + gimbal_manager_set_manual_control.msg + gimbal_manager_status.msg gps_dump.msg gps_inject_data.msg + heater_status.msg home_position.msg hover_thrust_estimate.msg input_rc.msg @@ -127,11 +136,11 @@ set(msg_files sensor_selection.msg sensors_status_imu.msg system_power.msg + takeoff_status.msg task_stack_info.msg tecs_status.msg telemetry_status.msg test_motor.msg - takeoff_status.msg timesync.msg timesync_status.msg trajectory_bezier.msg @@ -173,7 +182,7 @@ set(msg_files vehicle_trajectory_waypoint.msg vtol_vehicle_status.msg wheel_encoders.msg - wind_estimate.msg + wind.msg yaw_estimator_status.msg ) diff --git a/msg/wind_estimate.msg b/msg/airspeed_wind.msg similarity index 74% rename from msg/wind_estimate.msg rename to msg/airspeed_wind.msg index e92de9f75f..03033bbbdd 100644 --- a/msg/wind_estimate.msg +++ b/msg/airspeed_wind.msg @@ -16,8 +16,7 @@ float32 beta_innov_var # Sideslip measurement innovation variance uint8 source # source of wind estimate -uint8 SOURCE_EKF = 0 # wind estimate source is the EKF -uint8 SOURCE_AS_BETA_ONLY = 1 # wind estimate from airspeed selector, only based on synthetic sideslip fusion -uint8 SOURCE_AS_SENSOR_1 = 2 # combined synthetic sideslip and airspeed fusion (data from first airspeed sensor) -uint8 SOURCE_AS_SENSOR_2 = 3 # combined synthetic sideslip and airspeed fusion (data from second airspeed sensor) -uint8 SOURCE_AS_SENSOR_3 = 4 # combined synthetic sideslip and airspeed fusion (data from third airspeed sensor) +uint8 SOURCE_AS_BETA_ONLY = 0 # wind estimate only based on synthetic sideslip fusion +uint8 SOURCE_AS_SENSOR_1 = 1 # combined synthetic sideslip and airspeed fusion (data from first airspeed sensor) +uint8 SOURCE_AS_SENSOR_2 = 2 # combined synthetic sideslip and airspeed fusion (data from second airspeed sensor) +uint8 SOURCE_AS_SENSOR_3 = 3 # combined synthetic sideslip and airspeed fusion (data from third airspeed sensor) diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index 5c3e765b50..7f8dc28cb0 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -105,6 +105,12 @@ uint16 solution_status_flags # Bitmask indicating which filter kinematic state o # 10 - True if the EKF has detected a GPS glitch # 11 - True if the EKF has detected bad accelerometer data +uint8 reset_count_vel_ne # number of horizontal position reset events (allow to wrap if count exceeds 255) +uint8 reset_count_vel_d # number of vertical velocity reset events (allow to wrap if count exceeds 255) +uint8 reset_count_pos_ne # number of horizontal position reset events (allow to wrap if count exceeds 255) +uint8 reset_count_pod_d # number of vertical position reset events (allow to wrap if count exceeds 255) +uint8 reset_count_quat # number of quaternion reset events (allow to wrap if count exceeds 255) + float32 time_slip # cumulative amount of time in seconds that the EKF inertial calculation has slipped relative to system time bool pre_flt_fail_innov_heading diff --git a/msg/gimbal_device_attitude_status.msg b/msg/gimbal_device_attitude_status.msg new file mode 100644 index 0000000000..de2d7e892b --- /dev/null +++ b/msg/gimbal_device_attitude_status.msg @@ -0,0 +1,18 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 target_system +uint8 target_component +uint16 device_flags + +uint16 DEVICE_FLAGS_RETRACT = 1 +uint16 DEVICE_FLAGS_NEUTRAL = 2 +uint16 DEVICE_FLAGS_ROLL_LOCK = 4 +uint16 DEVICE_FLAGS_PITCH_LOCK = 8 +uint16 DEVICE_FLAGS_YAW_LOCK = 16 + +float32[4] q +float32 angular_velocity_x +float32 angular_velocity_y +float32 angular_velocity_z + +uint32 failure_flags diff --git a/msg/gimbal_device_information.msg b/msg/gimbal_device_information.msg new file mode 100644 index 0000000000..8f7a416439 --- /dev/null +++ b/msg/gimbal_device_information.msg @@ -0,0 +1,36 @@ +uint64 timestamp # time since system start (microseconds) + +uint8[32] vendor_name +uint8[32] model_name +uint8[32] custom_name +uint32 firmware_version +uint32 hardware_version +uint64 uid + +uint16 cap_flags + +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT = 1 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL = 2 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS = 4 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW = 8 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_LOCK = 16 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS = 32 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW = 64 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK = 128 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS = 256 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW = 512 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK = 1024 +uint32 GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 + +uint16 custom_cap_flags + +float32 roll_min # [rad] +float32 roll_max # [rad] + +float32 pitch_min # [rad] +float32 pitch_max # [rad] + +float32 yaw_min # [rad] +float32 yaw_max # [rad] + +uint8 gimbal_device_compid diff --git a/msg/gimbal_device_set_attitude.msg b/msg/gimbal_device_set_attitude.msg new file mode 100644 index 0000000000..f224a23441 --- /dev/null +++ b/msg/gimbal_device_set_attitude.msg @@ -0,0 +1,17 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 target_system +uint8 target_component + +uint16 flags +uint32 GIMBAL_DEVICE_FLAGS_RETRACT = 1 +uint32 GIMBAL_DEVICE_FLAGS_NEUTRAL = 2 +uint32 GIMBAL_DEVICE_FLAGS_ROLL_LOCK = 4 +uint32 GIMBAL_DEVICE_FLAGS_PITCH_LOCK = 8 +uint32 GIMBAL_DEVICE_FLAGS_YAW_LOCK = 16 + +float32[4] q + +float32 angular_velocity_x +float32 angular_velocity_y +float32 angular_velocity_z diff --git a/msg/gimbal_manager_information.msg b/msg/gimbal_manager_information.msg new file mode 100644 index 0000000000..28db68a456 --- /dev/null +++ b/msg/gimbal_manager_information.msg @@ -0,0 +1,29 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 cap_flags + +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_RETRACT = 1 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL = 2 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_AXIS = 4 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_FOLLOW = 8 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK = 16 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS = 32 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_FOLLOW = 64 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK = 128 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS = 256 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_FOLLOW = 512 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK = 1024 +uint32 GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 +uint32 GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_LOCAL = 65536 +uint32 GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL = 131072 + +uint8 gimbal_device_id + +float32 roll_min # [rad] +float32 roll_max # [rad] + +float32 pitch_min # [rad] +float32 pitch_max # [rad] + +float32 yaw_min # [rad] +float32 yaw_max # [rad] diff --git a/msg/gimbal_manager_set_attitude.msg b/msg/gimbal_manager_set_attitude.msg new file mode 100644 index 0000000000..d88acca8b6 --- /dev/null +++ b/msg/gimbal_manager_set_attitude.msg @@ -0,0 +1,22 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 origin_sysid +uint8 origin_compid + +uint8 target_system +uint8 target_component + +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 + +uint32 flags +uint8 gimbal_device_id + +float32[4] q + +float32 angular_velocity_x +float32 angular_velocity_y +float32 angular_velocity_z diff --git a/msg/gimbal_manager_set_manual_control.msg b/msg/gimbal_manager_set_manual_control.msg new file mode 100644 index 0000000000..4061438f72 --- /dev/null +++ b/msg/gimbal_manager_set_manual_control.msg @@ -0,0 +1,21 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 origin_sysid +uint8 origin_compid + +uint8 target_system +uint8 target_component + +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 + +uint32 flags +uint8 gimbal_device_id + +float32 pitch # unitless -1..1, can be NAN +float32 yaw # unitless -1..1, can be NAN +float32 pitch_rate # unitless -1..1, can be NAN +float32 yaw_rate # unitless -1..1, can be NAN diff --git a/msg/gimbal_manager_status.msg b/msg/gimbal_manager_status.msg new file mode 100644 index 0000000000..002e5c90e7 --- /dev/null +++ b/msg/gimbal_manager_status.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 flags +uint8 gimbal_device_id +uint8 primary_control_sysid +uint8 primary_control_compid +uint8 secondary_control_sysid +uint8 secondary_control_compid diff --git a/msg/heater_status.msg b/msg/heater_status.msg new file mode 100644 index 0000000000..158d28fb99 --- /dev/null +++ b/msg/heater_status.msg @@ -0,0 +1,19 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 device_id + +bool heater_on + +float32 temperature_sensor +float32 temperature_target + +uint32 controller_period_usec +uint32 controller_time_on_usec + +float32 proportional_value +float32 integrator_value +float32 feed_forward_value + +uint8 MODE_GPIO = 1 +uint8 MODE_PX4IO = 2 +uint8 mode diff --git a/msg/input_rc.msg b/msg/input_rc.msg index 8857e4e6b8..8f333e7f66 100644 --- a/msg/input_rc.msg +++ b/msg/input_rc.msg @@ -15,6 +15,7 @@ uint8 RC_INPUT_SOURCE_PX4FMU_SUMD = 11 uint8 RC_INPUT_SOURCE_PX4FMU_DSM = 12 uint8 RC_INPUT_SOURCE_PX4IO_SUMD = 13 uint8 RC_INPUT_SOURCE_PX4FMU_CRSF = 14 +uint8 RC_INPUT_SOURCE_PX4FMU_GHST = 15 uint8 RC_INPUT_MAX_CHANNELS = 18 # Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. diff --git a/msg/templates/urtps/RtpsTopics.cpp.em b/msg/templates/urtps/RtpsTopics.cpp.em index a65c38c63e..e9e1b27105 100644 --- a/msg/templates/urtps/RtpsTopics.cpp.em +++ b/msg/templates/urtps/RtpsTopics.cpp.em @@ -139,10 +139,13 @@ bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr) @[ if topic == 'Timesync' or topic == 'timesync']@ if (getMsgSysID(&msg) == 0) { @[ end if]@ - // apply timestamp offset + // apply timestamps offset uint64_t timestamp = getMsgTimestamp(&msg); + uint64_t timestamp_sample = getMsgTimestampSample(&msg); _timesync->addOffset(timestamp); setMsgTimestamp(&msg, timestamp); + _timesync->addOffset(timestamp_sample); + setMsgTimestampSample(&msg, timestamp_sample); msg.serialize(scdr); ret = true; @[ if topic == 'Timesync' or topic == 'timesync']@ diff --git a/msg/templates/urtps/RtpsTopics.h.em b/msg/templates/urtps/RtpsTopics.h.em index 6d8d2975f4..4f3f9d4e15 100644 --- a/msg/templates/urtps/RtpsTopics.h.em +++ b/msg/templates/urtps/RtpsTopics.h.em @@ -63,6 +63,7 @@ except AttributeError: #include #include #include +#include #include "microRTPS_timesync.h" @@ -116,11 +117,28 @@ private: @[end for]@ @[end if]@ + // SFINAE + template struct hasTimestampSample{ + private: + static void detect(...); + template static decltype(std::declval().timestamp_sample()) detect(const U&); + public: + static constexpr bool value = std::is_same()))>::value; + }; + + template + inline typename std::enable_if::value, uint64_t>::type + getMsgTimestampSample_impl(const T*) { return 0; } + /** Msg metada Getters **/ @[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ template inline uint64_t getMsgTimestamp(const T* msg) { return msg->timestamp_(); } + template + inline typename std::enable_if::value, uint64_t>::type + getMsgTimestampSample_impl(const T* msg) { return msg->timestamp_sample_(); } + template inline uint8_t getMsgSysID(const T* msg) { return msg->sys_id_(); } @@ -130,6 +148,10 @@ private: template inline uint64_t getMsgTimestamp(const T* msg) { return msg->timestamp(); } + template + inline typename std::enable_if::value, uint64_t>::type + getMsgTimestampSample_impl(const T* msg) { return msg->timestamp_sample(); } + template inline uint8_t getMsgSysID(const T* msg) { return msg->sys_id(); } @@ -137,11 +159,22 @@ private: inline uint8_t getMsgSeq(const T* msg) { return msg->seq(); } @[end if]@ + template + inline uint64_t getMsgTimestampSample(const T* msg) { return getMsgTimestampSample_impl(msg); } + + template + inline typename std::enable_if::value, void>::type + setMsgTimestampSample_impl(T*, const uint64_t&) {} + /** Msg metadata Setters **/ @[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@ template inline void setMsgTimestamp(T* msg, const uint64_t& timestamp) { msg->timestamp_() = timestamp; } + template + inline typename std::enable_if::value, void>::type + setMsgTimestampSample_impl(T* msg, const uint64_t& timestamp_sample) { msg->timestamp_sample_() = timestamp_sample; } + template inline void setMsgSysID(T* msg, const uint8_t& sys_id) { msg->sys_id_() = sys_id; } @@ -151,6 +184,10 @@ private: template inline void setMsgTimestamp(T* msg, const uint64_t& timestamp) { msg->timestamp() = timestamp; } + template + inline typename std::enable_if::value, void>::type + setMsgTimestampSample_impl(T* msg, const uint64_t& timestamp_sample) { msg->timestamp_sample() = timestamp_sample; } + template inline void setMsgSysID(T* msg, const uint8_t& sys_id) { msg->sys_id() = sys_id; } @@ -158,6 +195,9 @@ private: inline void setMsgSeq(T* msg, const uint8_t& seq) { msg->seq() = seq; } @[end if]@ + template + inline void setMsgTimestampSample(T* msg, const uint64_t& timestamp_sample) { setMsgTimestampSample_impl(msg, timestamp_sample); } + /** * @@brief Timesync object ptr. * This object is used to compuyte and apply the time offsets to the diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 070a61c5ba..94655e8b2b 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -225,7 +225,7 @@ rtps: receive: true - msg: vtol_vehicle_status id: 105 - - msg: wind_estimate + - msg: wind id: 106 - msg: collision_constraints id: 107 @@ -313,6 +313,24 @@ rtps: id: 148 - msg: takeoff_status id: 149 + - msg: heater_status + id: 150 + - msg: gimbal_device_attitude_status + id: 151 + - msg: gimbal_device_information + id: 152 + - msg: gimbal_device_set_attitude + id: 153 + - msg: gimbal_manager_information + id: 154 + - msg: gimbal_manager_set_attitude + id: 155 + - msg: gimbal_manager_status + id: 156 + - msg: gimbal_manager_set_manual_control + id: 157 + - msg: airspeed_wind + id: 158 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 170 @@ -415,4 +433,7 @@ rtps: - msg: actuator_controls_5 id: 202 alias: actuator_controls + - msg: estimator_wind + id: 203 + alias: wind ########## multi topics: end ########## diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 9b73b96faa..6cf61a0af5 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -66,12 +66,18 @@ uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243 # UAVCAN configuration. If param 1 == uint16 VEHICLE_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| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260 # Mission command to set a Camera Auto Mount Pivoting Oblique Survey for this flight|Camera trigger distance (meters)| Shutter integration time (ms)| Camera minimum trigger interval| Number of positions| Roll| Pitch| Empty| +uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal + uint16 VEHICLE_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)| uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm| uint16 VEHICLE_CMD_INJECT_FAILURE = 420 # Inject artificial failure for testing purposes uint16 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| +uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512 # Request to send a single instance of the specified message uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530 # Set camera capture mode (photo, video, etc.) uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531 # Set camera zoom +uint16 VEHICLE_CMD_SET_CAMERA_FOCUS = 532 +uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000 # Setpoint to be sent to a gimbal manager to set a gimbal pitch and yaw +uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001 # Gimbal configuration to set which sysid/compid is in primary and secondary control uint16 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system uint16 VEHICLE_CMD_LOGGING_START = 2510 # start streaming ULog data uint16 VEHICLE_CMD_LOGGING_STOP = 2511 # stop streaming ULog data diff --git a/msg/wind.msg b/msg/wind.msg new file mode 100644 index 0000000000..ff8b6f4535 --- /dev/null +++ b/msg/wind.msg @@ -0,0 +1,16 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32 windspeed_north # Wind component in north / X direction (m/sec) +float32 windspeed_east # Wind component in east / Y direction (m/sec) + +float32 variance_north # Wind estimate error variance in north / X direction (m/sec)**2 - set to zero (no uncertainty) if not estimated +float32 variance_east # Wind estimate error variance in east / Y direction (m/sec)**2 - set to zero (no uncertainty) if not estimated + +float32 tas_innov # True airspeed innovation +float32 tas_innov_var # True airspeed innovation variance + +float32 beta_innov # Sideslip measurement innovation +float32 beta_innov_var # Sideslip measurement innovation variance + +# TOPICS wind estimator_wind diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt index be145bdbae..2e5dc931a6 100644 --- a/platforms/common/CMakeLists.txt +++ b/platforms/common/CMakeLists.txt @@ -36,7 +36,7 @@ set(SRCS) if (NOT "${PX4_PLATFORM}" MATCHES "qurt" AND NOT "${PX4_BOARD}" MATCHES "io-v2" AND NOT "${PX4_BOARD_LABEL}" MATCHES "bootloader") list(APPEND SRCS px4_log.cpp - ) + ) endif() add_library(px4_platform @@ -50,11 +50,12 @@ add_library(px4_platform shutdown.cpp spi.cpp ${SRCS} - ) +) add_dependencies(px4_platform prebuild_targets) -if (NOT "${PX4_PLATFORM}" MATCHES "qurt" AND NOT "${PX4_BOARD}" MATCHES "io-v2") - target_link_libraries(px4_platform PRIVATE modules__uORB) # px4_log awkward dependency with uORB, TODO: orb should part of the platform layer +if (NOT "${PX4_BOARD}" MATCHES "io-v2") + add_subdirectory(uORB) + target_link_libraries(px4_platform PRIVATE uORB) endif() add_subdirectory(px4_work_queue) diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index 2cbd524766..506d30ddf8 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -587,6 +587,52 @@ int board_power_off(int status); int board_reset(int status); #endif +/**************************************************************************** + * Name: board_configure_reset + * + * Description: + * Configures the device that maintains the state shared by the + * application and boot loader. This is usually an RTC. + * + * Input Parameters: + * mode - The type of reset. See reset_mode_e + * + * Returned Value: + * 0 for Success + * 1 if invalid argument + * + ****************************************************************************/ + +#ifdef CONFIG_BOARDCTL_RESET + +typedef enum reset_mode_e { + BOARD_RESET_MODE_CLEAR = 0, /* Clear the mode */ + BOARD_RESET_MODE_BOOT_TO_BL = 1, /* Reboot and stay in the bootloader */ + BOARD_RESET_MODE_BOOT_TO_VALID_APP = 2, /* Reboot to a valid app or stay in bootloader */ + BOARD_RESET_MODE_CAN_BL = 3, /* Used to pass a node ID and stay in the can bootloader */ + BOARD_RESET_MODE_RTC_BOOT_FWOK = 4 /* Set by a a watch dogged application after running > 30 Seconds */ +} reset_mode_e; + +int board_configure_reset(reset_mode_e mode, uint32_t arg); +#endif + +#if defined(SUPPORT_ALT_CAN_BOOTLOADER) +/**************************************************************************** + * Name: board_booted_by_px4 + * + * Description: + * Determines if the the boot loader was PX4 + * + * Input Parameters: + * none + * + * Returned Value: + * true if booted byt a PX4 bootloader. + * + ****************************************************************************/ + +bool board_booted_by_px4(void); +#endif /************************************************************************************ * Name: board_query_manifest * diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index a2e81faf4e..4d467819f4 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -66,7 +66,7 @@ static constexpr wq_config_t I2C3{"wq:I2C3", 2336, -11}; static constexpr wq_config_t I2C4{"wq:I2C4", 2336, -12}; // PX4 att/pos controllers, highest priority after sensors. -static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 1760, -13}; +static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 1824, -13}; static constexpr wq_config_t INS0{"wq:INS0", 6000, -14}; static constexpr wq_config_t INS1{"wq:INS1", 6000, -15}; @@ -75,7 +75,7 @@ static constexpr wq_config_t INS3{"wq:INS3", 6000, -17}; static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18}; -static constexpr wq_config_t uavcan{"wq:uavcan", 2176, -19}; +static constexpr wq_config_t uavcan{"wq:uavcan", 2576, -19}; static constexpr wq_config_t UART0{"wq:UART0", 1400, -21}; static constexpr wq_config_t UART1{"wq:UART1", 1400, -22}; diff --git a/src/modules/uORB/CMakeLists.txt b/platforms/common/uORB/CMakeLists.txt similarity index 67% rename from src/modules/uORB/CMakeLists.txt rename to platforms/common/uORB/CMakeLists.txt index 8c97b527ba..5838d8c40e 100644 --- a/src/modules/uORB/CMakeLists.txt +++ b/platforms/common/uORB/CMakeLists.txt @@ -31,44 +31,35 @@ # ############################################################################ -if(NOT PX4_BOARD MATCHES "px4_io") # TODO: fix this hack (move uORB to platform layer) +# this includes the generated topics directory +include_directories(${CMAKE_CURRENT_BINARY_DIR}) - # this includes the generated topics directory - include_directories(${CMAKE_CURRENT_BINARY_DIR}) +px4_add_library(uORB + ORBSet.hpp + Publication.hpp + PublicationMulti.hpp + Subscription.cpp + Subscription.hpp + SubscriptionCallback.hpp + SubscriptionInterval.hpp + SubscriptionMultiArray.hpp + uORB.cpp + uORB.h + uORBCommon.hpp + uORBCommunicator.hpp + uORBDeviceMaster.cpp + uORBDeviceMaster.hpp + uORBDeviceNode.cpp + uORBDeviceNode.hpp + uORBManager.cpp + uORBManager.hpp + uORBUtils.cpp + uORBUtils.hpp +) - px4_add_module( - MODULE modules__uORB - MAIN uorb - COMPILE_FLAGS - ${MAX_CUSTOM_OPT_LEVEL} - SRCS - ORBSet.hpp - Publication.hpp - PublicationMulti.hpp - Subscription.cpp - Subscription.hpp - SubscriptionCallback.hpp - SubscriptionInterval.hpp - SubscriptionMultiArray.hpp - uORB.cpp - uORB.h - uORBCommon.hpp - uORBCommunicator.hpp - uORBDeviceMaster.cpp - uORBDeviceMaster.hpp - uORBDeviceNode.cpp - uORBDeviceNode.hpp - uORBMain.cpp - uORBManager.cpp - uORBManager.hpp - uORBUtils.cpp - uORBUtils.hpp - DEPENDS - cdev - uorb_msgs - ) +target_compile_options(uORB PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_link_libraries(uORB PRIVATE cdev uorb_msgs) - if(PX4_TESTING) - add_subdirectory(uORB_tests) - endif() +if(PX4_TESTING) + add_subdirectory(uORB_tests) endif() diff --git a/src/modules/uORB/ORBSet.hpp b/platforms/common/uORB/ORBSet.hpp similarity index 99% rename from src/modules/uORB/ORBSet.hpp rename to platforms/common/uORB/ORBSet.hpp index 94ba85138e..aed38dbc8f 100644 --- a/src/modules/uORB/ORBSet.hpp +++ b/platforms/common/uORB/ORBSet.hpp @@ -143,4 +143,3 @@ private: Node *_top; Node *_end; }; - diff --git a/src/modules/uORB/Publication.hpp b/platforms/common/uORB/Publication.hpp similarity index 100% rename from src/modules/uORB/Publication.hpp rename to platforms/common/uORB/Publication.hpp diff --git a/src/modules/uORB/PublicationMulti.hpp b/platforms/common/uORB/PublicationMulti.hpp similarity index 100% rename from src/modules/uORB/PublicationMulti.hpp rename to platforms/common/uORB/PublicationMulti.hpp diff --git a/src/modules/uORB/Subscription.cpp b/platforms/common/uORB/Subscription.cpp similarity index 100% rename from src/modules/uORB/Subscription.cpp rename to platforms/common/uORB/Subscription.cpp diff --git a/src/modules/uORB/Subscription.hpp b/platforms/common/uORB/Subscription.hpp similarity index 100% rename from src/modules/uORB/Subscription.hpp rename to platforms/common/uORB/Subscription.hpp diff --git a/src/modules/uORB/SubscriptionBlocking.hpp b/platforms/common/uORB/SubscriptionBlocking.hpp similarity index 100% rename from src/modules/uORB/SubscriptionBlocking.hpp rename to platforms/common/uORB/SubscriptionBlocking.hpp diff --git a/src/modules/uORB/SubscriptionCallback.hpp b/platforms/common/uORB/SubscriptionCallback.hpp similarity index 100% rename from src/modules/uORB/SubscriptionCallback.hpp rename to platforms/common/uORB/SubscriptionCallback.hpp diff --git a/src/modules/uORB/SubscriptionInterval.hpp b/platforms/common/uORB/SubscriptionInterval.hpp similarity index 100% rename from src/modules/uORB/SubscriptionInterval.hpp rename to platforms/common/uORB/SubscriptionInterval.hpp diff --git a/src/modules/uORB/SubscriptionMultiArray.hpp b/platforms/common/uORB/SubscriptionMultiArray.hpp similarity index 100% rename from src/modules/uORB/SubscriptionMultiArray.hpp rename to platforms/common/uORB/SubscriptionMultiArray.hpp diff --git a/src/modules/uORB/uORB.cpp b/platforms/common/uORB/uORB.cpp similarity index 84% rename from src/modules/uORB/uORB.cpp rename to platforms/common/uORB/uORB.cpp index 23830d4638..7e3b96904d 100644 --- a/src/modules/uORB/uORB.cpp +++ b/platforms/common/uORB/uORB.cpp @@ -37,9 +37,60 @@ */ #include "uORB.h" + #include "uORBManager.hpp" #include "uORBCommon.hpp" +static uORB::DeviceMaster *g_dev = nullptr; + +int uorb_start(void) +{ + if (g_dev != nullptr) { + PX4_WARN("already loaded"); + /* user wanted to start uorb, its already running, no error */ + return 0; + } + + if (!uORB::Manager::initialize()) { + PX4_ERR("uorb manager alloc failed"); + return -ENOMEM; + } + + /* create the driver */ + g_dev = uORB::Manager::get_instance()->get_device_master(); + + if (g_dev == nullptr) { + return -errno; + } + + return OK; +} + +int uorb_status(void) +{ + if (g_dev != nullptr) { + g_dev->printStatistics(); + + } else { + PX4_INFO("uorb is not running"); + } + + return OK; +} + +int uorb_top(char **topic_filter, int num_filters) +{ + if (g_dev != nullptr) { + g_dev->showTop(topic_filter, num_filters); + + } else { + PX4_INFO("uorb is not running"); + } + + return OK; +} + + orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { return uORB::Manager::get_instance()->orb_advertise(meta, data); diff --git a/src/modules/uORB/uORB.h b/platforms/common/uORB/uORB.h similarity index 98% rename from src/modules/uORB/uORB.h rename to platforms/common/uORB/uORB.h index b9d0f2705f..95061c37f4 100644 --- a/src/modules/uORB/uORB.h +++ b/platforms/common/uORB/uORB.h @@ -114,6 +114,10 @@ typedef const struct orb_metadata *orb_id_t; __BEGIN_DECLS +int uorb_start(void); +int uorb_status(void); +int uorb_top(char **topic_filter, int num_filters); + /** * ORB topic advertiser handle. * diff --git a/src/modules/uORB/uORBCommon.hpp b/platforms/common/uORB/uORBCommon.hpp similarity index 100% rename from src/modules/uORB/uORBCommon.hpp rename to platforms/common/uORB/uORBCommon.hpp diff --git a/src/modules/uORB/uORBCommunicator.hpp b/platforms/common/uORB/uORBCommunicator.hpp similarity index 100% rename from src/modules/uORB/uORBCommunicator.hpp rename to platforms/common/uORB/uORBCommunicator.hpp diff --git a/src/modules/uORB/uORBDeviceMaster.cpp b/platforms/common/uORB/uORBDeviceMaster.cpp similarity index 100% rename from src/modules/uORB/uORBDeviceMaster.cpp rename to platforms/common/uORB/uORBDeviceMaster.cpp diff --git a/src/modules/uORB/uORBDeviceMaster.hpp b/platforms/common/uORB/uORBDeviceMaster.hpp similarity index 100% rename from src/modules/uORB/uORBDeviceMaster.hpp rename to platforms/common/uORB/uORBDeviceMaster.hpp diff --git a/src/modules/uORB/uORBDeviceNode.cpp b/platforms/common/uORB/uORBDeviceNode.cpp similarity index 100% rename from src/modules/uORB/uORBDeviceNode.cpp rename to platforms/common/uORB/uORBDeviceNode.cpp diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/platforms/common/uORB/uORBDeviceNode.hpp similarity index 100% rename from src/modules/uORB/uORBDeviceNode.hpp rename to platforms/common/uORB/uORBDeviceNode.hpp diff --git a/src/modules/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp similarity index 100% rename from src/modules/uORB/uORBManager.cpp rename to platforms/common/uORB/uORBManager.cpp diff --git a/src/modules/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp similarity index 100% rename from src/modules/uORB/uORBManager.hpp rename to platforms/common/uORB/uORBManager.hpp diff --git a/src/modules/uORB/uORBTopics.h b/platforms/common/uORB/uORBTopics.h similarity index 100% rename from src/modules/uORB/uORBTopics.h rename to platforms/common/uORB/uORBTopics.h diff --git a/src/modules/uORB/uORBUtils.cpp b/platforms/common/uORB/uORBUtils.cpp similarity index 100% rename from src/modules/uORB/uORBUtils.cpp rename to platforms/common/uORB/uORBUtils.cpp diff --git a/src/modules/uORB/uORBUtils.hpp b/platforms/common/uORB/uORBUtils.hpp similarity index 100% rename from src/modules/uORB/uORBUtils.hpp rename to platforms/common/uORB/uORBUtils.hpp diff --git a/src/modules/uORB/uORB_tests/CMakeLists.txt b/platforms/common/uORB/uORB_tests/CMakeLists.txt similarity index 100% rename from src/modules/uORB/uORB_tests/CMakeLists.txt rename to platforms/common/uORB/uORB_tests/CMakeLists.txt diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp similarity index 100% rename from src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp rename to platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.hpp similarity index 100% rename from src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp rename to platforms/common/uORB/uORB_tests/uORBTest_UnitTest.hpp diff --git a/src/modules/uORB/uORB_tests/uORB_tests_main.cpp b/platforms/common/uORB/uORB_tests/uORB_tests_main.cpp similarity index 100% rename from src/modules/uORB/uORB_tests/uORB_tests_main.cpp rename to platforms/common/uORB/uORB_tests/uORB_tests_main.cpp diff --git a/platforms/nuttx/CMakeLists.txt b/platforms/nuttx/CMakeLists.txt index 0c78f21541..93c34ffc80 100644 --- a/platforms/nuttx/CMakeLists.txt +++ b/platforms/nuttx/CMakeLists.txt @@ -93,7 +93,7 @@ list(APPEND nuttx_libs nuttx_fs nuttx_mm nuttx_sched - ) +) if (CONFIG_NET) list(APPEND nuttx_libs nuttx_net) @@ -163,7 +163,7 @@ endif() add_custom_command(OUTPUT ${PX4_BINARY_OUTPUT} COMMAND ${CMAKE_OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME} ${PX4_BINARY_OUTPUT} DEPENDS px4 - ) +) # create .px4 with parameter and airframe metadata if (TARGET parameters_xml AND TARGET airframes_xml) @@ -203,7 +203,7 @@ add_custom_target(weak_symbols DEPENDS px4 VERBATIM USES_TERMINAL - ) +) # debugger helpers @@ -292,7 +292,7 @@ add_custom_target(debug DEPENDS px4 ${PX4_BINARY_DIR}/.gdbinit WORKING_DIRECTORY ${PX4_BINARY_DIR} USES_TERMINAL - ) +) include(blackmagic) include(jlink) diff --git a/platforms/nuttx/src/canbootloader/CMakeLists.txt b/platforms/nuttx/src/canbootloader/CMakeLists.txt index 38b386c107..5703458716 100644 --- a/platforms/nuttx/src/canbootloader/CMakeLists.txt +++ b/platforms/nuttx/src/canbootloader/CMakeLists.txt @@ -48,4 +48,5 @@ target_compile_options(canbootloader PRIVATE -Wno-error) target_link_libraries(canbootloader PRIVATE drivers_bootloaders + arch_watchdog_iwdg ) diff --git a/platforms/nuttx/src/canbootloader/include/can.h b/platforms/nuttx/src/canbootloader/include/can.h index cded836c71..fcf4f4930d 100644 --- a/platforms/nuttx/src/canbootloader/include/can.h +++ b/platforms/nuttx/src/canbootloader/include/can.h @@ -53,7 +53,8 @@ typedef enum { CAN_125KBAUD = 1, CAN_250KBAUD = 2, CAN_500KBAUD = 3, - CAN_1MBAUD = 4 + CAN_1MBAUD = 4, + CAN_UNDEFINED = 999, } can_speed_t; typedef enum { diff --git a/platforms/nuttx/src/canbootloader/uavcan/main.c b/platforms/nuttx/src/canbootloader/uavcan/main.c index 74db7cc605..5e64734093 100644 --- a/platforms/nuttx/src/canbootloader/uavcan/main.c +++ b/platforms/nuttx/src/canbootloader/uavcan/main.c @@ -57,6 +57,8 @@ #include "random.h" #include +#include +#include #include //#define DEBUG_APPLICATION_INPLACE 1 /* Never leave defined */ @@ -94,6 +96,7 @@ typedef volatile struct bootloader_t { bool wait_for_getnodeinfo; bool app_bl_request; bool sent_node_info_response; + uint16_t percentage_done; union { uint32_t l; uint8_t b[sizeof(uint32_t)]; @@ -106,7 +109,7 @@ bootloader_t bootloader; const uint8_t debug_log_source[uavcan_byte_count(LogMessage, source)] = {'B', 'o', 'o', 't'}; /**************************************************************************** - * Name: start_the_watch_dog + * Name: early_start_the_watch_dog * * Description: * This function will start the hardware watchdog. Once stated the code must @@ -120,11 +123,32 @@ const uint8_t debug_log_source[uavcan_byte_count(LogMessage, source)] = {'B', 'o * ****************************************************************************/ -static inline void start_the_watch_dog(void) +static inline void early_start_the_watch_dog(void) { #ifdef OPT_ENABLE_WD #endif } + +/**************************************************************************** + * Name: app_start_the_watch_dog + * + * Description: + * This function will start the hardware watchdog. Once stated the code must + * kick it before it time out a reboot will occur. + * + * Input Parameters: + * None + * + * Returned Value: + * None + * + ****************************************************************************/ + +static inline void app_start_the_watch_dog(void) +{ + watchdog_init(); + watchdog_pet(); +} /**************************************************************************** * Name: kick_the_watch_dog * @@ -208,7 +232,7 @@ static void node_info_process(bl_timer_id id, void *context) bootloader.fw_image_descriptor->minor_version; response.software_version.vcs_commit = - bootloader.fw_image_descriptor->vcs_commit; + bootloader.fw_image_descriptor->git_hash; response.software_version.image_crc = bootloader.fw_image_descriptor->image_crc; @@ -263,7 +287,7 @@ static void node_status_process(bl_timer_id id, void *context) message.u8 = uavcan_pack(bootloader.sub_mode, NodeStatus, sub_mode) | uavcan_pack(bootloader.mode, NodeStatus, mode) | uavcan_pack(bootloader.health, NodeStatus, health); - message.vendor_specific_status_code = 0u; + message.vendor_specific_status_code = bootloader.percentage_done; uavcan_tx_dsdl(DSDLMsgNodeStatus, &protocol, (const uint8_t *) &message, sizeof(uavcan_NodeStatus_t)); } @@ -292,9 +316,9 @@ static void find_descriptor(void) app_descriptor_t *descriptor = NULL; union { uint64_t ull; - char text[sizeof(uint64_t)]; + uint8_t bytes[sizeof(uint64_t)]; } sig = { - .text = {APP_DESCRIPTOR_SIGNATURE} + .bytes = APP_DESCRIPTOR_SIGNATURE }; do { @@ -312,8 +336,8 @@ static void find_descriptor(void) * * Description: * This functions validates the applications image based on the validity of - * the Application firmware descriptor's crc and the value of the first word - * in the FLASH image. + * the Application firmware descriptor's 2 crcs and the value of + * the first word in the FLASH image. * * * Input Parameters: @@ -326,9 +350,9 @@ static void find_descriptor(void) ****************************************************************************/ static bool is_app_valid(uint32_t first_word) { - uint64_t crc; - size_t i, length, crc_offset; - uint32_t word; + uint32_t block_crc1; + uint32_t block_crc2; + size_t length; find_descriptor(); @@ -338,36 +362,30 @@ static bool is_app_valid(uint32_t first_word) length = bootloader.fw_image_descriptor->image_size; - if (length > APPLICATION_SIZE) { + if (length > APPLICATION_SIZE || length == 0) { return false; } - crc_offset = (size_t)(&bootloader.fw_image_descriptor->image_crc) - - (size_t) bootloader.fw_image; - crc_offset >>= 2u; - length >>= 2u; + size_t block2_len = bootloader.fw_image_descriptor->image_size - ((size_t)&bootloader.fw_image_descriptor->major_version + - (size_t)bootloader.fw_image); - crc = crc64_add_word(CRC64_INITIAL, first_word); - - for (i = 1u; i < length; i++) { - if (i == crc_offset || i == crc_offset + 1u) { - /* Zero out the CRC field while computing the CRC */ - word = 0u; - - } else { - word = bootloader.fw_image[i]; - } - - crc = crc64_add_word(crc, word); + if (block2_len > APPLICATION_SIZE || block2_len == 0) { + return false; } - crc ^= CRC64_OUTPUT_XOR; + block_crc1 = crc32_signature(0, sizeof(first_word), (const uint8_t *)&first_word); + block_crc1 = crc32_signature(block_crc1, (size_t)(&bootloader.fw_image_descriptor->crc32_block1) - + (size_t)(bootloader.fw_image + 1), (const uint8_t *)(bootloader.fw_image + 1)); + + block_crc2 = crc32_signature(0, block2_len, + (const uint8_t *) &bootloader.fw_image_descriptor->major_version); #if defined(DEBUG_APPLICATION_INPLACE) return true; #endif - return crc == bootloader.fw_image_descriptor->image_crc; + return block_crc1 == bootloader.fw_image_descriptor->crc32_block1 + && block_crc2 == bootloader.fw_image_descriptor->crc32_block2; } /**************************************************************************** @@ -737,6 +755,8 @@ static flash_error_t file_read_and_program(const uavcan_Path_t *fw_path, uint8_t /* Set up the read request */ memcpy(&request.path, fw_path, sizeof(uavcan_Path_t)); + bootloader.percentage_done = 0; + uint8_t retries = UavcanServiceRetries; /* @@ -833,6 +853,7 @@ static flash_error_t file_read_and_program(const uavcan_Path_t *fw_path, uint8_t length + (length & 1)); request.offset += length; + bootloader.percentage_done = (request.offset / 1024) + 1; /* rate limit */ while (!timer_expired(tread)) { @@ -851,7 +872,7 @@ static flash_error_t file_read_and_program(const uavcan_Path_t *fw_path, uint8_t * was not. */ if (uavcan_status == UavcanOk && flash_status == FLASH_OK && request.offset == fw_image_size && length != 0) { - + bootloader.percentage_done = 0; return FLASH_OK; } else { @@ -952,6 +973,7 @@ static void application_run(size_t fw_image_size, bootloader_app_shared_t *commo putreg32(APPLICATION_LOAD_ADDRESS, NVIC_VECTAB); __asm volatile("dsb"); /* extract the stack and entrypoint from the app vector table and go */ + app_start_the_watch_dog(); do_jump(fw_image[0], fw_image[1]); } } @@ -976,11 +998,16 @@ static void application_run(size_t fw_image_size, bootloader_app_shared_t *commo static int autobaud_and_get_dynamic_node_id(bl_timer_id tboot, can_speed_t *speed, uint32_t *node_id) { board_indicate(autobaud_start); - + bool autobaud_only = *speed == CAN_UNDEFINED; int rv = can_autobaud(speed, tboot); if (rv != CAN_BOOT_TIMEOUT) { board_indicate(autobaud_end); + + if (autobaud_only) { + return rv; + } + board_indicate(allocation_start); #if defined(DEBUG_APPLICATION_INPLACE) *node_id = 125; @@ -1018,7 +1045,7 @@ __EXPORT int main(int argc, char *argv[]) flash_error_t status; bootloader_app_shared_t common; - start_the_watch_dog(); + early_start_the_watch_dog(); /* Begin with all data zeroed */ memset((void *)&bootloader, 0, sizeof(bootloader)); @@ -1075,6 +1102,20 @@ __EXPORT int main(int argc, char *argv[]) bootloader.app_bl_request = (OK == bootloader_app_shared_read(&common, App)) && common.bus_speed && common.node_id; +#if defined(SUPPORT_ALT_CAN_BOOTLOADER) + /* Was this boot a result of An Alternate Application being told it has a FW update ? */ + + bootloader_alt_app_shared_t *ps = (bootloader_alt_app_shared_t *) &_sapp_bl_shared; + + if (!bootloader.app_bl_request && ps->signature == BL_ALT_APP_SHARED_SIGNATURE) { + + common.node_id = ps->node_id; + common.bus_speed = CAN_UNDEFINED; + bootloader.app_bl_request = ps->node_id != 0; + ps->signature = 0; + } + +#endif /* * Mark CRC to say this is not from * auto baud and Node Allocation @@ -1130,7 +1171,30 @@ __EXPORT int main(int argc, char *argv[]) if (bootloader.app_bl_request) { bootloader.bus_speed = common.bus_speed; + + /* if the the bootloader_alt_app_shared_t was used there is not bit rate. + * So let auto baud only as signaled by bootloader.bus_speed == CAN_UNDEFINED + */ + + if (common.bus_speed == CAN_UNDEFINED) { + if (CAN_OK != autobaud_and_get_dynamic_node_id(tboot, (can_speed_t *)&bootloader.bus_speed, &common.node_id)) { + /* + * It is OK that node ID is set to the preferred Appl Node ID because + * common.crc.valid is not true yet + */ + + goto boot; + + } + } + can_init(can_freq2speed(common.bus_speed), CAN_Mode_Normal); + /* + * Mark CRC to say this is from + * auto baud and Node Allocation + */ + common.crc.valid = true; + } else { diff --git a/platforms/nuttx/src/px4/common/CMakeLists.txt b/platforms/nuttx/src/px4/common/CMakeLists.txt index 5bb17b5877..6a87f19a10 100644 --- a/platforms/nuttx/src/px4/common/CMakeLists.txt +++ b/platforms/nuttx/src/px4/common/CMakeLists.txt @@ -32,7 +32,7 @@ ############################################################################ # skip for px4_layer support on an IO board -if(NOT PX4_BOARD MATCHES "px4_io") +if(NOT PX4_BOARD MATCHES "io-v2") add_library(px4_layer board_crashdump.c @@ -57,6 +57,7 @@ if(NOT PX4_BOARD MATCHES "px4_io") nuttx_apps nuttx_sched px4_work_queue + uORB ) else() add_library(px4_layer ${PX4_SOURCE_DIR}/platforms/common/empty.c) diff --git a/platforms/nuttx/src/px4/common/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp index c731ad16b6..563974a171 100644 --- a/platforms/nuttx/src/px4/common/px4_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_init.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include @@ -118,6 +119,10 @@ int px4_platform_init(void) px4::WorkQueueManagerStart(); + uorb_start(); + + px4_log_initialize(); + return PX4_OK; } diff --git a/platforms/nuttx/src/px4/stm/stm32_common/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/stm/stm32_common/board_reset/board_reset.cpp index ef65d94ab7..c5bf204779 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/board_reset/board_reset.cpp +++ b/platforms/nuttx/src/px4/stm/stm32_common/board_reset/board_reset.cpp @@ -38,6 +38,7 @@ */ #include +#include #include #include #include @@ -45,20 +46,53 @@ #ifdef CONFIG_BOARDCTL_RESET -static int board_reset_enter_bootloader() +/**************************************************************************** + * Name: board_configure_reset + * + * Description: + * Configures the device that maintains the state shared by the + * application and boot loader. This is usually an RTC. + * + * Input Parameters: + * mode - The type of reset. See reset_mode_e + * + * Returned Value: + * 0 for Success + * 1 if invalid argument + * + ****************************************************************************/ + +static const uint32_t modes[] = { + /* to tb */ + /* BOARD_RESET_MODE_CLEAR 5 y */ 0, + /* BOARD_RESET_MODE_BOOT_TO_BL 0 n */ 0xb0070001, + /* BOARD_RESET_MODE_BOOT_TO_VALID_APP 0 y */ 0xb0070002, + /* BOARD_RESET_MODE_CAN_BL 10 n */ 0xb0080000, + /* BOARD_RESET_MODE_RTC_BOOT_FWOK 0 n */ 0xb0093a26 +}; + +int board_configure_reset(reset_mode_e mode, uint32_t arg) { - stm32_pwr_enablebkp(true); + int rv = -1; - uint32_t regvalue = 0xb007b007; + if (mode < arraySize(modes)) { + + stm32_pwr_enablebkp(true); + + arg = mode == BOARD_RESET_MODE_CAN_BL ? arg & ~0xff : 0; + + // Check if we can to use the new register definition -// Check if we can to use the new register definition #ifndef STM32_RTC_BK0R - *(uint32_t *)STM32_BKP_BASE = regvalue; + *(uint32_t *)STM32_BKP_BASE = modes[mode] | arg; #else - *(uint32_t *)STM32_RTC_BK0R = regvalue; + *(uint32_t *)STM32_RTC_BK0R = modes[mode] | arg; #endif - stm32_pwr_enablebkp(false); - return OK; + stm32_pwr_enablebkp(false); + rv = OK; + } + + return rv; } /**************************************************************************** @@ -84,7 +118,7 @@ static int board_reset_enter_bootloader() int board_reset(int status) { if (status == 1) { - board_reset_enter_bootloader(); + board_configure_reset(BOARD_RESET_MODE_BOOT_TO_BL, 0); } #if defined(BOARD_HAS_ON_RESET) @@ -96,3 +130,27 @@ int board_reset(int status) } #endif /* CONFIG_BOARDCTL_RESET */ + +#if defined(SUPPORT_ALT_CAN_BOOTLOADER) +/**************************************************************************** + * Name: board_booted_by_px4 + * + * Description: + * Determines if the the boot loader was PX4 + * + * Input Parameters: + * none + * + * Returned Value: + * true if booted byt a PX4 bootloader. + * + ****************************************************************************/ +bool board_booted_by_px4(void) +{ + uint32_t *vectors = (uint32_t *) STM32_FLASH_BASE; + + /* Nuttx uses common vector */ + + return (vectors[2] == vectors[3]) && (vectors[4] == vectors[5]); +} +#endif diff --git a/platforms/nuttx/src/px4/stm/stm32f4/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f4/CMakeLists.txt index 17e59e776b..9b49f31f84 100644 --- a/platforms/nuttx/src/px4/stm/stm32f4/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32f4/CMakeLists.txt @@ -46,4 +46,5 @@ add_subdirectory(../stm32_common/tone_alarm tone_alarm) add_subdirectory(../stm32_common/version version) add_subdirectory(px4io_serial) +add_subdirectory(watchdog) diff --git a/platforms/nuttx/src/px4/stm/stm32f4/watchdog/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f4/watchdog/CMakeLists.txt new file mode 100644 index 0000000000..3c80878d02 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f4/watchdog/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(arch_watchdog_iwdg + iwdg.c +) diff --git a/platforms/nuttx/src/px4/stm/stm32f4/watchdog/iwdg.c b/platforms/nuttx/src/px4/stm/stm32f4/watchdog/iwdg.c new file mode 100644 index 0000000000..d8e66376c1 --- /dev/null +++ b/platforms/nuttx/src/px4/stm/stm32f4/watchdog/iwdg.c @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include "chip.h" +#include "stm32.h" +#include +#include "nvic.h" + +/**************************************************************************** + * Name: watchdog_pet() + * + * Description: + * This function resets the Independent watchdog (IWDG) + * + * + * Input Parameters: + * none. + * + * Returned value: + * none. + * + ****************************************************************************/ + +void watchdog_pet(void) +{ + putreg32(IWDG_KR_KEY_RELOAD, STM32_IWDG_KR); +} + +/**************************************************************************** + * Name: watchdog_init() + * + * Description: + * This function initialize the Independent watchdog (IWDG) + * + * + * Input Parameters: + * none. + * + * Returned value: + * none. + * + ****************************************************************************/ + +void watchdog_init(void) +{ +#if defined(CONFIG_STM32_JTAG_FULL_ENABLE) || \ + defined(CONFIG_STM32_JTAG_NOJNTRST_ENABLE) || \ + defined(CONFIG_STM32_JTAG_SW_ENABLE) + putreg32(getreg32(STM32_DBGMCU_APB1_FZ) | DBGMCU_APB1_IWDGSTOP, STM32_DBGMCU_APB1_FZ); +#endif + + /* unlock */ + + putreg32(IWDG_KR_KEY_ENABLE, STM32_IWDG_KR); + + /* Set the prescale value */ + + putreg32(IWDG_PR_DIV16, STM32_IWDG_PR); + + /* Set the reload value */ + + putreg32(IWDG_RLR_MAX, STM32_IWDG_RLR); + + /* Start the watch dog */ + + putreg32(IWDG_KR_KEY_START, STM32_IWDG_KR); + + watchdog_pet(); +} diff --git a/platforms/posix/CMakeLists.txt b/platforms/posix/CMakeLists.txt index 03eb02daa1..927c3e35b2 100644 --- a/platforms/posix/CMakeLists.txt +++ b/platforms/posix/CMakeLists.txt @@ -70,7 +70,7 @@ else() target_link_libraries(px4 PRIVATE rt) endif() - target_link_libraries(px4 PRIVATE modules__uORB) + target_link_libraries(px4 PRIVATE uORB) #============================================================================= # install diff --git a/platforms/posix/src/px4/common/px4_init.cpp b/platforms/posix/src/px4/common/px4_init.cpp index a28e01c014..b93a53ba5f 100644 --- a/platforms/posix/src/px4/common/px4_init.cpp +++ b/platforms/posix/src/px4/common/px4_init.cpp @@ -37,6 +37,7 @@ #include #include #include +#include int px4_platform_init(void) { @@ -46,5 +47,9 @@ int px4_platform_init(void) px4::WorkQueueManagerStart(); + uorb_start(); + + px4_log_initialize(); + return PX4_OK; } diff --git a/platforms/qurt/src/px4/common/commands_hil.c b/platforms/qurt/src/px4/common/commands_hil.c index 8baaa32416..6519fcce32 100644 --- a/platforms/qurt/src/px4/common/commands_hil.c +++ b/platforms/qurt/src/px4/common/commands_hil.c @@ -42,7 +42,6 @@ const char *get_commands() { static const char *commands = - "uorb start\n" "param set CAL_GYRO0_ID 2293760\n" "param set CAL_ACC0_ID 1310720\n" "param set CAL_ACC1_ID 1376256\n" diff --git a/platforms/qurt/src/px4/common/main.cpp b/platforms/qurt/src/px4/common/main.cpp index 5d69ae9491..a238dc33f5 100644 --- a/platforms/qurt/src/px4/common/main.cpp +++ b/platforms/qurt/src/px4/common/main.cpp @@ -184,7 +184,7 @@ const char *get_commands() // All that needs to be started automatically on the DSP side // are uorb and qshell. After that, everything else can get // started from the main startup script on the Linux side. - static const char *commands = "uorb start\nqshell start\n"; + static const char *commands = "qshell start\n"; return commands; } diff --git a/platforms/qurt/src/px4/common/px4_init.cpp b/platforms/qurt/src/px4/common/px4_init.cpp index 1921d88fe3..73bada3ae1 100644 --- a/platforms/qurt/src/px4/common/px4_init.cpp +++ b/platforms/qurt/src/px4/common/px4_init.cpp @@ -46,5 +46,9 @@ int px4_platform_init() px4::WorkQueueManagerStart(); + uorb_start(); + + //px4_log_initialize(); + return PX4_OK; } diff --git a/posix-configs/SITL/init/test/cmd_template.in b/posix-configs/SITL/init/test/cmd_template.in index 78a3c7726d..c758f6a8ec 100644 --- a/posix-configs/SITL/init/test/cmd_template.in +++ b/posix-configs/SITL/init/test/cmd_template.in @@ -3,8 +3,6 @@ # (px4-alias.sh is expected to be in the PATH) . px4-alias.sh -uorb start - param load param set CBRK_SUPPLY_CHK 894281 param set SYS_RESTART_TYPE 0 diff --git a/posix-configs/SITL/init/test/test_mavlink b/posix-configs/SITL/init/test/test_mavlink index 540eb03096..e5db40970f 100644 --- a/posix-configs/SITL/init/test/test_mavlink +++ b/posix-configs/SITL/init/test/test_mavlink @@ -3,8 +3,6 @@ # (px4-alias.sh is expected to be in the PATH) . px4-alias.sh -uorb start - param load param set CBRK_SUPPLY_CHK 894281 param set SYS_RESTART_TYPE 0 diff --git a/posix-configs/SITL/init/test/test_shutdown b/posix-configs/SITL/init/test/test_shutdown index 8fe5fe1f7d..6f5649f9f2 100644 --- a/posix-configs/SITL/init/test/test_shutdown +++ b/posix-configs/SITL/init/test/test_shutdown @@ -3,8 +3,6 @@ # (px4-alias.sh is expected to be in the PATH) . px4-alias.sh -uorb start - param load param set BAT_N_CELLS 3 diff --git a/posix-configs/SITL/init/test/test_template.in b/posix-configs/SITL/init/test/test_template.in index c55a98ece2..d495bde084 100644 --- a/posix-configs/SITL/init/test/test_template.in +++ b/posix-configs/SITL/init/test/test_template.in @@ -3,8 +3,6 @@ # (px4-alias.sh is expected to be in the PATH) . px4-alias.sh -uorb start - param load param set CBRK_SUPPLY_CHK 894281 param set SYS_RESTART_TYPE 0 diff --git a/posix-configs/SITL/init/test/tests_all b/posix-configs/SITL/init/test/tests_all index 252cc343a1..063dee50da 100644 --- a/posix-configs/SITL/init/test/tests_all +++ b/posix-configs/SITL/init/test/tests_all @@ -3,8 +3,6 @@ # (px4-alias.sh is expected to be in the PATH) . px4-alias.sh -uorb start - param load param set CBRK_SUPPLY_CHK 894281 param set SYS_RESTART_TYPE 0 diff --git a/posix-configs/bbblue/px4.config b/posix-configs/bbblue/px4.config index ed127ef660..25517b324d 100644 --- a/posix-configs/bbblue/px4.config +++ b/posix-configs/bbblue/px4.config @@ -6,7 +6,6 @@ # config for a quad # modified from ../rpi/px4.config -uorb start param load param set CBRK_SUPPLY_CHK 894281 diff --git a/posix-configs/bbblue/px4_fw.config b/posix-configs/bbblue/px4_fw.config index 11ec56d554..8ed0816836 100644 --- a/posix-configs/bbblue/px4_fw.config +++ b/posix-configs/bbblue/px4_fw.config @@ -6,7 +6,6 @@ # config for fixed wing (FW) # modified from ./px4.config, switch att/pos_control & mixer -uorb start param load param set CBRK_SUPPLY_CHK 894281 diff --git a/posix-configs/eagle/200qx/mainapp.config b/posix-configs/eagle/200qx/mainapp.config index fedc520c27..43495fd679 100644 --- a/posix-configs/eagle/200qx/mainapp.config +++ b/posix-configs/eagle/200qx/mainapp.config @@ -4,7 +4,6 @@ # (px4-alias.sh is expected to be in the PATH) . px4-alias.sh -uorb start muorb start logger start -t -b 200 # Wait 1s before setting parameters for muorb to initialize diff --git a/posix-configs/eagle/200qx/px4.config b/posix-configs/eagle/200qx/px4.config index 2ffb972c0b..90d35c64e8 100644 --- a/posix-configs/eagle/200qx/px4.config +++ b/posix-configs/eagle/200qx/px4.config @@ -1,4 +1,3 @@ -uorb start qshell start param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 4001 diff --git a/posix-configs/eagle/210qc/mainapp.config b/posix-configs/eagle/210qc/mainapp.config index fedc520c27..43495fd679 100644 --- a/posix-configs/eagle/210qc/mainapp.config +++ b/posix-configs/eagle/210qc/mainapp.config @@ -4,7 +4,6 @@ # (px4-alias.sh is expected to be in the PATH) . px4-alias.sh -uorb start muorb start logger start -t -b 200 # Wait 1s before setting parameters for muorb to initialize diff --git a/posix-configs/eagle/210qc/px4.config b/posix-configs/eagle/210qc/px4.config index 0780e33a9b..941a704607 100644 --- a/posix-configs/eagle/210qc/px4.config +++ b/posix-configs/eagle/210qc/px4.config @@ -1,4 +1,3 @@ -uorb start qshell start param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 4001 diff --git a/posix-configs/eagle/flight/mainapp.config b/posix-configs/eagle/flight/mainapp.config index fedc520c27..43495fd679 100644 --- a/posix-configs/eagle/flight/mainapp.config +++ b/posix-configs/eagle/flight/mainapp.config @@ -4,7 +4,6 @@ # (px4-alias.sh is expected to be in the PATH) . px4-alias.sh -uorb start muorb start logger start -t -b 200 # Wait 1s before setting parameters for muorb to initialize diff --git a/posix-configs/eagle/flight/px4.config b/posix-configs/eagle/flight/px4.config index 6b13a92bf0..0ae7e48503 100644 --- a/posix-configs/eagle/flight/px4.config +++ b/posix-configs/eagle/flight/px4.config @@ -1,4 +1,3 @@ -uorb start qshell start param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 4001 diff --git a/posix-configs/eagle/hil/mainapphil.config b/posix-configs/eagle/hil/mainapphil.config index 30ed3728fc..edfc7d21b7 100644 --- a/posix-configs/eagle/hil/mainapphil.config +++ b/posix-configs/eagle/hil/mainapphil.config @@ -1,4 +1,3 @@ -uorb start muorb start # Wait 1s before setting parameters for muorb to initialize sleep 1 diff --git a/posix-configs/eagle/hil/px4.config b/posix-configs/eagle/hil/px4.config index f8b8d9bb5c..f1afab09db 100644 --- a/posix-configs/eagle/hil/px4.config +++ b/posix-configs/eagle/hil/px4.config @@ -1,4 +1,3 @@ -uorb start qshell start param set CBRK_SUPPLY_CHK 894281 diff --git a/posix-configs/eagle/init/rcS b/posix-configs/eagle/init/rcS index 6ae93a882f..387dba1ebb 100644 --- a/posix-configs/eagle/init/rcS +++ b/posix-configs/eagle/init/rcS @@ -4,7 +4,6 @@ # (px4-alias.sh is expected to be in the PATH) . px4-alias.sh -uorb start muorb start # We need to wait until the DSP side is ready before diff --git a/posix-configs/excelsior/mainapp.config b/posix-configs/excelsior/mainapp.config index 1d26ad5f7b..c2a8740fc0 100644 --- a/posix-configs/excelsior/mainapp.config +++ b/posix-configs/excelsior/mainapp.config @@ -3,7 +3,6 @@ # (px4-alias.sh is expected to be in the PATH) . px4-alias.sh -uorb start muorb start logger start -e -t param set CBRK_SUPPLY_CHK 894281 diff --git a/posix-configs/excelsior/px4.config b/posix-configs/excelsior/px4.config index d0206e5f3d..f634095b35 100644 --- a/posix-configs/excelsior/px4.config +++ b/posix-configs/excelsior/px4.config @@ -1,4 +1,3 @@ -uorb start qshell start param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 4001 diff --git a/posix-configs/ocpoc/px4.config b/posix-configs/ocpoc/px4.config index 82de41b52f..e533c2b5a6 100644 --- a/posix-configs/ocpoc/px4.config +++ b/posix-configs/ocpoc/px4.config @@ -3,8 +3,6 @@ # (px4-alias.sh is expected to be in the PATH) . px4-alias.sh -uorb start - param select /root/rootfs/eeprom/parameters if [ -f /root/rootfs/eeprom/parameters ] then diff --git a/posix-configs/rpi/pilotpi_fw.config b/posix-configs/rpi/pilotpi_fw.config index 56ec62f6e1..977b27649e 100644 --- a/posix-configs/rpi/pilotpi_fw.config +++ b/posix-configs/rpi/pilotpi_fw.config @@ -3,8 +3,6 @@ # (px4-alias.sh is expected to be in the PATH) . px4-alias.sh -uorb start - if [ -f eeprom/parameters ] then param load diff --git a/posix-configs/rpi/pilotpi_mc.config b/posix-configs/rpi/pilotpi_mc.config index 12b6816f24..d09bd21c0e 100644 --- a/posix-configs/rpi/pilotpi_mc.config +++ b/posix-configs/rpi/pilotpi_mc.config @@ -3,8 +3,6 @@ # (px4-alias.sh is expected to be in the PATH) . px4-alias.sh -uorb start - if [ -f eeprom/parameters ] then param load diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index 55bdb73284..d9a63b25ae 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -5,8 +5,6 @@ # navio config for a quad -uorb start - if [ -f eeprom/parameters ] then param load diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index 7f2f5c228e..b814e624d0 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -5,8 +5,6 @@ # navio config for FW -uorb start - if [ -f eeprom/parameters ] then param load diff --git a/posix-configs/rpi/px4_hil.config b/posix-configs/rpi/px4_hil.config index b9d945d2bd..8d1abad588 100644 --- a/posix-configs/rpi/px4_hil.config +++ b/posix-configs/rpi/px4_hil.config @@ -7,8 +7,6 @@ # connect to it with jMAVSim: # ./Tools/jmavsim_run.sh -q -i -p 14577 -r 250 -uorb start - if [ -f eeprom/parameters ] then param load diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config index d31dc08ffe..07562034f9 100644 --- a/posix-configs/rpi/px4_test.config +++ b/posix-configs/rpi/px4_test.config @@ -5,8 +5,6 @@ # navio config for simple testing -uorb start - if [ -f eeprom/parameters ] then param load diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 8651c0b02c..8952a2c638 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -48,12 +48,10 @@ extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); BATT_SMBUS::BATT_SMBUS(I2CSPIBusOption bus_option, const int bus, SMBus *interface) : - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, + interface->get_device_address()), _interface(interface) { - battery_status_s new_report = {}; - _batt_topic = orb_advertise(ORB_ID(battery_status), &new_report); - int battsource = 1; int batt_device_type = (int)SMBUS_DEVICE_TYPE::UNDEFINED; @@ -192,7 +190,9 @@ void BATT_SMBUS::RunImpl() } new_report.interface_error = perf_event_count(_interface->_interface_errors); - orb_publish(ORB_ID(battery_status), _batt_topic, &new_report); + + int instance = 0; + orb_publish_auto(ORB_ID(battery_status), &_batt_topic, &new_report, &instance); _last_report = new_report; } diff --git a/src/drivers/bootloaders/CMakeLists.txt b/src/drivers/bootloaders/CMakeLists.txt index 3fca22697d..47e0df1942 100644 --- a/src/drivers/bootloaders/CMakeLists.txt +++ b/src/drivers/bootloaders/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2017, 2021 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,7 +31,10 @@ # ############################################################################ -add_library(drivers_bootloaders boot_app_shared.c) +add_library(drivers_bootloaders + boot_app_shared.c + boot_alt_app_shared.c + ) add_dependencies(drivers_bootloaders prebuild_targets) include_directories(include) @@ -45,7 +48,7 @@ if(NOT "${PX4_BOARD_LABEL}" MATCHES "canbootloader") set(SW_MAJOR ${uavcanblid_sw_version_major}) set(SW_MINOR ${uavcanblid_sw_version_minor}) - string(REPLACE "\"" "" HWNAME ${uavcanblid_name}) + math(EXPR HWBOARD_ID "(${HW_MAJOR} << 8) + ${HW_MINOR}") execute_process( COMMAND git rev-list HEAD --max-count=1 --abbrev=8 --abbrev-commit @@ -57,12 +60,16 @@ if(NOT "${PX4_BOARD_LABEL}" MATCHES "canbootloader") if ("${uavcanbl_git_desc}" STREQUAL "") set(uavcanbl_git_desc ffffffff) endif() - set(uavcan_bl_imange_name ${HWNAME}-${HW_MAJOR}.${HW_MINOR}-${SW_MAJOR}.${SW_MINOR}.${uavcanbl_git_desc}.uavcan.bin) + set(uavcan_bl_imange_name ${HWBOARD_ID}-${SW_MAJOR}.${SW_MINOR}.${uavcanbl_git_desc}.uavcan.bin) message(STATUS "Generating UAVCAN Bootable as ${uavcan_bl_imange_name}") - add_custom_command(OUTPUT ${uavcan_bl_imange_name} + add_custom_command(OUTPUT ${uavcan_bl_imange_name} deploy/${HWBOARD_ID}.bin COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/make_can_boot_descriptor.py - -v --use-git-hash ${PX4_BINARY_DIR}/${PX4_BOARD}.bin ${uavcan_bl_imange_name} + -v --use-git-hash ${PX4_BOARD}.bin ${uavcan_bl_imange_name} + COMMAND + COMMAND ${CMAKE_COMMAND} -E make_directory deploy + COMMAND + ${CMAKE_COMMAND} -E copy ${uavcan_bl_imange_name} deploy/${HWBOARD_ID}.bin DEPENDS ${PX4_BINARY_DIR}/${PX4_BOARD}.bin ${CMAKE_CURRENT_SOURCE_DIR}/make_can_boot_descriptor.py diff --git a/src/drivers/bootloaders/boot_alt_app_shared.c b/src/drivers/bootloaders/boot_alt_app_shared.c new file mode 100644 index 0000000000..ac0b726e85 --- /dev/null +++ b/src/drivers/bootloaders/boot_alt_app_shared.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#if defined(SUPPORT_ALT_CAN_BOOTLOADER) +#include +#include + +#include "chip.h" +#include "stm32.h" + +#include + +#include "boot_alt_app_shared.h" + +#include + +/**************************************************************************** + * Name: bootloader_alt_app_shared_read + * + * Description: + * Based on the role requested, this function will conditionally populate + * a bootloader_alt_app_shared_t structure from the physical locations used + * to transfer the shared data to/from an application (internal data) . + * + * The functions will only populate the structure and return a status + * indicating success, if the internal data has the correct signature as + * requested by the Role AND has a valid crc. + * + * Input Parameters: + * shared - A pointer to a bootloader_alt_app_shared_t return the data in if + * the internal data is valid for the requested Role + * role - An eRole_t of App or BootLoader to validate the internal data + * against. For a Bootloader this would be the value of App to + * read the application passed data. + * + * Returned value: + * OK - Indicates that the internal data has been copied to callers + * bootloader_alt_app_shared_t structure. + * + * -EBADR - internal data was not valid. The copy did not occur. + * + ****************************************************************************/ +__EXPORT int bootloader_alt_app_shared_read(bootloader_alt_app_shared_t *alt_shared) +{ + int rv = EBADR; + bootloader_alt_app_shared_t *bootloader_alt_app_shared = (bootloader_alt_app_shared_t *)&_sapp_bl_shared; + + if (bootloader_alt_app_shared->signature == BL_ALT_APP_SHARED_SIGNATURE) { + *alt_shared = *bootloader_alt_app_shared; + rv = 0; + } + + return rv; +} + +/**************************************************************************** + * Name: bootloader_alt_app_shared_write + * + * Description: + * Based on the role, this function will commit the data passed + * into the physical locations used to transfer the shared data to/from + * an application (internal data) . + * + * The functions will populate the signature and crc the data + * based on the provided Role. + * + * Input Parameters: + * shared - A pointer to a bootloader_alt_app_shared_t data to commit to + * the internal data for passing to/from an application. + * role - An eRole_t of App or BootLoader to use in the internal data + * to be passed to/from an application. For a Bootloader this + * would be the value of Bootloader to write to the passed data. + * to the application via the internal data. + * + * Returned value: + * None. + * + ****************************************************************************/ +__EXPORT void bootloader_alt_app_shared_write(bootloader_alt_app_shared_t *alt_shared) +{ + + bootloader_alt_app_shared_t *bootloader_alt_app_shared = (bootloader_alt_app_shared_t *)&_sapp_bl_shared; + *bootloader_alt_app_shared = *alt_shared; + bootloader_alt_app_shared->signature = BL_ALT_APP_SHARED_SIGNATURE; + +} + +/**************************************************************************** + * Name: bootloader_app_shared_invalidate + * + * Description: + * Invalidates the data passed the physical locations used to transfer + * the shared data to/from an application (internal data) . + * + * The functions will invalidate the signature and crc and shoulf be used + * to prevent deja vu. + * + * Input Parameters: + * None. + * + * Returned value: + * None. + * + ****************************************************************************/ +__EXPORT void bootloader_alt_app_shared_invalidate(void) +{ + bootloader_alt_app_shared_t *bootloader_alt_app_shared = (bootloader_alt_app_shared_t *)&_sapp_bl_shared; + bootloader_alt_app_shared->signature = 0; + +} +#endif diff --git a/src/drivers/bootloaders/boot_alt_app_shared.h b/src/drivers/bootloaders/boot_alt_app_shared.h new file mode 100644 index 0000000000..a4ede85718 --- /dev/null +++ b/src/drivers/bootloaders/boot_alt_app_shared.h @@ -0,0 +1,148 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#if defined(SUPPORT_ALT_CAN_BOOTLOADER) +__BEGIN_DECLS + +/**************************************************************************** + * + * Bootloader and Application shared structure. + * + * The data in this structure is passed in SRAM or the the CAN filter + * registers from bootloader to application and application to bootloader. + * + * Do not assume any mapping or location for the passing of this data + * that is done in the read and write routines and is abstracted by design. + * + * For reference, the following is performed based on eRole in API calls + * defined below: + * + * The application must write BOOTLOADER_COMMON_APP_SIGNATURE to the + * signature field when passing data to the bootloader; when the + * bootloader passes data to the application, it must write + * BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE to the signature field. + * + * The CRC is calculated over the structure from signature to the + * last byte. The resulting values are then copied to the CAN filter + * registers by bootloader_app_shared_read and + * bootloader_app_shared_write. + * +****************************************************************************/ + +#define BL_ALT_APP_SHARED_SIGNATURE 0xc544ad9a +typedef begin_packed_struct struct bootloader_alt_app_shared_t { + uint32_t signature; + uint32_t reserved[4]; + uint8_t fw_server_node_id; + uint8_t node_id; + uint8_t path[201]; +} end_packed_struct bootloader_alt_app_shared_t; +#pragma GCC diagnostic pop + +extern bootloader_alt_app_shared_t _sapp_bl_shared; + +/**************************************************************************** + * Name: bootloader_alt_app_shared_read + * + * Description: + * Based on the role requested, this function will conditionally populate + * a bootloader_alt_app_shared_t structure from the physical locations used + * to transfer the shared data to/from an application (internal data) . + * + * The functions will only populate the structure and return a status + * indicating success, if the internal data has the correct signature as + * requested by the Role AND has a valid crc. + * + * Input Parameters: + * shared - A pointer to a bootloader_alt_app_shared_t return the data in if + * the internal data is valid for the requested Role + * + * Returned value: + * OK - Indicates that the internal data has been copied to callers + * bootloader_alt_app_shared_t structure. + * + * -EBADR - The Role or crc of the internal data was not valid. The copy + * did not occur. + * + ****************************************************************************/ + +int bootloader_alt_app_shared_read(bootloader_alt_app_shared_t *alt_shared); + +/**************************************************************************** + * Name: bootloader_alt_app_shared_write + * + * Description: + * Based on the role, this function will commit the data passed + * into the physical locations used to transfer the shared data to/from + * an application (internal data) . + * + * The functions will populate the signature and crc the data + * based on the provided Role. + * + * Input Parameters: + * shared - A pointer to a bootloader_alt_app_shared_t data to commit to + * the internal data for passing to/from an application. + * + * Returned value: + * None. + * + ****************************************************************************/ + +void bootloader_alt_app_shared_write(bootloader_alt_app_shared_t *alt_shared); + +/**************************************************************************** + * Name: bootloader_alt_app_shared_invalidate + * + * Description: + * Invalidates the data passed the physical locations used to transfer + * the shared data to/from an application (internal data) . + * + * The functions will invalidate the signature and crc and should be used + * to prevent deja vu. + * + * Input Parameters: + * None. + * + * Returned value: + * None. + * + ****************************************************************************/ + +void bootloader_alt_app_shared_invalidate(void); + +__END_DECLS +#endif diff --git a/src/drivers/bootloaders/boot_app_shared.h b/src/drivers/bootloaders/boot_app_shared.h index 71f424895f..ed36d53769 100644 --- a/src/drivers/bootloaders/boot_app_shared.h +++ b/src/drivers/bootloaders/boot_app_shared.h @@ -44,9 +44,7 @@ __BEGIN_DECLS * revision number of 00 used in app_descriptor_t */ -#define APP_DESCRIPTOR_SIGNATURE_ID 'A','P','D','e','s','c' -#define APP_DESCRIPTOR_SIGNATURE_REV '0','0' -#define APP_DESCRIPTOR_SIGNATURE APP_DESCRIPTOR_SIGNATURE_ID, APP_DESCRIPTOR_SIGNATURE_REV +#define APP_DESCRIPTOR_SIGNATURE { 0x40, 0xa2, 0xe4, 0xf1, 0x64, 0x68, 0x91, 0x06 } /* N.B. the .ld file must emit this sections */ # define boot_app_shared_section __attribute__((section(".app_descriptor"))) @@ -96,7 +94,6 @@ typedef begin_packed_struct struct bootloader_app_shared_t { uint32_t bus_speed; uint32_t node_id; } end_packed_struct bootloader_app_shared_t; -#pragma GCC diagnostic pop /**************************************************************************** * @@ -128,12 +125,19 @@ typedef begin_packed_struct struct bootloader_app_shared_t { #pragma GCC diagnostic ignored "-Wpacked" typedef begin_packed_struct struct app_descriptor_t { uint8_t signature[sizeof(uint64_t)]; - uint64_t image_crc; + union { + uint64_t image_crc; + struct { + uint32_t crc32_block1; + uint32_t crc32_block2; + }; + }; uint32_t image_size; - uint32_t vcs_commit; - uint8_t major_version; - uint8_t minor_version; - uint8_t reserved[6]; + uint32_t git_hash; + uint8_t major_version; + uint8_t minor_version; + uint16_t board_id; + uint8_t reserved[ 3 + 3 + 2]; } end_packed_struct app_descriptor_t; #pragma GCC diagnostic pop diff --git a/src/drivers/bootloaders/make_can_boot_descriptor.py b/src/drivers/bootloaders/make_can_boot_descriptor.py index ccda76c96a..a0a5256142 100755 --- a/src/drivers/bootloaders/make_can_boot_descriptor.py +++ b/src/drivers/bootloaders/make_can_boot_descriptor.py @@ -7,6 +7,41 @@ import struct import optparse import binascii from io import BytesIO +import array + +crctab = array.array('I', [ + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d]) class GitWrapper: @classmethod @@ -21,26 +56,31 @@ class GitWrapper: class AppDescriptor(object): """ UAVCAN firmware image descriptor format: - uint64_t signature (bytes [7:0] set to 'APDesc00' by linker script) - uint64_t image_crc (set to 0 by linker script) - uint32_t image_size (set to 0 by linker script) - uint32_t vcs_commit (set in source or by this tool) + uint64_t signature (bytes [7:0] set to '{ 0x40, 0xa2, 0xe4, 0xf1, 0x64, 0x68, 0x91, 0x06 }' by the build + uint32_t crc32_block1 From offset 0 to . (non inclusive) (set by this tool) + uint32_t crc32_block2 From offsetof(minor_version) to end (set by this tool) + uint32_t image_size (set to 0 by linker script) + uint32_t vcs_commit (set in source or by this tool) uint8_t version_major (set in source) uint8_t version_minor (set in source) + uint16_t board_id (set in source) uint8_t reserved[6] (set to 0xFF by linker script) """ - LENGTH = 8 + 8 + 4 + 4 + 1 + 1 + 6 - SIGNATURE = b"APDesc00" - RESERVED = b"\xFF" * 6 + LENGTH = 8 + 4 + 4 + 4 + 4 + 1 + 1 + 2 + 8 + DESLENGTH = 4 + 4 + 4 + 4 + SIGNATURE = b"\x40\xa2\xe4\xf1\x64\x68\x91\x06" + RESERVED = b"\xFF" * 8 def __init__(self, bytes=None): self.signature = AppDescriptor.SIGNATURE - self.image_crc = 0 + self.crc32_block1 = 0 + self.crc32_block2 = 0 self.image_size = 0 self.vcs_commit = 0 self.version_major = 0 self.version_minor = 0 + self.board_id = 0 self.reserved = AppDescriptor.RESERVED if bytes: @@ -51,15 +91,16 @@ class AppDescriptor(object): binascii.b2a_hex(bytes))) def pack(self): - return struct.pack("<8sQLLBB6s", self.signature, self.image_crc, - self.image_size, self.vcs_commit, + return struct.pack("<8sLLLLBBH8s", self.signature, self.crc32_block1, + self.crc32_block2, self.image_size, self.vcs_commit, self.version_major, self.version_minor, + self.board_id, self.reserved) def unpack(self, bytes): - (self.signature, self.image_crc, self.image_size, self.vcs_commit, - self.version_major, self.version_minor, self.reserved) = \ - struct.unpack("<8sQLLBB6s", bytes) + (self.signature, self.crc32_block1, self.crc32_block2, self.image_size, self.vcs_commit, + self.version_major, self.version_minor, self.board_id, self.reserved) = \ + struct.unpack("<8sLLLLBBH8s", bytes) if not self.empty and not self.valid: raise ValueError() @@ -67,13 +108,14 @@ class AppDescriptor(object): @property def empty(self): return (self.signature == AppDescriptor.SIGNATURE and - self.image_crc == 0 and self.image_size == 0 and - self.reserved == AppDescriptor.RESERVED) + self.crc32_block1 == 0 and self.crc32_block2 == 0 and + self.image_size == 0 and self.reserved == AppDescriptor.RESERVED) @property def valid(self): return (self.signature == AppDescriptor.SIGNATURE and - self.image_crc != 0 and self.image_size > 0 and + self.crc32_block1 != 0 and self.crc32_block2 != 0 and + self.image_size > 0 and self.board_id != 0 and self.reserved == AppDescriptor.RESERVED) @@ -136,38 +178,31 @@ class FirmwareImage(object): # Set the descriptor's length and CRC to the values required for # CRC computation self.app_descriptor.image_size = self.length - self.app_descriptor.image_crc = 0 + self.app_descriptor.crc32_block1 = 0 + self.app_descriptor.crc32_block2 = 0 self._write_descriptor_raw() + content = bytearray(self._contents.getvalue()) + if self._padding: + content += bytearray.fromhex("ff" * self._padding) + # Update the descriptor's CRC based on the computed value and write # it out again - self.app_descriptor.image_crc = self.crc + + self.app_descriptor.crc32_block1 = self.crc32(content[:self.app_descriptor_offset + len(AppDescriptor.SIGNATURE)]) + b2 = self.app_descriptor_offset + len(AppDescriptor.SIGNATURE) + AppDescriptor.DESLENGTH + self.app_descriptor.crc32_block2 = self.crc32(content[b2:]) self._write_descriptor_raw() - @property - def crc(self): - MASK = 0xFFFFFFFFFFFFFFFF - POLY = 0x42F0E1EBA9EA3693 + def crc32(self, bytes, crc = 0): - # Calculate the image CRC with the image_crc field in the app - # descriptor zeroed out. - crc_offset = self.app_descriptor_offset + len(AppDescriptor.SIGNATURE) - content = bytearray(self._contents.getvalue()) - content[crc_offset:crc_offset + 8] = bytearray.fromhex("00" * 8) - if self._padding: - content += bytearray.fromhex("ff" * self._padding) - val = MASK - for byte in content: - val ^= (byte << 56) & MASK - for bit in range(8): - if val & (1 << 63): - val = ((val << 1) & MASK) ^ POLY - else: - val <<= 1 - return (val & MASK) ^ MASK + for byte in bytes: + index = (crc ^ byte) & 0xff + crc = crctab[index] ^ (crc >> 8) + return crc @property def padding(self): @@ -234,7 +269,6 @@ class FirmwareImage(object): def app_descriptor(self, value): self._descriptor = value - if __name__ == "__main__": parser = optparse.OptionParser(usage="usage: %prog [options] [IN OUT]") parser.add_option("--vcs-commit", dest="vcs_commit", default=None, @@ -305,15 +339,31 @@ bootloader image to the output image. """.format(in_image, in_image.app_descriptor, out_image.app_descriptor, bootloader_size, len(bootloader_image))) sys.stderr.write( -"""------------------------------------------------------------------------------ +"""READ VALUES +------------------------------------------------------------------------------ +Field Type Value +signature uint64 {1.signature!r} +crc32_block1 uint32 0x{1.crc32_block1:08X} +crc32_block2 uint32 0x{1.crc32_block2:08X} +image_size uint32 0x{1.image_size:X} ({1.image_size:d} B) +vcs_commit uint32 {1.vcs_commit:08X} +version_major uint8 {1.version_major:d} +version_minor uint8 {1.version_minor:d} +board_id uint32 0x{1.board_id:X} +reserved uint8[8] {1.reserved!r} + +WRITTEN VALUES +------------------------------------------------------------------------------ Field Type Value signature uint64 {2.signature!r} -image_crc uint64 0x{2.image_crc:016X} +crc32_block1 uint32 0x{2.crc32_block1:08X} +crc32_block2 uint32 0x{2.crc32_block2:08X} image_size uint32 0x{2.image_size:X} ({2.image_size:d} B) vcs_commit uint32 {2.vcs_commit:08X} version_major uint8 {2.version_major:d} version_minor uint8 {2.version_minor:d} -reserved uint8[6] {2.reserved!r} +board_id uint32 0x{2.board_id:X} +reserved uint8[8] {2.reserved!r} """.format(in_image, in_image.app_descriptor, out_image.app_descriptor, bootloader_size, len(bootloader_image))) if out_image.padding: diff --git a/src/drivers/drv_watchdog.h b/src/drivers/drv_watchdog.h new file mode 100644 index 0000000000..9baeeb063d --- /dev/null +++ b/src/drivers/drv_watchdog.h @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file drv_watchdog.h + * + * watchdog driver interface. + * + */ + +#pragma once + +#include + +__BEGIN_DECLS + +void watchdog_init(void); +void watchdog_pet(void); +__END_DECLS diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 4338dcb7f3..c278eb48e1 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 4338dcb7f38c45fc37c1946c37cffb1b2ef1e935 +Subproject commit c278eb48e1b57e6b789242324e28581caf9ccf96 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index a2f9c34b7b..e7bd0a4cdb 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -103,8 +103,8 @@ public: Count }; - GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool fake_gps, bool enable_sat_info, - Instance instance, unsigned configured_baudrate); + GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool enable_sat_info, Instance instance, + unsigned configured_baudrate); ~GPS() override; /** @see ModuleBase */ @@ -175,8 +175,6 @@ private: unsigned _num_bytes_read{0}; ///< counter for number of read bytes from the UART (within update interval) unsigned _rate_reading{0}; ///< reading rate in B/s - const bool _fake_gps; ///< fake gps output - const Instance _instance; uORB::Subscription _orb_inject_data_sub{ORB_ID(gps_inject_data)}; @@ -258,12 +256,11 @@ px4::atomic GPS::_secondary_instance{nullptr}; extern "C" __EXPORT int gps_main(int argc, char *argv[]); -GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool fake_gps, - bool enable_sat_info, Instance instance, unsigned configured_baudrate) : +GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interface, bool enable_sat_info, + Instance instance, unsigned configured_baudrate) : _configured_baudrate(configured_baudrate), _mode(mode), _interface(interface), - _fake_gps(fake_gps), _instance(instance) { /* store port name */ @@ -637,37 +634,35 @@ void GPS::dumpGpsData(uint8_t *data, size_t len, bool msg_to_gps_device) void GPS::run() { - if (!_fake_gps) { - /* open the serial port */ - _serial_fd = ::open(_port, O_RDWR | O_NOCTTY); + /* open the serial port */ + _serial_fd = ::open(_port, O_RDWR | O_NOCTTY); - if (_serial_fd < 0) { - PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno); - return; - } + if (_serial_fd < 0) { + PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno); + return; + } #ifdef __PX4_LINUX - if (_interface == GPSHelper::Interface::SPI) { - int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi) - int status_value = ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); + if (_interface == GPSHelper::Interface::SPI) { + int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi) + int status_value = ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); - if (status_value < 0) { - PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno); - return; - } - - status_value = ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed); - - if (status_value < 0) { - PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno); - return; - } + if (status_value < 0) { + PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno); + return; } -#endif /* __PX4_LINUX */ + status_value = ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed); + + if (status_value < 0) { + PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno); + return; + } } +#endif /* __PX4_LINUX */ + param_t handle = param_find("GPS_YAW_OFFSET"); float heading_offset = 0.f; @@ -719,122 +714,89 @@ GPS::run() /* loop handling received serial bytes and also configuring in between */ while (!should_exit()) { + if (_helper != nullptr) { + delete (_helper); + _helper = nullptr; + } - if (_fake_gps) { - _report_gps_pos.timestamp = hrt_absolute_time(); - _report_gps_pos.lat = (int32_t)47.378301e7f; - _report_gps_pos.lon = (int32_t)8.538777e7f; - _report_gps_pos.alt = (int32_t)1200e3f; - _report_gps_pos.alt_ellipsoid = 10000; - _report_gps_pos.s_variance_m_s = 0.5f; - _report_gps_pos.c_variance_rad = 0.1f; - _report_gps_pos.fix_type = 3; - _report_gps_pos.eph = 0.8f; - _report_gps_pos.epv = 1.2f; - _report_gps_pos.hdop = 0.9f; - _report_gps_pos.vdop = 0.9f; - _report_gps_pos.vel_n_m_s = 0.0f; - _report_gps_pos.vel_e_m_s = 0.0f; - _report_gps_pos.vel_d_m_s = 0.0f; - _report_gps_pos.vel_m_s = 0.0f; - _report_gps_pos.cog_rad = 0.0f; - _report_gps_pos.vel_ned_valid = true; - _report_gps_pos.satellites_used = 10; - _report_gps_pos.heading = NAN; - _report_gps_pos.heading_offset = NAN; + switch (_mode) { + case GPS_DRIVER_MODE_NONE: + _mode = GPS_DRIVER_MODE_UBX; - /* no time and satellite information simulated */ - - - publish(); - - px4_usleep(200000); - - } else { - - if (_helper != nullptr) { - delete (_helper); - _helper = nullptr; - } - - switch (_mode) { - case GPS_DRIVER_MODE_NONE: - _mode = GPS_DRIVER_MODE_UBX; - - /* FALLTHROUGH */ - case GPS_DRIVER_MODE_UBX: - _helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info, - gps_ubx_dynmodel, heading_offset, ubx_mode); - break; + /* FALLTHROUGH */ + case GPS_DRIVER_MODE_UBX: + _helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info, + gps_ubx_dynmodel, heading_offset, ubx_mode); + break; #ifndef CONSTRAINED_FLASH - case GPS_DRIVER_MODE_MTK: - _helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos); - break; + case GPS_DRIVER_MODE_MTK: + _helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos); + break; - case GPS_DRIVER_MODE_ASHTECH: - _helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset); - break; + case GPS_DRIVER_MODE_ASHTECH: + _helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset); + break; - case GPS_DRIVER_MODE_EMLIDREACH: - _helper = new GPSDriverEmlidReach(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info); - break; + case GPS_DRIVER_MODE_EMLIDREACH: + _helper = new GPSDriverEmlidReach(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info); + break; #endif // CONSTRAINED_FLASH - default: - break; + default: + break; + } + + _baudrate = _configured_baudrate; + GPSHelper::GPSConfig gpsConfig{}; + gpsConfig.output_mode = GPSHelper::OutputMode::GPS; + gpsConfig.gnss_systems = static_cast(gnssSystemsParam); + + if (_helper && _helper->configure(_baudrate, gpsConfig) == 0) { + + /* reset report */ + memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); + _report_gps_pos.heading = NAN; + _report_gps_pos.heading_offset = heading_offset; + + if (_mode == GPS_DRIVER_MODE_UBX) { + + /* GPS is obviously detected successfully, reset statistics */ + _helper->resetUpdateRates(); } - _baudrate = _configured_baudrate; - GPSHelper::GPSConfig gpsConfig{}; - gpsConfig.output_mode = GPSHelper::OutputMode::GPS; - gpsConfig.gnss_systems = static_cast(gnssSystemsParam); + int helper_ret; - if (_helper && _helper->configure(_baudrate, gpsConfig) == 0) { + while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !should_exit()) { - /* reset report */ - memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); - _report_gps_pos.heading = NAN; - _report_gps_pos.heading_offset = heading_offset; + if (helper_ret & 1) { + publish(); - if (_mode == GPS_DRIVER_MODE_UBX) { + last_rate_count++; + } - /* GPS is obviously detected successfully, reset statistics */ + if (_p_report_sat_info && (helper_ret & 2)) { + publishSatelliteInfo(); + } + + reset_if_scheduled(); + + /* measure update rate every 5 seconds */ + if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { + float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f; + _rate = last_rate_count / dt; + _rate_rtcm_injection = _last_rate_rtcm_injection_count / dt; + _rate_reading = _num_bytes_read / dt; + last_rate_measurement = hrt_absolute_time(); + last_rate_count = 0; + _last_rate_rtcm_injection_count = 0; + _num_bytes_read = 0; + _helper->storeUpdateRates(); _helper->resetUpdateRates(); } - int helper_ret; - - while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !should_exit()) { - - if (helper_ret & 1) { - publish(); - - last_rate_count++; - } - - if (_p_report_sat_info && (helper_ret & 2)) { - publishSatelliteInfo(); - } - - reset_if_scheduled(); - - /* measure update rate every 5 seconds */ - if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { - float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f; - _rate = last_rate_count / dt; - _rate_rtcm_injection = _last_rate_rtcm_injection_count / dt; - _rate_reading = _num_bytes_read / dt; - last_rate_measurement = hrt_absolute_time(); - last_rate_count = 0; - _last_rate_rtcm_injection_count = 0; - _num_bytes_read = 0; - _helper->storeUpdateRates(); - _helper->resetUpdateRates(); - } - - if (!_healthy) { - // Helpful for debugging, but too verbose for normal ops + if (!_healthy) { + // Helpful for debugging, but too verbose for normal ops // const char *mode_str = "unknown"; // // switch (_mode) { @@ -859,46 +821,44 @@ GPS::run() // } // // PX4_WARN("module found: %s", mode_str); - _healthy = true; - } - } - - if (_healthy) { - _healthy = false; - _rate = 0.0f; - _rate_rtcm_injection = 0.0f; + _healthy = true; } } - if (_mode_auto) { - switch (_mode) { - case GPS_DRIVER_MODE_UBX: + if (_healthy) { + _healthy = false; + _rate = 0.0f; + _rate_rtcm_injection = 0.0f; + } + } + + if (_mode_auto) { + switch (_mode) { + case GPS_DRIVER_MODE_UBX: #ifndef CONSTRAINED_FLASH - _mode = GPS_DRIVER_MODE_MTK; - break; + _mode = GPS_DRIVER_MODE_MTK; + break; - case GPS_DRIVER_MODE_MTK: - _mode = GPS_DRIVER_MODE_ASHTECH; - break; + case GPS_DRIVER_MODE_MTK: + _mode = GPS_DRIVER_MODE_ASHTECH; + break; - case GPS_DRIVER_MODE_ASHTECH: - _mode = GPS_DRIVER_MODE_EMLIDREACH; - break; + case GPS_DRIVER_MODE_ASHTECH: + _mode = GPS_DRIVER_MODE_EMLIDREACH; + break; - case GPS_DRIVER_MODE_EMLIDREACH: + case GPS_DRIVER_MODE_EMLIDREACH: #endif // CONSTRAINED_FLASH - _mode = GPS_DRIVER_MODE_UBX; - px4_usleep(500000); // tried all possible drivers. Wait a bit before next round - break; + _mode = GPS_DRIVER_MODE_UBX; + px4_usleep(500000); // tried all possible drivers. Wait a bit before next round + break; - default: - break; - } - - } else { - px4_usleep(500000); + default: + break; } + } else { + px4_usleep(500000); } } @@ -927,33 +887,28 @@ GPS::print_status() break; } - //GPS Mode - if (_fake_gps) { - PX4_INFO("protocol: SIMULATED"); - - } else { - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - PX4_INFO("protocol: UBX"); - break; + // GPS Mode + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + PX4_INFO("protocol: UBX"); + break; #ifndef CONSTRAINED_FLASH - case GPS_DRIVER_MODE_MTK: - PX4_INFO("protocol: MTK"); - break; + case GPS_DRIVER_MODE_MTK: + PX4_INFO("protocol: MTK"); + break; - case GPS_DRIVER_MODE_ASHTECH: - PX4_INFO("protocol: ASHTECH"); - break; + case GPS_DRIVER_MODE_ASHTECH: + PX4_INFO("protocol: ASHTECH"); + break; - case GPS_DRIVER_MODE_EMLIDREACH: - PX4_INFO("protocol: EMLIDREACH"); - break; + case GPS_DRIVER_MODE_EMLIDREACH: + PX4_INFO("protocol: EMLIDREACH"); + break; #endif // CONSTRAINED_FLASH - default: - break; - } + default: + break; } PX4_INFO("status: %s, port: %s, baudrate: %d", _healthy ? "OK" : "NOT OK", _port, _baudrate); @@ -966,10 +921,8 @@ GPS::print_status() PX4_INFO("rate velocity: \t\t%6.2f Hz", (double)_helper->getVelocityUpdateRate()); } - if (!_fake_gps) { - PX4_INFO("rate publication:\t\t%6.2f Hz", (double)_rate); - PX4_INFO("rate RTCM injection:\t%6.2f Hz", (double)_rate_rtcm_injection); - } + PX4_INFO("rate publication:\t\t%6.2f Hz", (double)_rate); + PX4_INFO("rate RTCM injection:\t%6.2f Hz", (double)_rate_rtcm_injection); print_message(_report_gps_pos); } @@ -1097,9 +1050,6 @@ There is a thread for each device polling for data. The GPS protocol classes are so that they can be used in other projects as well (eg. QGroundControl uses them too). ### Examples -For testing it can be useful to fake a GPS signal (it will signal the system that it has a valid position): -$ gps stop -$ gps start -f Starting 2 GPS devices (the main GPS on /dev/ttyS3 and the secondary on /dev/ttyS4): $ gps start -d /dev/ttyS3 -e /dev/ttyS4 @@ -1115,7 +1065,6 @@ $ gps reset warm PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "", "Optional secondary GPS device", true); PRINT_MODULE_USAGE_PARAM_INT('g', 0, 0, 3000000, "Baudrate (secondary GPS, can also be p:)", true); - PRINT_MODULE_USAGE_PARAM_FLAG('f', "Fake a GPS signal (useful for testing)", true); PRINT_MODULE_USAGE_PARAM_FLAG('s', "Enable publication of satellite info", true); PRINT_MODULE_USAGE_PARAM_STRING('i', "uart", "spi|uart", "GPS interface", true); @@ -1186,7 +1135,6 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) const char *device_name_secondary = nullptr; int baudrate_main = 0; int baudrate_secondary = 0; - bool fake_gps = false; bool enable_sat_info = false; GPSHelper::Interface interface = GPSHelper::Interface::UART; GPSHelper::Interface interface_secondary = GPSHelper::Interface::UART; @@ -1197,7 +1145,7 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) int ch; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "b:d:e:fg:si:j:p:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "b:d:e:g:si:j:p:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'b': if (px4_get_parameter_value(myoptarg, baudrate_main) != 0) { @@ -1220,10 +1168,6 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) device_name_secondary = myoptarg; break; - case 'f': - fake_gps = true; - break; - case 's': enable_sat_info = true; break; @@ -1290,7 +1234,7 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) GPS *gps; if (instance == Instance::Main) { - gps = new GPS(device_name, mode, interface, fake_gps, enable_sat_info, instance, baudrate_main); + gps = new GPS(device_name, mode, interface, enable_sat_info, instance, baudrate_main); if (gps && device_name_secondary) { task_spawn(argc, argv, Instance::Secondary); @@ -1308,7 +1252,7 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) } } } else { // secondary instance - gps = new GPS(device_name_secondary, mode, interface_secondary, fake_gps, enable_sat_info, instance, baudrate_secondary); + gps = new GPS(device_name_secondary, mode, interface_secondary, enable_sat_info, instance, baudrate_secondary); } return gps; diff --git a/src/drivers/heater/heater.cpp b/src/drivers/heater/heater.cpp index f73cbfca08..aca5d55eca 100644 --- a/src/drivers/heater/heater.cpp +++ b/src/drivers/heater/heater.cpp @@ -73,17 +73,11 @@ Heater::Heater() : } #endif - - heater_enable(); } Heater::~Heater() { heater_disable(); - -#ifdef HEATER_PX4IO - px4_close(_io_fd); -#endif } int Heater::custom_command(int argc, char *argv[]) @@ -97,28 +91,35 @@ int Heater::custom_command(int argc, char *argv[]) return print_usage("Unrecognized command."); } -uint32_t Heater::get_sensor_id() -{ - return _sensor_accel.device_id; -} - void Heater::heater_disable() { // Reset heater to off state. #ifdef HEATER_PX4IO - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_DISABLED); + if (_io_fd >= 0) { + px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_DISABLED); + } + #endif + #ifdef HEATER_GPIO px4_arch_configgpio(GPIO_HEATER_OUTPUT); #endif } -void Heater::heater_enable() +void Heater::heater_initialize() { // Initialize heater to off state. #ifdef HEATER_PX4IO - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF); + if (_io_fd < 0) { + _io_fd = px4_open(IO_HEATER_DEVICE_PATH, O_RDWR); + } + + if (_io_fd >= 0) { + px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF); + } + #endif + #ifdef HEATER_GPIO px4_arch_configgpio(GPIO_HEATER_OUTPUT); #endif @@ -127,8 +128,13 @@ void Heater::heater_enable() void Heater::heater_off() { #ifdef HEATER_PX4IO - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF); + + if (_io_fd >= 0) { + px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF); + } + #endif + #ifdef HEATER_GPIO px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0); #endif @@ -137,64 +143,170 @@ void Heater::heater_off() void Heater::heater_on() { #ifdef HEATER_PX4IO - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_ON); + + if (_io_fd >= 0) { + px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_ON); + } + #endif + #ifdef HEATER_GPIO px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 1); #endif } -void Heater::initialize_topics() +bool Heater::initialize_topics() { - // Get the total number of accelerometer instances. - uint8_t number_of_imus = orb_group_count(ORB_ID(sensor_accel)); + for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + uORB::SubscriptionData sensor_accel_sub{ORB_ID(sensor_accel), i}; - // Get the total number of accelerometer instances and check each instance for the correct ID. - for (uint8_t x = 0; x < number_of_imus; x++) { - _sensor_accel.device_id = 0; + if (sensor_accel_sub.get().timestamp != 0 && sensor_accel_sub.get().device_id != 0 + && PX4_ISFINITE(sensor_accel_sub.get().temperature)) { - while (_sensor_accel.device_id == 0) { - _sensor_accel_sub = uORB::Subscription{ORB_ID(sensor_accel), x}; - - if (!_sensor_accel_sub.advertised()) { - px4_usleep(100); - continue; + // If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance. + if (sensor_accel_sub.get().device_id == (uint32_t)_param_sens_temp_id.get()) { + _sensor_accel_sub.ChangeInstance(i); + _sensor_device_id = sensor_accel_sub.get().device_id; + return true; } - - _sensor_accel_sub.copy(&_sensor_accel); - } - - // If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance. - if (_sensor_accel.device_id == (uint32_t)_param_sens_temp_id.get()) { - break; } } - // Exit the driver if the sensor ID does not match the desired sensor. - if (_sensor_accel.device_id != (uint32_t)_param_sens_temp_id.get()) { - request_stop(); - PX4_ERR("Could not identify IMU sensor."); - } + return false; } -int Heater::print_status() +void Heater::Run() { - float _feedforward_value = _param_sens_imu_temp_ff.get(); + if (should_exit()) { +#if defined(HEATER_PX4IO) - PX4_INFO("Sensor ID: %d,\tSetpoint: %3.2fC,\t Sensor Temperature: %3.2fC,\tDuty Cycle (usec): %d", - _sensor_accel.device_id, - static_cast(_param_sens_imu_temp.get()), - static_cast(_sensor_accel.temperature), - _controller_period_usec); - PX4_INFO("Feed Forward control effort: %3.2f%%,\tProportional control effort: %3.2f%%,\tIntegrator control effort: %3.3f%%,\t Heater cycle: %3.2f%%", - static_cast(_feedforward_value * 100), - static_cast(_proportional_value * 100), - static_cast(_integrator_value * 100), - static_cast(static_cast(_controller_time_on_usec) / static_cast(_controller_period_usec) * 100)); + // must be closed from wq thread + if (_io_fd >= 0) { + px4_close(_io_fd); + } +#endif + exit_and_cleanup(); + return; + } + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + ModuleParams::updateParams(); + } + + if (_sensor_device_id == 0) { + if (initialize_topics()) { + heater_initialize(); + + } else { + // if sensor still not found try again in 1 second + ScheduleDelayed(1_s); + return; + } + } + + sensor_accel_s sensor_accel; + + if (_heater_on) { + // Turn the heater off. + heater_off(); + _heater_on = false; + + } else if (_sensor_accel_sub.update(&sensor_accel)) { + float temperature_delta {0.f}; + + // Update the current IMU sensor temperature if valid. + if (PX4_ISFINITE(sensor_accel.temperature)) { + temperature_delta = _param_sens_imu_temp.get() - sensor_accel.temperature; + _temperature_last = sensor_accel.temperature; + } + + _proportional_value = temperature_delta * _param_sens_imu_temp_p.get(); + _integrator_value += temperature_delta * _param_sens_imu_temp_i.get(); + + if (fabs(_param_sens_imu_temp_i.get()) <= 0.0001) { + _integrator_value = 0.f; + } + + // Guard against integrator wind up. + _integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f); + + _controller_time_on_usec = static_cast((_param_sens_imu_temp_ff.get() + _proportional_value + + _integrator_value) * static_cast(_controller_period_usec)); + + _controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec); + + _heater_on = true; + heater_on(); + } + + // Schedule the next cycle. + if (_heater_on) { + ScheduleDelayed(_controller_time_on_usec); + + } else { + ScheduleDelayed(_controller_period_usec - _controller_time_on_usec); + } + + + // publish status + heater_status_s status{}; + status.heater_on = _heater_on; + status.device_id = _sensor_device_id; + status.temperature_sensor = _temperature_last; + status.temperature_target = _param_sens_imu_temp.get(); + status.controller_period_usec = _controller_period_usec; + status.controller_time_on_usec = _controller_time_on_usec; + status.proportional_value = _proportional_value; + status.integrator_value = _integrator_value; + status.feed_forward_value = _param_sens_imu_temp_ff.get(); + +#ifdef HEATER_PX4IO + status.mode |= heater_status_s::MODE_PX4IO; +#endif +#ifdef HEATER_GPIO + status.mode |= heater_status_s::MODE_GPIO; +#endif + + status.timestamp = hrt_absolute_time(); + _heater_status_pub.publish(status); +} + +int Heater::start() +{ + // Exit the driver if the sensor ID does not match the desired sensor. + if (_param_sens_temp_id.get() == 0) { + PX4_ERR("Valid SENS_TEMP_ID required"); + request_stop(); + return PX4_ERROR; + } + + ScheduleNow(); return PX4_OK; } +int Heater::task_spawn(int argc, char *argv[]) +{ + Heater *heater = new Heater(); + + if (!heater) { + PX4_ERR("driver allocation failed"); + return PX4_ERROR; + } + + _object.store(heater); + _task_id = task_id_is_work_queue; + + heater->start(); + return 0; +} + int Heater::print_usage(const char *reason) { if (reason) { @@ -216,101 +328,7 @@ This task can be started at boot from the startup scripts by setting SENS_EN_THE return 0; } -void Heater::Run() -{ - if (should_exit()) { - exit_and_cleanup(); - return; - } - - if (_heater_on) { - // Turn the heater off. - heater_off(); - _heater_on = false; - - } else { - update_params(false); - - _sensor_accel_sub.update(&_sensor_accel); - - float temperature_delta {0.f}; - - // Update the current IMU sensor temperature if valid. - if (!isnan(_sensor_accel.temperature)) { - temperature_delta = _param_sens_imu_temp.get() - _sensor_accel.temperature; - } - - _proportional_value = temperature_delta * _param_sens_imu_temp_p.get(); - _integrator_value += temperature_delta * _param_sens_imu_temp_i.get(); - - if (fabs(_param_sens_imu_temp_i.get()) <= 0.0001) { - _integrator_value = 0.f; - } - - // Guard against integrator wind up. - _integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f); - - _controller_time_on_usec = static_cast((_param_sens_imu_temp_ff.get() + _proportional_value + - _integrator_value) * static_cast(_controller_period_usec)); - - _controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec); - - _heater_on = true; - heater_on(); - } - - // Schedule the next cycle. - if (_heater_on) { - ScheduleDelayed(_controller_time_on_usec); - - } else { - ScheduleDelayed(_controller_period_usec - _controller_time_on_usec); - } -} - -int Heater::start() -{ - update_params(true); - initialize_topics(); - - // Allow sufficient time for all additional sensors and processes to start. - ScheduleDelayed(100000); - return PX4_OK; -} - -int Heater::task_spawn(int argc, char *argv[]) -{ - Heater *heater = new Heater(); - - if (!heater) { - PX4_ERR("driver allocation failed"); - return PX4_ERROR; - } - - _object.store(heater); - _task_id = task_id_is_work_queue; - - heater->start(); - return 0; -} - -void Heater::update_params(const bool force) -{ - // check for parameter updates - if (_parameter_update_sub.updated() || force) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - // update parameters from storage - ModuleParams::updateParams(); - } -} - -/** - * Main entry point for the heater driver module - */ -int heater_main(int argc, char *argv[]) +extern "C" __EXPORT int heater_main(int argc, char *argv[]) { return Heater::main(argc, argv); } diff --git a/src/drivers/heater/heater.h b/src/drivers/heater/heater.h index 126287c452..37be610630 100644 --- a/src/drivers/heater/heater.h +++ b/src/drivers/heater/heater.h @@ -47,7 +47,9 @@ #include #include #include +#include #include +#include #include #include @@ -57,13 +59,6 @@ using namespace time_literals; #define CONTROLLER_PERIOD_DEFAULT 100000 -/** - * @brief IMU Heater Controller driver used to maintain consistent - * temparature at the IMU. - */ -extern "C" __EXPORT int heater_main(int argc, char *argv[]); - - class Heater : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: @@ -97,30 +92,6 @@ public: */ static int task_spawn(int argc, char *argv[]); - /** - * @brief Sets and/or reports the heater controller time period value in microseconds. - * @param argv Pointer to the input argument array. - * @return Returns 0 iff successful, -1 otherwise. - */ - int controller_period(char *argv[]); - - /** @brief Returns the id of the target sensor. */ - uint32_t get_sensor_id(); - - /** - * @brief Sets and/or reports the heater controller integrator gain value. - * @param argv Pointer to the input argument array. - * @return Returns the heater integrator gain value iff successful, 0.0f otherwise. - */ - float integrator(char *argv[]); - - /** - * @brief Sets and/or reports the heater controller proportional gain value. - * @param argv Pointer to the input argument array. - * @return Returns the heater proportional gain value iff successful, 0.0f otherwise. - */ - float proportional(char *argv[]); - /** * @brief Initiates the heater driver work queue, starts a new background task, * and fails if it is already running. @@ -128,25 +99,10 @@ public: */ int start(); - /** - * @brief Reports curent status and diagnostic information about the heater driver. - * @return Returns 0 iff successful, -1 otherwise. - */ - int print_status(); - - /** - * @brief Sets and/or reports the heater target temperature. - * @param argv Pointer to the input argument array. - * @return Returns the heater target temperature value iff successful, -1.0f otherwise. - */ - float temperature_setpoint(char *argv[]); - -protected: +private: /** @brief Called once to initialize uORB topics. */ - void initialize_topics(); - -private: + bool initialize_topics(); /** @brief Calculates the heater element on/off time and schedules the next cycle. */ void Run() override; @@ -158,7 +114,7 @@ private: void update_params(const bool force = false); /** Enables / configures the heater (either by GPIO or PX4IO). */ - void heater_enable(); + void heater_initialize(); /** Disnables the heater (either by GPIO or PX4IO). */ void heater_disable(); @@ -174,7 +130,7 @@ private: /** File descriptor for PX4IO for heater ioctl's */ #if defined(HEATER_PX4IO) - int _io_fd; + int _io_fd_ {-1}; #endif bool _heater_on = false; @@ -185,10 +141,15 @@ private: float _integrator_value = 0.0f; float _proportional_value = 0.0f; + uORB::Publication _heater_status_pub{ORB_ID(heater_status)}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _sensor_accel_sub{ORB_ID(sensor_accel)}; - sensor_accel_s _sensor_accel{}; + + uint32_t _sensor_device_id{0}; + + float _temperature_last{NAN}; DEFINE_PARAMETERS( (ParamFloat) _param_sens_imu_temp_ff, diff --git a/src/drivers/px4io/CMakeLists.txt b/src/drivers/px4io/CMakeLists.txt index f82c3f9487..df9dbdde43 100644 --- a/src/drivers/px4io/CMakeLists.txt +++ b/src/drivers/px4io/CMakeLists.txt @@ -58,6 +58,7 @@ ExternalProject_Add(px4io_firmware USES_TERMINAL_BUILD true DEPENDS git_nuttx git_nuttx_apps BUILD_ALWAYS 1 + BUILD_BYPRODUCTS "${PX4_BINARY_DIR}/external/Build/px4io_firmware/${config_io_board}.elf" ) ExternalProject_Get_Property(px4io_firmware BINARY_DIR) @@ -71,7 +72,7 @@ file(RELATIVE_PATH fw_io_bin_relative ${CMAKE_CURRENT_BINARY_DIR} ${fw_io_bin}) add_custom_command(OUTPUT ${fw_io_bin} COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/romfs_extras COMMAND ${CMAKE_OBJCOPY} -O binary ${fw_io_exe_relative} ${fw_io_bin_relative} - DEPENDS px4io_firmware + DEPENDS ${fw_io_exe} px4io_firmware COMMENT "Copying ${config_io_board} to ROMFS extras" ) add_custom_target(copy_px4io_bin DEPENDS ${fw_io_bin}) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 01439fc45e..1a89fa43fc 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1399,6 +1399,7 @@ PX4IO::io_set_control_state(unsigned group) case 3: changed = _t_actuator_controls_3.update(&controls); break; + } if (!changed && (!_in_esc_calibration_mode || group != 0)) { @@ -2652,6 +2653,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) { ret = -EINVAL; io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); + PX4_ERR("failed setting PWM rate on IO"); } break; @@ -2951,19 +2953,26 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) PX4_ERR("not upgrading IO firmware, system is armed"); return -EINVAL; - } else if (system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { - // re-enable safety - ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, PX4IO_P_STATUS_FLAGS_SAFETY_OFF, 0); - - // set new status - _status &= ~(PX4IO_P_STATUS_FLAGS_SAFETY_OFF); } + // re-enable safety + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC); + + if (ret != PX4_OK) { + PX4_ERR("IO refused to re-enable safety"); + } + + // set new status + _status &= ~(PX4IO_P_STATUS_FLAGS_SAFETY_OFF); + /* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */ usleep(1); - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg); - // we don't expect a reply from this operation - ret = OK; + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg); + + if (ret != PX4_OK) { + PX4_ERR("IO refused to reboot"); + } + break; case PX4IO_CHECK_CRC: { @@ -2976,7 +2985,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } if (io_crc != arg) { - PX4_DEBUG("crc mismatch 0x%08x 0x%08lx", io_crc, arg); + PX4_DEBUG("Firmware CRC mismatch 0x%08x 0x%08lx", io_crc, arg); return -EINVAL; } @@ -3172,7 +3181,8 @@ checkcrc(int argc, char *argv[]) (void)new PX4IO(interface); if (g_dev == nullptr) { - errx(1, "driver allocation failed"); + PX4_ERR("driver allocation failed"); + exit(1); } } else { @@ -3184,14 +3194,14 @@ checkcrc(int argc, char *argv[]) check IO CRC against CRC of a file */ if (argc < 2) { - warnx("usage: px4io checkcrc filename"); + PX4_WARN("usage: px4io checkcrc filename"); exit(1); } int fd = open(argv[1], O_RDONLY); if (fd == -1) { - warnx("open of %s failed: %d", argv[1], errno); + PX4_ERR("open of %s failed: %d", argv[1], errno); exit(1); } @@ -3225,8 +3235,11 @@ checkcrc(int argc, char *argv[]) } if (ret != OK) { - warn("check CRC failed: %d", ret); + PX4_ERR("check CRC failed: %d, CRC: %u", ret, fw_crc); exit(1); + + } else { + PX4_INFO("IO FW CRC match"); } exit(0); @@ -3398,100 +3411,85 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "update")) { - if (g_dev != nullptr) { - warnx("loaded, detaching first"); - /* stop the driver */ + constexpr unsigned MAX_RETRIES = 5; + unsigned retries = 0; + int ret = PX4_ERROR; + + while (ret != OK && retries < MAX_RETRIES) { + + retries++; + // Sleep 200 ms before the next attempt + usleep(200 * 1000); + + if (g_dev == nullptr) { + /* allocate the interface */ + device::Device *interface = get_interface(); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(interface); + + if (g_dev == nullptr) { + delete interface; + errx(1, "driver allocation failed"); + } + } + + // Try to reboot + ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, PX4IO_REBOOT_BL_MAGIC); + // tear down the px4io instance delete g_dev; g_dev = nullptr; + + if (ret != OK) { + PX4_ERR("reboot failed - %d, still attempting upgrade", ret); + } + + PX4IO_Uploader *up; + + /* Assume we are using default paths */ + + const char *fn[4] = PX4IO_FW_SEARCH_PATHS; + + /* Override defaults if a path is passed on command line */ + if (argc > 2) { + fn[0] = argv[2]; + fn[1] = nullptr; + } + + up = new PX4IO_Uploader; + ret = up->upload(&fn[0]); + delete up; } - PX4IO_Uploader *up; - - /* Assume we are using default paths */ - - const char *fn[4] = PX4IO_FW_SEARCH_PATHS; - - /* Override defaults if a path is passed on command line */ - if (argc > 2) { - fn[0] = argv[2]; - fn[1] = nullptr; - } - - up = new PX4IO_Uploader; - int ret = up->upload(&fn[0]); - delete up; - switch (ret) { case OK: break; case -ENOENT: - errx(1, "PX4IO firmware file not found"); + PX4_ERR("PX4IO firmware file not found"); + break; case -EEXIST: case -EIO: - errx(1, "error updating PX4IO - check that bootloader mode is enabled"); + PX4_ERR("error updating PX4IO - check that bootloader mode is enabled"); + break; case -EINVAL: - errx(1, "verify failed - retry the update"); + PX4_ERR("verify failed - retry the update"); + break; case -ETIMEDOUT: - errx(1, "timed out waiting for bootloader - power-cycle and try again"); + PX4_ERR("timed out waiting for bootloader - power-cycle and try again"); + break; default: - errx(1, "unexpected error %d", ret); + PX4_ERR("unexpected error %d", ret); + break; } return ret; } - if (!strcmp(argv[1], "forceupdate")) { - /* - force update of the IO firmware without requiring - the user to hold the safety switch down - */ - if (argc <= 3) { - warnx("usage: px4io forceupdate MAGIC filename"); - exit(1); - } - - if (g_dev == nullptr) { - warnx("px4io is not started, still attempting upgrade"); - - /* allocate the interface */ - device::Device *interface = get_interface(); - - /* create the driver - it will set g_dev */ - (void)new PX4IO(interface); - - if (g_dev == nullptr) { - delete interface; - errx(1, "driver allocation failed"); - } - } - - uint16_t arg = atol(argv[2]); - int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); - - if (ret != OK) { - warnx("reboot failed - %d", ret); - exit(1); - } - - // tear down the px4io instance - delete g_dev; - g_dev = nullptr; - - // upload the specified firmware - const char *fn[2]; - fn[0] = argv[3]; - fn[1] = nullptr; - PX4IO_Uploader *up = new PX4IO_Uploader; - up->upload(&fn[0]); - delete up; - exit(0); - } - /* commands below here require a started driver */ if (g_dev == nullptr) { @@ -3671,6 +3669,6 @@ px4io_main(int argc, char *argv[]) out: errx(1, "need a command, try 'start', 'stop', 'status', 'monitor', 'debug ',\n" "'recovery', 'bind', 'checkcrc', 'safety_on', 'safety_off',\n" - "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm',\n" + "'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm',\n" "'test_fmu_fail', 'test_fmu_ok'"); } diff --git a/src/drivers/rc_input/CMakeLists.txt b/src/drivers/rc_input/CMakeLists.txt index 1ad8262481..6c58fb2605 100644 --- a/src/drivers/rc_input/CMakeLists.txt +++ b/src/drivers/rc_input/CMakeLists.txt @@ -37,6 +37,7 @@ px4_add_module( SRCS RCInput.cpp crsf_telemetry.cpp + ghst_telemetry.cpp MODULE_CONFIG module.yaml DEPENDS diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 1e37018c21..8fcabd9f8a 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -72,6 +72,7 @@ RCInput::~RCInput() dsm_deinit(); delete _crsf_telemetry; + delete _ghst_telemetry; perf_free(_cycle_perf); perf_free(_publish_interval_perf); @@ -625,6 +626,53 @@ void RCInput::Run() } } + } else { + // Scan the next protocol + set_rc_scan_state(RC_SCAN_GHST); + } + + break; + + case RC_SCAN_GHST: + if (_rc_scan_begin == 0) { + _rc_scan_begin = cycle_timestamp; + // Configure serial port for GHST + ghst_config(_rcs_fd); + rc_io_invert(false); + + } else if (_rc_scan_locked + || cycle_timestamp - _rc_scan_begin < rc_scan_max) { + + // parse new data + if (newBytes > 0) { + int8_t ghst_rssi = -1; + rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi, + &_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS); + + if (rc_updated) { + // we have a new GHST frame. Publish it. + _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; + fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi); + + // Enable GHST Telemetry only on the Omnibus, because on Pixhawk (-related) boards + // we cannot write to the RC UART + // It might work on FMU-v5. Or another option is to use a different UART port +#ifdef CONFIG_ARCH_BOARD_OMNIBUS_F4SD + + if (!_rc_scan_locked && !_ghst_telemetry) { + _ghst_telemetry = new GHSTTelemetry(_rcs_fd); + } + +#endif /* CONFIG_ARCH_BOARD_OMNIBUS_F4SD */ + + _rc_scan_locked = true; + + if (_ghst_telemetry) { + _ghst_telemetry->update(cycle_timestamp); + } + } + } + } else { // Scan the next protocol set_rc_scan_state(RC_SCAN_SBUS); @@ -732,6 +780,10 @@ int RCInput::print_status() PX4_INFO("CRSF Telemetry: %s", _crsf_telemetry ? "yes" : "no"); break; + case RC_SCAN_GHST: + PX4_INFO("GHST Telemetry: %s", _ghst_telemetry ? "yes" : "no"); + break; + case RC_SCAN_SBUS: PX4_INFO("SBUS frame drops: %u", sbus_dropped_frames()); break; diff --git a/src/drivers/rc_input/RCInput.hpp b/src/drivers/rc_input/RCInput.hpp index 9d17b2abd8..f965aa764b 100644 --- a/src/drivers/rc_input/RCInput.hpp +++ b/src/drivers/rc_input/RCInput.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -60,6 +61,7 @@ #include #include "crsf_telemetry.h" +#include "ghst_telemetry.h" #ifdef HRT_PPM_CHANNEL # include @@ -94,16 +96,18 @@ private: RC_SCAN_DSM, RC_SCAN_SUMD, RC_SCAN_ST24, - RC_SCAN_CRSF + RC_SCAN_CRSF, + RC_SCAN_GHST } _rc_scan_state{RC_SCAN_SBUS}; - static constexpr char const *RC_SCAN_STRING[6] { + static constexpr char const *RC_SCAN_STRING[7] { "PPM", "SBUS", "DSM", "SUMD", "ST24", - "CRSF" + "CRSF", + "GHST" }; void Run() override; @@ -159,6 +163,7 @@ private: uint16_t _raw_rc_count{}; CRSFTelemetry *_crsf_telemetry{nullptr}; + GHSTTelemetry *_ghst_telemetry{nullptr}; perf_counter_t _cycle_perf; perf_counter_t _publish_interval_perf; diff --git a/src/drivers/rc_input/ghst_telemetry.cpp b/src/drivers/rc_input/ghst_telemetry.cpp new file mode 100644 index 0000000000..ab9da12698 --- /dev/null +++ b/src/drivers/rc_input/ghst_telemetry.cpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ghst_telemetry.cpp + * + * IRC Ghost (Immersion RC Ghost) telemetry. + * + * @author Igor Misic + * @author Juraj Ciberlin + */ + +#include "ghst_telemetry.h" +#include + +GHSTTelemetry::GHSTTelemetry(int uart_fd) : + _uart_fd(uart_fd) +{ +} + +bool GHSTTelemetry::update(const hrt_abstime &now) +{ + const int update_rate_hz = 10; + + if (now - _last_update <= 1_s / (update_rate_hz * num_data_types)) { + return false; + } + + bool sent = false; + + switch (_next_type) { + case 0: + sent = send_battery_status(); + break; + } + + _last_update = now; + _next_type = (_next_type + 1) % num_data_types; + + return sent; +} + +bool GHSTTelemetry::send_battery_status() +{ + battery_status_s battery_status; + + if (!_battery_status_sub.update(&battery_status)) { + return false; + } + + uint16_t voltage = battery_status.voltage_filtered_v * 10; + uint16_t current = battery_status.current_filtered_a * 10; + uint16_t fuel = battery_status.discharged_mah; + return ghst_send_telemetry_battery(_uart_fd, voltage, current, fuel); +} diff --git a/src/drivers/rc_input/ghst_telemetry.h b/src/drivers/rc_input/ghst_telemetry.h new file mode 100644 index 0000000000..0bbba0e0d5 --- /dev/null +++ b/src/drivers/rc_input/ghst_telemetry.h @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ghst_telemetry.cpp + * + * IRC Ghost (Immersion RC Ghost) telemetry. + * + * @author Igor Misic + * @author Juraj Ciberlin + */ + +#pragma once + +#include +#include +#include + +using namespace time_literals; + +/** + * High-level class that handles sending of GHST telemetry data + */ +class GHSTTelemetry +{ +public: + /** + * @param uart_fd file descriptor for the UART to use. It is expected to be configured + * already. + */ + GHSTTelemetry(int uart_fd); + + ~GHSTTelemetry() = default; + + /** + * Send telemetry data. Call this regularly (i.e. at 100Hz), it will automatically + * limit the sending rate. + * @return true if new data sent + */ + bool update(const hrt_abstime &now); + +private: + bool send_battery_status(); + + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + + hrt_abstime _last_update{0}; + + static constexpr int num_data_types{1}; // number of different telemetry data types + int _next_type{0}; + + int _uart_fd; +}; diff --git a/src/drivers/uavcan/libuavcan b/src/drivers/uavcan/libuavcan index 04377cc270..29e1aa6c12 160000 --- a/src/drivers/uavcan/libuavcan +++ b/src/drivers/uavcan/libuavcan @@ -1 +1 @@ -Subproject commit 04377cc2704eb7e2fae36fffa5e67063cc40b4c0 +Subproject commit 29e1aa6c12e3590252b102153d8c62155047a676 diff --git a/src/drivers/uavcan/uavcan_module.hpp b/src/drivers/uavcan/uavcan_module.hpp index 4d8247ccb3..ea7420482a 100644 --- a/src/drivers/uavcan/uavcan_module.hpp +++ b/src/drivers/uavcan/uavcan_module.hpp @@ -49,12 +49,13 @@ // firmware paths #define UAVCAN_MAX_PATH_LENGTH (128 + 40) -#define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw" +#define UAVCAN_SD_ROOT_PATH "/fs/microsd/" +#define UAVCAN_FIRMWARE_PATH UAVCAN_SD_ROOT_PATH"ufw" #define UAVCAN_ROMFS_FW_PATH "/etc/uavcan/fw" #define UAVCAN_ROMFS_FW_PREFIX "_" // logging -#define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db" +#define UAVCAN_NODE_DB_PATH UAVCAN_SD_ROOT_PATH"/uavcan.db" #define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log" // device files diff --git a/src/drivers/uavcan/uavcan_servers.cpp b/src/drivers/uavcan/uavcan_servers.cpp index a11590b838..827374d84b 100644 --- a/src/drivers/uavcan/uavcan_servers.cpp +++ b/src/drivers/uavcan/uavcan_servers.cpp @@ -38,6 +38,7 @@ #include #include +#include #include #include #include @@ -72,6 +73,7 @@ /* * UavcanNode */ + UavcanServers *UavcanServers::_instance; UavcanServers::UavcanServers(uavcan::INode &main_node) : @@ -208,7 +210,7 @@ UavcanServers::init() * Initialize the fw version checker. * giving it its path */ - ret = _fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH); + ret = _fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH); if (ret < 0) { PX4_ERR("FirmwareVersionChecker init: %d, errno: %d", ret, errno); @@ -217,7 +219,7 @@ UavcanServers::init() /* Start fw file server back */ - ret = _fw_server.start(); + ret = _fw_server.start(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH); if (ret < 0) { PX4_ERR("BasicFileServer init: %d, errno: %d", ret, errno); @@ -263,7 +265,7 @@ UavcanServers::init() } /* Start the fw version checker */ - ret = _fw_upgrade_trigger.start(_node_info_retriever, _fw_version_checker.getFirmwarePath()); + ret = _fw_upgrade_trigger.start(_node_info_retriever); if (ret < 0) { PX4_ERR("FirmwareUpdateTrigger init: %d", ret); @@ -282,10 +284,10 @@ UavcanServers::run(pthread_addr_t) Lock lock(_subnode_mutex); /* - Copy any firmware bundled in the ROMFS to the appropriate location on the - SD card, unless the user has copied other firmware for that device. + Check for firmware in the root directory, move it to appropriate location on + the SD card, as defined by the APDesc. */ - unpackFwFromROMFS(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH); + migrateFWFromRoot(UAVCAN_FIRMWARE_PATH, UAVCAN_SD_ROOT_PATH); /* the subscribe call needs to happen in the same thread, * so not in the constructor */ @@ -949,259 +951,84 @@ UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResultd_type)) { - continue; + if (rv != 0) { + PX4_ERR("dev: couldn't create '%s'", sd_path); + return; } - - // Make sure the path fits - size_t dev_dirname_len = strlen(dev_dirent->d_name); - size_t srcpath_dev_len = romfs_path_len + 1 + dev_dirname_len; - - if (srcpath_dev_len > maxlen) { - PX4_WARN("dev: srcpath '%s/%s' too long", romfs_path, dev_dirent->d_name); - continue; - } - - size_t dstpath_dev_len = sd_path_len + 1 + dev_dirname_len; - - if (dstpath_dev_len > maxlen) { - PX4_WARN("dev: dstpath '%s/%s' too long", sd_path, dev_dirent->d_name); - continue; - } - - // Create the device name directory on the SD card if it doesn't already exist - dstpath[sd_path_len] = '/'; - memcpy(&dstpath[sd_path_len + 1], dev_dirent->d_name, dev_dirname_len + 1); - - if (stat(dstpath, &sb) != 0 || !S_ISDIR(sb.st_mode)) { - rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO); - - if (rv != 0) { - PX4_ERR("dev: couldn't create '%s'", dstpath); - continue; - } - } - - // Set up the source path - srcpath[romfs_path_len] = '/'; - memcpy(&srcpath[romfs_path_len + 1], dev_dirent->d_name, dev_dirname_len + 1); - - DIR *const dev_dir = opendir(srcpath); - - if (!dev_dir) { - PX4_ERR("dev: couldn't open '%s'", srcpath); - continue; - } - - // Iterate over all version directories in the current ROMFS device directory - struct dirent *ver_dirent = NULL; - - while ((ver_dirent = readdir(dev_dir)) != NULL) { - // Skip if not a directory - if (!DIRENT_ISDIRECTORY(ver_dirent->d_type)) { - continue; - } - - // Make sure the path fits - size_t ver_dirname_len = strlen(ver_dirent->d_name); - size_t srcpath_ver_len = srcpath_dev_len + 1 + ver_dirname_len; - - if (srcpath_ver_len > maxlen) { - PX4_ERR("ver: srcpath '%s/%s' too long", srcpath, ver_dirent->d_name); - continue; - } - - size_t dstpath_ver_len = dstpath_dev_len + 1 + ver_dirname_len; - - if (dstpath_ver_len > maxlen) { - PX4_ERR("ver: dstpath '%s/%s' too long", dstpath, ver_dirent->d_name); - continue; - } - - // Create the device version directory on the SD card if it doesn't already exist - dstpath[dstpath_dev_len] = '/'; - memcpy(&dstpath[dstpath_dev_len + 1], ver_dirent->d_name, ver_dirname_len + 1); - - if (stat(dstpath, &sb) != 0 || !S_ISDIR(sb.st_mode)) { - rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO); - - if (rv != 0) { - PX4_ERR("ver: couldn't create '%s'", dstpath); - continue; - } - } - - // Set up the source path - srcpath[srcpath_dev_len] = '/'; - memcpy(&srcpath[srcpath_dev_len + 1], ver_dirent->d_name, ver_dirname_len + 1); - - // Find the name of the bundled firmware file, or move on to the - // next directory if there's no file here. - DIR *const src_ver_dir = opendir(srcpath); - - if (!src_ver_dir) { - PX4_ERR("ver: couldn't open '%s'", srcpath); - continue; - } - - struct dirent *src_fw_dirent = NULL; - - while ((src_fw_dirent = readdir(src_ver_dir)) != NULL && - !DIRENT_ISFILE(src_fw_dirent->d_type)); - - if (!src_fw_dirent) { - (void)closedir(src_ver_dir); - continue; - } - - size_t fw_len = strlen(src_fw_dirent->d_name); - - bool copy_fw = true; - - // Clear out any romfs_ files in the version directory on the SD card - DIR *const dst_ver_dir = opendir(dstpath); - - if (!dst_ver_dir) { - PX4_ERR("unlink: couldn't open '%s'", dstpath); - - } else { - struct dirent *fw_dirent = NULL; - - while ((fw_dirent = readdir(dst_ver_dir)) != NULL) { - // Skip if not a file - if (!DIRENT_ISFILE(fw_dirent->d_type)) { - continue; - } - - if (!memcmp(&fw_dirent->d_name, src_fw_dirent->d_name, fw_len)) { - /* - * Exact match between SD card filename and ROMFS filename; must be the same version - * so don't bother deleting and rewriting it. - */ - copy_fw = false; - - } else if (!memcmp(fw_dirent->d_name, UAVCAN_ROMFS_FW_PREFIX, sizeof(UAVCAN_ROMFS_FW_PREFIX) - 1)) { - size_t dst_fw_len = strlen(fw_dirent->d_name); - size_t dstpath_fw_len = dstpath_ver_len + dst_fw_len; - - if (dstpath_fw_len > maxlen) { - // sizeof(prefix) includes trailing NUL, cancelling out the +1 for the path separator - PX4_ERR("unlink: path '%s/%s' too long", dstpath, fw_dirent->d_name); - - } else { - // File name starts with "_", delete it. - dstpath[dstpath_ver_len] = '/'; - memcpy(&dstpath[dstpath_ver_len + 1], fw_dirent->d_name, dst_fw_len + 1); - unlink(dstpath); - PX4_ERR("unlink: removed '%s'", dstpath); - } - - } else { - // User file, don't copy firmware - copy_fw = false; - } - } - - (void)closedir(dst_ver_dir); - } - - // If we need to, copy the file from ROMFS to the SD card - if (copy_fw) { - size_t srcpath_fw_len = srcpath_ver_len + 1 + fw_len; - size_t dstpath_fw_len = dstpath_ver_len + fw_len; - - if (srcpath_fw_len > maxlen) { - PX4_ERR("copy: srcpath '%s/%s' too long", srcpath, src_fw_dirent->d_name); - - } else if (dstpath_fw_len > maxlen) { - PX4_ERR("copy: dstpath '%s/%s' too long", dstpath, src_fw_dirent->d_name); - - } else { - // All OK, make the paths and copy the file - srcpath[srcpath_ver_len] = '/'; - memcpy(&srcpath[srcpath_ver_len + 1], src_fw_dirent->d_name, fw_len + 1); - - dstpath[dstpath_ver_len] = '/'; - memcpy(&dstpath[dstpath_ver_len + 1], src_fw_dirent->d_name, fw_len + 1); - - rv = copyFw(dstpath, srcpath); - - if (rv != 0) { - PX4_ERR("copy: '%s' -> '%s' failed: %d", srcpath, dstpath, rv); - - } else { - PX4_INFO("copy: '%s' -> '%s' succeeded", srcpath, dstpath); - } - } - } - - (void)closedir(src_ver_dir); - } - - (void)closedir(dev_dir); } - (void)closedir(romfs_dir); + // Iterate over all bin files in root directory + struct dirent *dev_dirent = NULL; + + while ((dev_dirent = readdir(sd_root_dir)) != nullptr) { + + uavcan_posix::FirmwareVersionChecker::AppDescriptor descriptor; + + // Looking for all uavcan.bin files. + + if (DIRENT_ISFILE(dev_dirent->d_type) && strstr(dev_dirent->d_name, ".bin") != nullptr) { + + // Make sure the path fits + + size_t filename_len = strlen(dev_dirent->d_name); + size_t srcpath_len = sd_root_path_len + 1 + filename_len; + + if (srcpath_len > maxlen) { + PX4_WARN("file: srcpath '%s%s' too long", sd_root_path, dev_dirent->d_name); + continue; + } + + snprintf(srcpath, sizeof(srcpath), "%s%s", sd_root_path, dev_dirent->d_name); + + if (uavcan_posix::FirmwareVersionChecker::getFileInfo(srcpath, descriptor, 1024) != 0) { + continue; + } + + if (descriptor.image_crc == 0) { + continue; + } + + snprintf(dstpath, sizeof(dstpath), "%s/%d.bin", sd_path, descriptor.board_id); + + if (copyFw(dstpath, srcpath) >= 0) { + unlink(srcpath); + } + } + } + + if (dev_dirent != nullptr) { + (void)closedir(dev_dirent); + } } int diff --git a/src/drivers/uavcan/uavcan_servers.hpp b/src/drivers/uavcan/uavcan_servers.hpp index 9fed3ec28b..15fa6cabeb 100644 --- a/src/drivers/uavcan/uavcan_servers.hpp +++ b/src/drivers/uavcan/uavcan_servers.hpp @@ -220,5 +220,6 @@ private: uavcan::ServiceClient _enumeration_save_client; void unpackFwFromROMFS(const char *sd_path, const char *romfs_path); + void migrateFWFromRoot(const char *sd_path, const char *sd_root_path); int copyFw(const char *dst, const char *src); }; diff --git a/src/drivers/uavcannode/CMakeLists.txt b/src/drivers/uavcannode/CMakeLists.txt index 2e02adf84f..7478f21d4a 100644 --- a/src/drivers/uavcannode/CMakeLists.txt +++ b/src/drivers/uavcannode/CMakeLists.txt @@ -134,6 +134,7 @@ px4_add_module( UavcanNodeParamManager.hpp UavcanNodeParamManager.cpp DEPENDS + arch_watchdog_iwdg px4_uavcan_dsdlc drivers_bootloaders diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index a45392d364..a5b2ef1768 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -34,7 +34,9 @@ #include "UavcanNode.hpp" #include "boot_app_shared.h" +#include "boot_alt_app_shared.h" +#include #include #include @@ -73,13 +75,16 @@ namespace uavcannode * image crc, size etc of this application */ boot_app_shared_section app_descriptor_t AppDescriptor = { - .signature = {APP_DESCRIPTOR_SIGNATURE}, - .image_crc = 0, + .signature = APP_DESCRIPTOR_SIGNATURE, + { + 0, + }, .image_size = 0, - .vcs_commit = 0, + .git_hash = 0, .major_version = APP_VERSION_MAJOR, .minor_version = APP_VERSION_MINOR, - .reserved = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff } + .board_id = HW_VERSION_MAJOR << 8 | HW_VERSION_MINOR, + .reserved = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff } }; UavcanNode *UavcanNode::_instance; @@ -144,6 +149,7 @@ int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver) int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) { + if (_instance != nullptr) { PX4_WARN("Already started"); return -1; @@ -231,6 +237,7 @@ void UavcanNode::busevent_signal_trampoline() static void cb_reboot(const uavcan::TimerEvent &) { + watchdog_pet(); board_reset(0); } @@ -246,6 +253,19 @@ void UavcanNode::cb_beginfirmware_update(const uavcan::ReceivedDataStructure 30_s) { + up_time = 0; + board_configure_reset(BOARD_RESET_MODE_RTC_BOOT_FWOK, 0); + } + perf_end(_cycle_perf); pthread_mutex_unlock(&_node_mutex); @@ -433,6 +508,9 @@ extern "C" int uavcannode_start(int argc, char *argv[]) { //board_app_initialize(nullptr); + // Sarted byt the bootloader, we must pet it + watchdog_pet(); + // CAN bitrate int32_t bitrate = 0; @@ -453,11 +531,24 @@ extern "C" int uavcannode_start(int argc, char *argv[]) } else { // Node ID - (void)param_get(param_find("CANNODE_NODE_ID"), &node_id); - (void)param_get(param_find("CANNODE_BITRATE"), &bitrate); +#if defined(SUPPORT_ALT_CAN_BOOTLOADER) + if (!board_booted_by_px4()) { + node_id = 0; + bitrate = 1000000; + + } else +#endif + { + (void)param_get(param_find("CANNODE_NODE_ID"), &node_id); + (void)param_get(param_find("CANNODE_BITRATE"), &bitrate); + } } - if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { + if ( +#if defined(SUPPORT_ALT_CAN_BOOTLOADER) + board_booted_by_px4() && +#endif + (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast())) { PX4_ERR("Invalid Node ID %i", node_id); return 1; } diff --git a/src/drivers/uavcannode/UavcanNode.hpp b/src/drivers/uavcannode/UavcanNode.hpp index 353bf87190..3bee1eb276 100644 --- a/src/drivers/uavcannode/UavcanNode.hpp +++ b/src/drivers/uavcannode/UavcanNode.hpp @@ -58,6 +58,7 @@ #include #include #include +#include #include diff --git a/src/examples/fake_gps/CMakeLists.txt b/src/examples/fake_gps/CMakeLists.txt new file mode 100644 index 0000000000..41539388da --- /dev/null +++ b/src/examples/fake_gps/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE modules__fake_gps + MAIN fake_gps + COMPILE_FLAGS + SRCS + FakeGps.cpp + FakeGps.hpp + DEPENDS + px4_work_queue +) diff --git a/src/examples/fake_gps/FakeGps.cpp b/src/examples/fake_gps/FakeGps.cpp new file mode 100644 index 0000000000..1605a2f9b9 --- /dev/null +++ b/src/examples/fake_gps/FakeGps.cpp @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "FakeGps.hpp" + +using namespace time_literals; + +FakeGps::FakeGps(double latitude_deg, double longitude_deg, float altitude_m) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default), + _latitude(latitude_deg * 10e6), + _longitude(longitude_deg * 10e6), + _altitude(altitude_m * 10e2f) +{ +} + +bool FakeGps::init() +{ + ScheduleOnInterval(SENSOR_INTERVAL_US); + return true; +} + +void FakeGps::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + sensor_gps_s sensor_gps{}; + sensor_gps.time_utc_usec = hrt_absolute_time() + 1613692609599954; + sensor_gps.lat = _latitude; + sensor_gps.lon = _longitude; + sensor_gps.alt = _altitude; + sensor_gps.alt_ellipsoid = _altitude; + sensor_gps.s_variance_m_s = 0.3740f; + sensor_gps.c_variance_rad = 0.6737f; + sensor_gps.eph = 2.1060f; + sensor_gps.epv = 3.8470f; + sensor_gps.hdop = 0.8800f; + sensor_gps.vdop = 1.3300f; + sensor_gps.noise_per_ms = 101; + sensor_gps.jamming_indicator = 35; + sensor_gps.vel_m_s = 0.0420f; + sensor_gps.vel_n_m_s = 0.0370f; + sensor_gps.vel_e_m_s = 0.0200f; + sensor_gps.vel_d_m_s = -0.0570f; + sensor_gps.cog_rad = 0.3988f; + sensor_gps.timestamp_time_relative = 0; + sensor_gps.heading = NAN; + sensor_gps.heading_offset = 0.0000; + sensor_gps.fix_type = 4; + sensor_gps.jamming_state = 0; + sensor_gps.vel_ned_valid = true; + sensor_gps.satellites_used = 14; + sensor_gps.timestamp = hrt_absolute_time(); + _sensor_gps_pub.publish(sensor_gps); +} + +int FakeGps::task_spawn(int argc, char *argv[]) +{ + FakeGps *instance = new FakeGps(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int FakeGps::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int FakeGps::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("fake_gps", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int fake_gps_main(int argc, char *argv[]) +{ + return FakeGps::main(argc, argv); +} diff --git a/src/examples/fake_gps/FakeGps.hpp b/src/examples/fake_gps/FakeGps.hpp new file mode 100644 index 0000000000..ef5a9197eb --- /dev/null +++ b/src/examples/fake_gps/FakeGps.hpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +class FakeGps : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + FakeGps(double latitude_deg = 29.6603018, double longitude_deg = -82.3160500, float altitude_m = 30.1f); + + ~FakeGps() override = default; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + static constexpr uint32_t SENSOR_INTERVAL_US{1000000 / 5}; // 5 Hz + + void Run() override; + + uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; + + int32_t _latitude{296603018}; // Latitude in 1e-7 degrees + int32_t _longitude{-823160500}; // Longitude in 1e-7 degrees + int32_t _altitude{30100}; // Altitude in 1e-3 meters above MSL, (millimetres) +}; diff --git a/src/lib/airspeed_validator/AirspeedValidator.cpp b/src/lib/airspeed_validator/AirspeedValidator.cpp index 75f80e9fbe..7b26acf6af 100644 --- a/src/lib/airspeed_validator/AirspeedValidator.cpp +++ b/src/lib/airspeed_validator/AirspeedValidator.cpp @@ -86,10 +86,10 @@ AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float air } // this function returns the current states of the wind estimator to be published in the airspeed module -wind_estimate_s +airspeed_wind_s AirspeedValidator::get_wind_estimator_states(uint64_t timestamp) { - wind_estimate_s wind_est = {}; + airspeed_wind_s wind_est = {}; wind_est.timestamp = timestamp; float wind[2]; diff --git a/src/lib/airspeed_validator/AirspeedValidator.hpp b/src/lib/airspeed_validator/AirspeedValidator.hpp index 5a02d3bd1a..8e1bc5e4d5 100644 --- a/src/lib/airspeed_validator/AirspeedValidator.hpp +++ b/src/lib/airspeed_validator/AirspeedValidator.hpp @@ -40,7 +40,7 @@ #include #include -#include +#include using matrix::Dcmf; @@ -86,7 +86,7 @@ public: bool get_airspeed_valid() { return _airspeed_valid; } float get_CAS_scale() {return _CAS_scale;} - wind_estimate_s get_wind_estimator_states(uint64_t timestamp); + airspeed_wind_s get_wind_estimator_states(uint64_t timestamp); // setters wind estimator parameters void set_wind_estimator_wind_p_noise(float wind_sigma) { _wind_estimator.set_wind_p_noise(wind_sigma); } diff --git a/src/lib/drivers/smbus/SMBus.cpp b/src/lib/drivers/smbus/SMBus.cpp index e0516a749e..3a5b62c161 100644 --- a/src/lib/drivers/smbus/SMBus.cpp +++ b/src/lib/drivers/smbus/SMBus.cpp @@ -108,8 +108,12 @@ int SMBus::write_word(const uint8_t cmd_code, uint16_t data) int SMBus::block_read(const uint8_t cmd_code, void *data, const uint8_t length, const bool use_pec) { uint8_t byte_count = 0; - // addr(wr), cmd_code, addr(r), byte_count, data (32 bytes max), pec - uint8_t rx_data[32 + 5]; + // addr(wr), cmd_code, addr(r), byte_count, data (MAX_BLOCK_LEN bytes max), pec + uint8_t rx_data[MAX_BLOCK_LEN + 5]; + + if (length > MAX_BLOCK_LEN) { + return -EINVAL; + } int result = transfer(&cmd_code, 1, (uint8_t *)&rx_data[3], length + 2); @@ -122,7 +126,7 @@ int SMBus::block_read(const uint8_t cmd_code, void *data, const uint8_t length, rx_data[0] = (device_address << 1) | 0x00; rx_data[1] = cmd_code; rx_data[2] = (device_address << 1) | 0x01; - byte_count = math::min(rx_data[3], (uint8_t)32); + byte_count = math::min(rx_data[3], MAX_BLOCK_LEN); // ensure data is not longer than given buffer memcpy(data, &rx_data[4], math::min(byte_count, length)); @@ -131,7 +135,7 @@ int SMBus::block_read(const uint8_t cmd_code, void *data, const uint8_t length, uint8_t pec = get_pec(rx_data, byte_count + 4); if (pec != rx_data[byte_count + 4]) { - result = -EINVAL; + result = -EIO; perf_count(_interface_errors); } } @@ -141,8 +145,12 @@ int SMBus::block_read(const uint8_t cmd_code, void *data, const uint8_t length, int SMBus::block_write(const uint8_t cmd_code, const void *data, uint8_t byte_count, const bool use_pec) { - // cmd code[1], byte count[1], data[byte_count] (32max), pec[1] (optional) - uint8_t buf[32 + 2]; + // cmd code[1], byte count[1], data[byte_count] (MAX_BLOCK_LEN max), pec[1] (optional) + uint8_t buf[MAX_BLOCK_LEN + 2]; + + if (byte_count > MAX_BLOCK_LEN) { + return -EINVAL; + } buf[0] = cmd_code; buf[1] = (uint8_t)byte_count; diff --git a/src/lib/drivers/smbus/SMBus.hpp b/src/lib/drivers/smbus/SMBus.hpp index dbd44dfeb0..949103eadf 100644 --- a/src/lib/drivers/smbus/SMBus.hpp +++ b/src/lib/drivers/smbus/SMBus.hpp @@ -48,6 +48,8 @@ class SMBus : public device::I2C { public: + static constexpr uint8_t MAX_BLOCK_LEN = 34; + SMBus(int bus_num, uint16_t address); ~SMBus() override; @@ -55,7 +57,7 @@ public: * @brief Sends a block write command. * @param cmd_code The command code. * @param data The data to be written. - * @param length The number of bytes being written. + * @param length The number of bytes being written. Maximum is SMBus::MAX_BLOCK_LEN. * @return Returns PX4_OK on success, -errno on failure. */ int block_write(const uint8_t cmd_code, const void *data, uint8_t byte_count, const bool use_pec); @@ -63,8 +65,8 @@ public: /** * @brief Sends a block read command. * @param cmd_code The command code. - * @param data The returned data. The returned data will always contain 2 bytes of address information followed by data[length]. - * @param length The number of bytes being read. + * @param data The returned data. + * @param length The number of bytes being read. Maximum is SMBus::MAX_BLOCK_LEN. * @return Returns PX4_OK on success, -errno on failure. */ int block_read(const uint8_t cmd_code, void *data, const uint8_t length, const bool use_pec); diff --git a/src/lib/ecl b/src/lib/ecl index ffab483504..81937370ac 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit ffab483504a6c2eb3b61ae10d612cb35c8fbc550 +Subproject commit 81937370ac5234a4c318d838acee4d9bb4bfa697 diff --git a/src/lib/parameters/CMakeLists.txt b/src/lib/parameters/CMakeLists.txt index 3964ade448..4978af39d1 100644 --- a/src/lib/parameters/CMakeLists.txt +++ b/src/lib/parameters/CMakeLists.txt @@ -150,11 +150,7 @@ if (NOT "${PX4_BOARD}" MATCHES "px4_io") px4_parameters.hpp ) - if ("${CONFIG_SHMEM}" STREQUAL "1") - target_link_libraries(parameters PRIVATE px4_layer) - endif() - - target_link_libraries(parameters PRIVATE perf tinybson) + target_link_libraries(parameters PRIVATE perf tinybson px4_layer) target_compile_definitions(parameters PRIVATE -DMODULE_NAME="parameters") target_compile_options(parameters PRIVATE diff --git a/src/lib/parameters/flashparams/flashparams.cpp b/src/lib/parameters/flashparams/flashparams.cpp index 8769bf0c8f..3df2f71a3b 100644 --- a/src/lib/parameters/flashparams/flashparams.cpp +++ b/src/lib/parameters/flashparams/flashparams.cpp @@ -53,7 +53,7 @@ #include -#include "systemlib/uthash/utarray.h" +#include "../uthash/utarray.h" #include #include "flashparams.h" #include "flashfs.h" diff --git a/src/lib/parameters/flashparams/flashparams.h b/src/lib/parameters/flashparams/flashparams.h index f2c35af24e..9f646a21ec 100644 --- a/src/lib/parameters/flashparams/flashparams.h +++ b/src/lib/parameters/flashparams/flashparams.h @@ -49,7 +49,6 @@ #include #include #include -#include "systemlib/uthash/utarray.h" __BEGIN_DECLS diff --git a/src/lib/parameters/parameters.cpp b/src/lib/parameters/parameters.cpp index e393bc54ea..b016b759cc 100644 --- a/src/lib/parameters/parameters.cpp +++ b/src/lib/parameters/parameters.cpp @@ -60,7 +60,7 @@ #include #include #include -#include +#include "uthash/utarray.h" using namespace time_literals; diff --git a/src/lib/parameters/parameters_shmem.cpp b/src/lib/parameters/parameters_shmem.cpp index a50eb52244..c7985e2c8f 100644 --- a/src/lib/parameters/parameters_shmem.cpp +++ b/src/lib/parameters/parameters_shmem.cpp @@ -57,7 +57,7 @@ #include #include -#include +#include "uthash/utarray.h" #include "uORB/uORB.h" #include "uORB/topics/parameter_update.h" diff --git a/src/lib/systemlib/uthash/doc/utarray.txt b/src/lib/parameters/uthash/doc/utarray.txt similarity index 100% rename from src/lib/systemlib/uthash/doc/utarray.txt rename to src/lib/parameters/uthash/doc/utarray.txt diff --git a/src/lib/systemlib/uthash/utarray.h b/src/lib/parameters/uthash/utarray.h similarity index 100% rename from src/lib/systemlib/uthash/utarray.h rename to src/lib/parameters/uthash/utarray.h diff --git a/src/lib/rc/CMakeLists.txt b/src/lib/rc/CMakeLists.txt index d4162502b2..bcba0a41a0 100644 --- a/src/lib/rc/CMakeLists.txt +++ b/src/lib/rc/CMakeLists.txt @@ -33,6 +33,7 @@ add_library(rc crsf.cpp + ghst.cpp st24.cpp sumd.cpp sbus.cpp diff --git a/src/lib/rc/common_rc.cpp b/src/lib/rc/common_rc.cpp index 2a271c8d0e..6cda37b9c3 100644 --- a/src/lib/rc/common_rc.cpp +++ b/src/lib/rc/common_rc.cpp @@ -2,3 +2,30 @@ #include "common_rc.h" __EXPORT rc_decode_buf_t rc_decode_buf; + +uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a) +{ + crc ^= a; + + for (int i = 0; i < 8; ++i) { + if (crc & 0x80) { + crc = (crc << 1) ^ 0xD5; + + } else { + crc = crc << 1; + } + } + + return crc; +} + +uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len) +{ + uint8_t crc = 0; + + for (int i = 0; i < len; ++i) { + crc = crc8_dvb_s2(crc, buf[i]); + } + + return crc; +} diff --git a/src/lib/rc/common_rc.h b/src/lib/rc/common_rc.h index 85e55b6eab..54b7f0046e 100644 --- a/src/lib/rc/common_rc.h +++ b/src/lib/rc/common_rc.h @@ -4,6 +4,7 @@ #include #include "crsf.h" +#include "ghst.h" #include "dsm.h" #include "sbus.h" #include "st24.h" @@ -13,6 +14,7 @@ typedef struct rc_decode_buf_ { union { crsf_frame_t crsf_frame; + ghst_frame_t ghst_frame; dsm_decode_t dsm; sbus_frame_t sbus_frame; ReceiverFcPacket _strxpacket; @@ -22,3 +24,6 @@ typedef struct rc_decode_buf_ { #pragma pack(pop) extern rc_decode_buf_t rc_decode_buf; + +uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a); +uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len); diff --git a/src/lib/rc/crsf.cpp b/src/lib/rc/crsf.cpp index 85831421ba..383a0fbe71 100644 --- a/src/lib/rc/crsf.cpp +++ b/src/lib/rc/crsf.cpp @@ -138,8 +138,6 @@ static crsf_parser_state_t parser_state = crsf_parser_state_t::unsynced; */ static bool crsf_parse_buffer(uint16_t *values, uint16_t *num_values, uint16_t max_channels); -static uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a); -static uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len); uint8_t crsf_frame_CRC(const crsf_frame_t &frame); @@ -202,33 +200,6 @@ bool crsf_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t return ret; } -static uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a) -{ - crc ^= a; - - for (int i = 0; i < 8; ++i) { - if (crc & 0x80) { - crc = (crc << 1) ^ 0xD5; - - } else { - crc = crc << 1; - } - } - - return crc; -} - -static uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len) -{ - uint8_t crc = 0; - - for (int i = 0; i < len; ++i) { - crc = crc8_dvb_s2(crc, buf[i]); - } - - return crc; -} - uint8_t crsf_frame_CRC(const crsf_frame_t &frame) { // CRC includes type and payload diff --git a/src/lib/rc/ghst.cpp b/src/lib/rc/ghst.cpp new file mode 100644 index 0000000000..49a8b50570 --- /dev/null +++ b/src/lib/rc/ghst.cpp @@ -0,0 +1,383 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ghst.cpp + * + * RC protocol definition for IRC Ghost (Immersion RC Ghost). + * + * @author Igor Misic + * @author Juraj Ciberlin + */ + +#if 0 // enable non-verbose debugging +#define GHST_DEBUG PX4_WARN +#else +#define GHST_DEBUG(...) +#endif + +#if 0 // verbose debugging. Careful when enabling: it leads to too much output, causing dropped bytes +#define GHST_VERBOSE PX4_WARN +#else +#define GHST_VERBOSE(...) +#endif + +#include +#include +#include +#include + +// TODO: include RSSI dBm to percentage conversion for ghost receiver +#include "spektrum_rssi.h" + +#include "ghst.h" +#include "common_rc.h" + +#define MIN(a,b) (((a)<(b))?(a):(b)) +#define MAX(a,b) (((a)>(b))?(a):(b)) + +#define GHST_FRAME_PAYLOAD_SIZE_TELEMETRY (10u) +#define GHST_FRAME_CRC_SIZE (1) +#define GHST_FRAME_TYPE_SIZE (1) +#define GHST_TYPE_DATA_CRC_SIZE (12u) +#define GHST_MAX_NUM_CHANNELS (16) + +enum class ghst_parser_state_t : uint8_t { + unsynced = 0, + synced +}; + +// only RSSI frame contains value of RSSI, if it is not received, send last received RSSI +static int8_t ghst_rssi = -1; + +static ghst_frame_t &ghst_frame = rc_decode_buf.ghst_frame; +static uint32_t current_frame_position = 0; +static ghst_parser_state_t parser_state = ghst_parser_state_t::unsynced; + +static uint16_t prev_rc_vals[GHST_MAX_NUM_CHANNELS]; + +/** + * parse the current ghst_frame buffer + */ +static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_values, uint16_t max_channels); + +int ghst_config(int uart_fd) +{ + struct termios t; + int ret_val; + + // no parity, one stop bit + tcgetattr(uart_fd, &t); + cfsetspeed(&t, GHST_BAUDRATE); + t.c_cflag &= ~(CSTOPB | PARENB); + memset(prev_rc_vals, (int)UINT16_MAX, sizeof(uint16_t) * GHST_MAX_NUM_CHANNELS); + ret_val = tcsetattr(uart_fd, TCSANOW, &t); + return ret_val; +} + +/** + * Convert from RC to PWM value + * @param chan_value channel value in [172, 1811] + * @return PWM channel value in [1000, 2000] + */ +static uint16_t convert_channel_value(unsigned chan_value); + + +bool ghst_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values, + int8_t *rssi, uint16_t *num_values, uint16_t max_channels) +{ + bool success = false; + uint8_t *ghst_frame_ptr = (uint8_t *)&ghst_frame; + + memcpy(values, prev_rc_vals, sizeof(uint16_t) * GHST_MAX_NUM_CHANNELS); + + while (len > 0) { + + // fill in the ghst_buffer, as much as we can + const unsigned current_len = MIN(len, sizeof(ghst_frame_t) - current_frame_position); + memcpy(ghst_frame_ptr + current_frame_position, frame, current_len); + current_frame_position += current_len; + + // protection to guarantee parsing progress + if (current_len == 0) { + GHST_DEBUG("========== parser bug: no progress (%i) ===========", len); + + for (unsigned i = 0; i < current_frame_position; ++i) { + GHST_DEBUG("ghst_frame_ptr[%i]: 0x%x", i, (int)ghst_frame_ptr[i]); + } + + // reset the parser + current_frame_position = 0; + parser_state = ghst_parser_state_t::unsynced; + return false; + } + + len -= current_len; + frame += current_len; + + if (ghst_parse_buffer(values, rssi, num_values, max_channels)) { + success = true; + } + } + + + return success; +} + +uint8_t ghst_frame_CRC(const ghst_frame_t &frame) +{ + uint8_t crc = crc8_dvb_s2(0, frame.type); + + for (int i = 0; i < frame.header.length - GHST_FRAME_CRC_SIZE - GHST_FRAME_TYPE_SIZE; ++i) { + crc = crc8_dvb_s2(crc, frame.payload[i]); + } + + return crc; +} + +static uint16_t convert_channel_value(unsigned chan_value) +{ + /* + * RC PWM + * min 172 -> 988us + * mid 992 -> 1500us + * max 1811 -> 2012us + */ + static constexpr float scale = (2012.f - 988.f) / (1811.f - 172.f); + static constexpr float offset = 988.f - 172.f * scale; + return (scale * chan_value) + offset; +} + +static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_values, uint16_t max_channels) +{ + uint8_t *ghst_frame_ptr = (uint8_t *)&ghst_frame; + + if (parser_state == ghst_parser_state_t::unsynced) { + // there is no sync yet, try to find an RC packet by searching for a matching frame length and type + for (unsigned i = 1; i < current_frame_position - 1; ++i) { + if ((ghst_frame_ptr[i + 1] >= (uint8_t)ghstFrameType::frameTypeFirst) && + (ghst_frame_ptr[i + 1] <= (uint8_t)ghstFrameType::frameTypeLast)) { + if (ghst_frame_ptr[i] == GHST_TYPE_DATA_CRC_SIZE) { + parser_state = ghst_parser_state_t::synced; + unsigned frame_offset = i - 1; + GHST_VERBOSE("RC channels found at offset %i", frame_offset); + + // move the rest of the buffer to the beginning + if (frame_offset != 0) { + memmove(ghst_frame_ptr, ghst_frame_ptr + frame_offset, current_frame_position - frame_offset); + current_frame_position -= frame_offset; + } + + break; + } + } + } + } + + if (parser_state != ghst_parser_state_t::synced) { + if (current_frame_position >= sizeof(ghst_frame_t)) { + // discard most of the data, but keep the last 3 bytes (otherwise we could miss the frame start) + current_frame_position = 3; + + memcpy(ghst_frame_ptr, ghst_frame_ptr + sizeof(ghst_frame_t) - current_frame_position, current_frame_position); + + GHST_VERBOSE("Discarding buffer"); + } + + return false; + } + + + if (current_frame_position < 3) { + // wait until we have the address, length and type + return false; + } + + // now we have at least the header and the type + + const unsigned current_frame_length = ghst_frame.header.length + sizeof(ghst_frame_header_t); + + if (current_frame_length > sizeof(ghst_frame_t) || current_frame_length < 4) { + // frame too long or bogus (frame length should be longer than 4, at least 1 address, 1 length, 1 type, 1 data, 1 crc) + // discard everything and go into unsynced state + current_frame_position = 0; + parser_state = ghst_parser_state_t::unsynced; + GHST_DEBUG("Frame too long/bogus (%i, type=%i) -> unsync", current_frame_length, ghst_frame.type); + return false; + } + + if (current_frame_position < current_frame_length) { + // we do not have the full frame yet -> wait for more data + GHST_VERBOSE("waiting for more data (%i < %i)", current_frame_position, current_frame_length); + return false; + } + + bool ret = false; + + // now we have the full frame + + if ((ghst_frame.type >= (uint8_t)ghstFrameType::frameTypeFirst) && + (ghst_frame.type <= (uint8_t)ghstFrameType::frameTypeLast) && + (ghst_frame.header.length == GHST_TYPE_DATA_CRC_SIZE)) { + const uint8_t crc = ghst_frame.payload[ghst_frame.header.length - 2]; + + if (crc == ghst_frame_CRC(ghst_frame)) { + const ghstPayloadData_t *const rcChannels = (ghstPayloadData_t *)&ghst_frame.payload; + *num_values = MIN(max_channels, GHST_MAX_NUM_CHANNELS); + + // all frames contain data from chan1to4 + if (max_channels > 0) { values[0] = convert_channel_value(rcChannels->chan1to4.chan1 >> 1); } + + if (max_channels > 1) { values[1] = convert_channel_value(rcChannels->chan1to4.chan2 >> 1); } + + if (max_channels > 2) { values[2] = convert_channel_value(rcChannels->chan1to4.chan3 >> 1); } + + if (max_channels > 3) { values[3] = convert_channel_value(rcChannels->chan1to4.chan4 >> 1); } + + if (ghst_frame.type == (uint8_t)ghstFrameType::frameType5to8) { + if (max_channels > 4) { values[4] = convert_channel_value(rcChannels->chanA << 3); } + + if (max_channels > 5) { values[5] = convert_channel_value(rcChannels->chanB << 3); } + + if (max_channels > 6) { values[6] = convert_channel_value(rcChannels->chanC << 3); } + + if (max_channels > 7) { values[7] = convert_channel_value(rcChannels->chanD << 3); } + + } else if (ghst_frame.type == (uint8_t)ghstFrameType::frameType9to12) { + if (max_channels > 8) { values[8] = convert_channel_value(rcChannels->chanA << 3); } + + if (max_channels > 9) { values[9] = convert_channel_value(rcChannels->chanB << 3); } + + if (max_channels > 10) { values[10] = convert_channel_value(rcChannels->chanC << 3); } + + if (max_channels > 11) { values[11] = convert_channel_value(rcChannels->chanD << 3); } + + } else if (ghst_frame.type == (uint8_t)ghstFrameType::frameType13to16) { + if (max_channels > 12) { values[12] = convert_channel_value(rcChannels->chanA << 3); } + + if (max_channels > 13) { values[13] = convert_channel_value(rcChannels->chanB << 3); } + + if (max_channels > 14) { values[14] = convert_channel_value(rcChannels->chanC << 3); } + + if (max_channels > 15) { values[15] = convert_channel_value(rcChannels->chanD << 3); } + + } else if (ghst_frame.type == (uint8_t)ghstFrameType::frameTypeRssi) { + const ghstPayloadRssi_t *const rssiValues = (ghstPayloadRssi_t *)&ghst_frame.payload; + // TODO: call function for RSSI dBm to percentage conversion for ghost receiver + ghst_rssi = spek_dbm_to_percent(rssiValues->rssidBm); + } + + else { + GHST_DEBUG("Frame type: %i", ghst_frame.type); + } + + *rssi = ghst_rssi; + + memcpy(prev_rc_vals, values, sizeof(uint16_t) * GHST_MAX_NUM_CHANNELS); + + GHST_VERBOSE("Got Channels"); + + ret = true; + + } else { + GHST_DEBUG("CRC check failed"); + } + + } else { + GHST_DEBUG("Got Non-RC frame (len=%i, type=%i)", current_frame_length, ghst_frame.type); + } + + // either reset or move the rest of the buffer + if (current_frame_position > current_frame_length) { + GHST_VERBOSE("Moving buffer (%i > %i)", current_frame_position, current_frame_length); + memmove(ghst_frame_ptr, ghst_frame_ptr + current_frame_length, current_frame_position - current_frame_length); + current_frame_position -= current_frame_length; + + } else { + current_frame_position = 0; + } + + return ret; +} + +/** + * write an uint8_t value to a buffer at a given offset and increment the offset + */ +static inline void write_uint8_t(uint8_t *buf, int &offset, uint8_t value) +{ + buf[offset++] = value; +} + +/** + * write an uint16_t value to a buffer at a given offset and increment the offset + */ +static inline void write_uint16_t(uint8_t *buf, int &offset, uint16_t value) +{ + buf[offset] = value & 0xff; + buf[offset + 1] = value >> 8; + offset += 2; +} + +/** + * write frame header + */ +static inline void write_frame_header(uint8_t *buf, int &offset, ghstTelemetryType type, uint8_t payload_size) +{ + write_uint8_t(buf, offset, (uint8_t)ghstAddress::rxAddress); + write_uint8_t(buf, offset, payload_size + GHST_FRAME_CRC_SIZE + GHST_FRAME_TYPE_SIZE); + write_uint8_t(buf, offset, (uint8_t)type); +} + +/** + * write frame CRC + */ +static inline void write_frame_crc(uint8_t *buf, int &offset, int buf_size) +{ + write_uint8_t(buf, offset, crc8_dvb_s2_buf(buf + 2, buf_size - 3)); +} + +bool ghst_send_telemetry_battery(int uart_fd, uint16_t voltage, uint16_t current, uint16_t fuel) +{ + uint8_t buf[GHST_FRAME_PAYLOAD_SIZE_TELEMETRY + 4u]; // address, frame length, type, crc + int offset = 0; + write_frame_header(buf, offset, ghstTelemetryType::batteryPack, GHST_FRAME_PAYLOAD_SIZE_TELEMETRY); + write_uint16_t(buf, offset, voltage); + write_uint16_t(buf, offset, current); + write_uint16_t(buf, offset, fuel); + write_uint8_t(buf, offset, 0x00); // empty + write_uint8_t(buf, offset, 0x00); // empty + write_uint8_t(buf, offset, 0x00); // empty + write_uint8_t(buf, offset, 0x00); // empty + write_frame_crc(buf, offset, sizeof(buf)); + return write(uart_fd, buf, offset) == offset; +} diff --git a/src/lib/rc/ghst.h b/src/lib/rc/ghst.h new file mode 100644 index 0000000000..c1a7a0c0a0 --- /dev/null +++ b/src/lib/rc/ghst.h @@ -0,0 +1,141 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file ghst.h + * + * RC protocol definition for IRC Ghost (Immersion RC Ghost). + * + * @author Igor Misic + * @author Juraj Ciberlin + */ + +#pragma once + +#include +#include + +__BEGIN_DECLS + +#define GHST_BAUDRATE (420000u) +#define GHST_PAYLOAD_MAX_SIZE (14u) + +enum class ghstAddress { + rxAddress = 0x89 // Rx address +}; + +enum class ghstFrameType { + frameTypeFirst = 0x10, // first frame type + frameType5to8 = 0x10, // channel 5-8 + frameType9to12 = 0x11, // channel 9-12 + frameType13to16 = 0x12, // channel 13-16 + frameTypeRssi = 0x13, // RSSI frame type, contains LQ, RSSI, RF mode, Tx power + frameTypeLast = 0x1f // last frame type +}; + +enum class ghstTelemetryType { + batteryPack = 0x23 // battery pack status frame type +}; + +struct ghst_frame_header_t { + uint8_t device_address; // device address + uint8_t length; // length +}; + +struct ghst_frame_t { + ghst_frame_header_t header; // header + uint8_t type; // frame type + uint8_t payload[GHST_PAYLOAD_MAX_SIZE + 1]; // payload data including 1 byte CRC at the end +}; + +// Channel data (1-4) +typedef struct { + unsigned int chan1: 12; + unsigned int chan2: 12; + unsigned int chan3: 12; + unsigned int chan4: 12; +} __attribute__((__packed__)) ghstChannelData_t; + +// Payload data +typedef struct { + ghstChannelData_t chan1to4; + unsigned int chanA: 8; + unsigned int chanB: 8; + unsigned int chanC: 8; + unsigned int chanD: 8; +} __attribute__((__packed__)) ghstPayloadData_t; + +// Payload data - RSSI frame type +typedef struct { + ghstChannelData_t chan1to4; + unsigned int lq: 8; // link quality + unsigned int rssidBm: 8; // RSSI [dBm] + unsigned int rfMode: 8; // RF mode + int txPowerdBm: 8; // Tx power [dBm] +} __attribute__((__packed__)) ghstPayloadRssi_t; + +/** + * Configure an UART port to be used for GHST + * @param uart_fd UART file descriptor + * @return 0 on success, -errno otherwise + */ +__EXPORT int ghst_config(int uart_fd); + + +/** + * Parse the GHST protocol and extract RC channel data. + * + * @param now current time + * @param frame data to parse + * @param len length of frame + * @param values output channel values + * @param rssi received signal strength indicator + * @param num_values set to the number of parsed channels in values + * @param max_channels maximum length of values + * @return true if channels successfully decoded + */ +__EXPORT bool ghst_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values, + int8_t *rssi, uint16_t *num_values, uint16_t max_channels); + + +/** + * Send telemetry battery information + * @param uart_fd UART file descriptor + * @param voltage Voltage [0.1V] + * @param current Current [0.1A] + * @param fuel drawn mAh + * @return true on success + */ +__EXPORT bool ghst_send_telemetry_battery(int uart_fd, uint16_t voltage, uint16_t current, uint16_t fuel); + +__END_DECLS diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp index 0efa51b82d..0f4d6c86a5 100644 --- a/src/lib/rc/rc_tests/RCTest.cpp +++ b/src/lib/rc/rc_tests/RCTest.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #if defined(CONFIG_ARCH_BOARD_PX4_SITL) #define TEST_DATA_PATH "./test_data/" @@ -30,6 +31,7 @@ public: private: bool crsfTest(); + bool ghstTest(); bool dsmTest(const char *filepath, unsigned expected_chancount, unsigned expected_dropcount, unsigned chan0); bool dsmTest10Ch(); bool dsmTest16Ch(); @@ -42,6 +44,7 @@ private: bool RCTest::run_tests() { ut_run_test(crsfTest); + ut_run_test(ghstTest); ut_run_test(dsmTest10Ch); ut_run_test(dsmTest16Ch); ut_run_test(dsmTest22msDSMX16Ch); @@ -138,6 +141,94 @@ bool RCTest::crsfTest() return true; } +bool RCTest::ghstTest() +{ + const char *filepath = TEST_DATA_PATH "ghst_rc_channels.txt"; + + FILE *fp = fopen(filepath, "rt"); + + ut_test(fp); + + int uart_fd = -1; + const int line_size = 500; + char line[line_size]; + bool has_decoded_values = false; + const int max_channels = 16; + uint16_t rc_values[max_channels]; + uint16_t num_values = 0; + int line_counter = 1; + int8_t ghst_rssi = -1; + ghst_config(uart_fd); + + while (fgets(line, line_size, fp) != nullptr) { + + if (strncmp(line, "INPUT ", 6) == 0) { + + if (has_decoded_values) { + PX4_ERR("Parser decoded values that are not in the test file (line=%i)", line_counter); + return false; + } + + // read the values + const char *file_buffer = line + 6; + int frame_len = 0; + uint8_t frame[300]; + int offset; + int number; + + while (sscanf(file_buffer, "%x, %n", &number, &offset) > 0) { + frame[frame_len++] = number; + file_buffer += offset; + } + + // Pipe the data into the parser + hrt_abstime now = hrt_absolute_time(); + + bool result = ghst_parse(now, frame, frame_len, rc_values, &ghst_rssi, &num_values, max_channels); + + if (result) { + has_decoded_values = true; + } + + } else if (strncmp(line, "DECODED ", 8) == 0) { + + if (!has_decoded_values) { + PX4_ERR("Test file contains decoded values but the parser did not decode anything (line=%i)", line_counter); + return false; + } + + // read the values + const char *file_buffer = line + 8; + int offset; + int expected_rc_value; + int expected_num_channels = 0; + + while (sscanf(file_buffer, "%x, %n", &expected_rc_value, &offset) > 0) { + + // allow a small difference + if (abs(expected_rc_value - (int)rc_values[expected_num_channels]) > 10) { + PX4_ERR("File line: %i, channel: %i", line_counter, expected_num_channels); + ut_compare("Wrong decoded channel", expected_rc_value, rc_values[expected_num_channels]); + } + + file_buffer += offset; + ++expected_num_channels; + } + + if (expected_num_channels != num_values) { + PX4_ERR("File line: %d", line_counter); + ut_compare("Unexpected number of decoded channels", expected_num_channels, num_values); + } + + has_decoded_values = false; + } + + ++line_counter; + } + + return true; +} + bool RCTest::dsmTest10Ch() { return dsmTest(TEST_DATA_PATH "dsm_x_data.txt", 10, 6, 1500); diff --git a/src/lib/systemlib/crc.c b/src/lib/systemlib/crc.c index 9a66ea139b..18e1586d46 100644 --- a/src/lib/systemlib/crc.c +++ b/src/lib/systemlib/crc.c @@ -132,6 +132,44 @@ uint16_t crc16_signature(uint16_t initial, size_t length, const uint8_t *bytes) return initial ^ CRC16_OUTPUT_XOR; } +/**************************************************************************** + * Name: crc32_signature + * + * Description: + * Calculates a CRC-32 function + * + * Input Parameters: + * acc - The accumulator value to uses as the crc's starting point + * length - The number of bytes to add to the crc + * bytes - A pointer to any array of length bytes + * + * Returned Value: + * The crc32 of the array of bytes + * + ****************************************************************************/ + +uint32_t crc32_signature(uint32_t acc, size_t length, const uint8_t *bytes) +{ + size_t i; + const uint32_t poly = 0xedb88320u; + const uint8_t bits = 8u; + uint8_t w = bits; + + for (i = 0u; i < length; i++) { + acc ^= bytes[i]; + w = bits; + + while (w--) { + const uint32_t xor = -(acc & 1); + acc >>= 1; + acc ^= (poly & xor); + } + } + + return acc; +} + + /**************************************************************************** * Name: crc64_add_word * diff --git a/src/lib/systemlib/crc.h b/src/lib/systemlib/crc.h index 032193f3b2..b0c7ee803b 100644 --- a/src/lib/systemlib/crc.h +++ b/src/lib/systemlib/crc.h @@ -96,6 +96,27 @@ uint16_t crc16_add(uint16_t crc, uint8_t value); uint16_t crc16_signature(uint16_t initial, size_t length, const uint8_t *bytes); +/**************************************************************************** + * Name: crc32_signature + * + * Description: + * Calculates a CRC-32 + * function + * + * Input Parameters: + * acc - The Initial value to uses as the crc's starting point + * length - The number of bytes to add to the crc + * bytes - A pointer to any array of length bytes + * + * Returned Value: + * The crc16 of the array of bytes + * + ****************************************************************************/ + +uint32_t crc32_signature(uint32_t acc, size_t length, + const uint8_t *bytes); + + /**************************************************************************** * Name: crc64_add_word * diff --git a/src/lib/systemlib/uthash/doc/userguide.txt b/src/lib/systemlib/uthash/doc/userguide.txt deleted file mode 100644 index 2d746593f2..0000000000 --- a/src/lib/systemlib/uthash/doc/userguide.txt +++ /dev/null @@ -1,1682 +0,0 @@ -uthash User Guide -================= -Troy D. Hanson -v1.9.6, April 2012 - -include::sflogo.txt[] -include::topnav.txt[] - -A hash in C ------------ -include::toc.txt[] - -This document is written for C programmers. Since you're reading this, chances -are that you know a hash is used for looking up items using a key. In scripting -languages like Perl, hashes are used all the time. In C, hashes don't exist in -the language itself. This software provides a hash table for C structures. - -What can it do? -~~~~~~~~~~~~~~~~~ -This software supports these operations on items in a hash table: - -1. add -2. find -3. delete -4. count -5. iterate -6. sort -7. select - -Is it fast? -~~~~~~~~~~~ -Add, find and delete are normally constant-time operations. This is influenced -by your key domain and the hash function. - -This hash aims to be minimalistic and efficient. It's around 900 lines of C. -It inlines automatically because it's implemented as macros. It's fast as long -as the hash function is suited to your keys. You can use the default hash -function, or easily compare performance and choose from among several other -<>. - -Is it a library? -~~~~~~~~~~~~~~~~ -No, it's just a single header file: `uthash.h`. All you need to do is copy -the header file into your project, and: - - #include "uthash.h" - -Since uthash is a header file only, there is no library code to link against. - -C/C++ and platforms -~~~~~~~~~~~~~~~~~~~ -This software can be used in C and C++ programs. It has been tested on: - - * Linux - * Mac OS X - * Windows using Visual Studio 2008 and 2010 - * Solaris - * OpenBSD - * FreeBSD - -Test suite -^^^^^^^^^^ -To run the test suite, enter the `tests` directory. Then, - - * on Unix platforms, run `make` - * on Windows, run the "do_tests_win32.cmd" batch file. (You may edit the - batch file if your Visual Studio is installed in a non-standard location). - -BSD licensed -~~~~~~~~~~~~ -This software is made available under the -link:license.html[revised BSD license]. -It is free and open source. - -Obtaining uthash -~~~~~~~~~~~~~~~~ -Please follow the link to download on the -http://uthash.sourceforge.net[uthash website] at http://uthash.sourceforge.net. - -A number of platforms include uthash in their package repositories. For example, -Debian/Ubuntu users may simply `aptitude install uthash-dev`. - -Getting help -~~~~~~~~~~~~ -Feel free to mailto:tdh@tkhanson.net[email the author] at -tdh@tkhanson.net. - -Resources -~~~~~~~~~ -Users of uthash may wish to follow the news feed for information about new -releases. Also, there are some extra bonus headers included with uthash. - -News:: - The author has a news feed for http://troydhanson.wordpress.com/[software updates] image:img/rss.png[(RSS)]. -Extras included with uthash:: - uthash ships with these "extras"-- independent headers similar to uthash. - First link:utlist.html[utlist.h] provides linked list macros for C structures. - Second, link:utarray.html[utarray.h] implements dynamic arrays using macros. - Third, link:utstring.html[utstring.h] implements a basic dynamic string. -Other software:: - Other open-source software by the author is listed at http://tkhanson.net. - -Who's using it? -~~~~~~~~~~~~~~~ -Since releasing uthash in 2006, it has been downloaded thousands of times, -incorporated into commercial software, academic research, and into other -open-source software. - -Your structure --------------- - -In uthash, a hash table is comprised of structures. Each structure represents a -key-value association. One or more of the structure fields constitute the key. -The structure pointer itself is the value. - -.Defining a structure that can be hashed ----------------------------------------------------------------------- -#include "uthash.h" - -struct my_struct { - int id; /* key */ - char name[10]; - UT_hash_handle hh; /* makes this structure hashable */ -}; ----------------------------------------------------------------------- - -Note that, in uthash, your structure will never be moved or copied into another -location when you add it into a hash table. This means that you can keep other -data structures that safely point to your structure-- regardless of whether you -add or delete it from a hash table during your program's lifetime. - -The key -~~~~~~~ -There are no restrictions on the data type or name of the key field. The key -can also comprise multiple contiguous fields, having any names and data types. - -.Any data type... really? -***************************************************************************** -Yes, your key and structure can have any data type. Unlike function calls with -fixed prototypes, uthash consists of macros-- whose arguments are untyped-- and -thus able to work with any type of structure or key. -***************************************************************************** - -Unique keys -^^^^^^^^^^^ -As with any hash, every item must have a unique key. Your application must -enforce key uniqueness. Before you add an item to the hash table, you must -first know (if in doubt, check!) that the key is not already in use. You -can check whether a key already exists in the hash table using `HASH_FIND`. - -The hash handle -~~~~~~~~~~~~~~~ -The `UT_hash_handle` field must be present in your structure. It is used for -the internal bookkeeping that makes the hash work. It does not require -initialization. It can be named anything, but you can simplify matters by -naming it `hh`. This allows you to use the easier "convenience" macros to add, -find and delete items. - -A word about memory -~~~~~~~~~~~~~~~~~~~ -Some have asked how uthash cleans up its internal memory. The answer is simple: -'when you delete the final item' from a hash table, uthash releases all the -internal memory associated with that hash table, and sets its pointer to NULL. - -Hash operations ---------------- - -This section introduces the uthash macros by example. For a more succinct -listing, see <>. - -.Convenience vs. general macros: -***************************************************************************** -The uthash macros fall into two categories. The 'convenience' macros can be used -with integer, pointer or string keys (and require that you chose the conventional -name `hh` for the `UT_hash_handle` field). The convenience macros take fewer -arguments than the general macros, making their usage a bit simpler for these -common types of keys. - -The 'general' macros can be used for any types of keys, or for multi-field keys, -or when the `UT_hash_handle` has been named something other than `hh`. These -macros take more arguments and offer greater flexibility in return. But if the -convenience macros suit your needs, use them-- your code will be more readable. -***************************************************************************** - -Declare the hash -~~~~~~~~~~~~~~~~ -Your hash must be declared as a `NULL`-initialized pointer to your structure. - - struct my_struct *users = NULL; /* important! initialize to NULL */ - -Add item -~~~~~~~~ -Allocate and initialize your structure as you see fit. The only aspect -of this that matters to uthash is that your key must be initialized to -a unique value. Then call `HASH_ADD`. (Here we use the convenience macro -`HASH_ADD_INT`, which offers simplified usage for keys of type `int`). - -.Add an item to a hash ----------------------------------------------------------------------- -void add_user(int user_id, char *name) { - struct my_struct *s; - - s = malloc(sizeof(struct my_struct)); - s->id = user_id; - strcpy(s->name, name); - HASH_ADD_INT( users, id, s ); /* id: name of key field */ -} ----------------------------------------------------------------------- - -The first parameter to `HASH_ADD_INT` is the hash table, and the -second parameter is the 'name' of the key field. Here, this is `id`. The -last parameter is a pointer to the structure being added. - -[[validc]] -.Wait.. the field name is a parameter? -******************************************************************************* -If you find it strange that `id`, which is the 'name of a field' in the -structure, can be passed as a parameter, welcome to the world of macros. Don't -worry- the C preprocessor expands this to valid C code. -******************************************************************************* - -Key must not be modified while in-use -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Once a structure has been added to the hash, do not change the value of its key. -Instead, delete the item from the hash, change the key, and then re-add it. - -Passing the hash pointer into functions -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -In the example above `users` is a global variable, but what if the caller wanted -to pass the hash pointer 'into' the `add_user` function? At first glance it would -appear that you could simply pass `users` as an argument, but that won't work -right. - - /* bad */ - void add_user(struct my_struct *users, int user_id, char *name) { - ... - HASH_ADD_INT( users, id, s ); - } - -You really need to pass 'a pointer' to the hash pointer: - - /* good */ - void add_user(struct my_struct **users, int user_id, char *name) { ... - ... - HASH_ADD_INT( *users, id, s ); - } - -Note that we dereferenced the pointer in the `HASH_ADD` also. - -The reason it's necessary to deal with a pointer to the hash pointer is simple: -the hash macros modify it (in other words, they modify the 'pointer itself' not -just what it points to). - -Find item -~~~~~~~~~ -To look up a structure in a hash, you need its key. Then call `HASH_FIND`. -(Here we use the convenience macro `HASH_FIND_INT` for keys of type `int`). - -.Find a structure using its key ----------------------------------------------------------------------- -struct my_struct *find_user(int user_id) { - struct my_struct *s; - - HASH_FIND_INT( users, &user_id, s ); /* s: output pointer */ - return s; -} ----------------------------------------------------------------------- - -Here, the hash table is `users`, and `&user_id` points to the key (an integer -in this case). Last, `s` is the 'output' variable of `HASH_FIND_INT`. The -final result is that `s` points to the structure with the given key, or -is `NULL` if the key wasn't found in the hash. - -[NOTE] -The middle argument is a 'pointer' to the key. You can't pass a literal key -value to `HASH_FIND`. Instead assign the literal value to a variable, and pass -a pointer to the variable. - - -Delete item -~~~~~~~~~~~ -To delete a structure from a hash, you must have a pointer to it. (If you only -have the key, first do a `HASH_FIND` to get the structure pointer). - -.Delete an item from a hash ----------------------------------------------------------------------- -void delete_user(struct my_struct *user) { - HASH_DEL( users, user); /* user: pointer to deletee */ - free(user); /* optional; it's up to you! */ -} ----------------------------------------------------------------------- - -Here again, `users` is the hash table, and `user` is a pointer to the -structure we want to remove from the hash. - -uthash never frees your structure -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Deleting a structure just removes it from the hash table-- it doesn't `free` -it. The choice of when to free your structure is entirely up to you; uthash -will never free your structure. - -Delete can change the pointer -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -The hash table pointer (which initially points to the first item added to the -hash) can change in response to `HASH_DEL` (i.e. if you delete the first item -in the hash table). - -Iterative deletion -^^^^^^^^^^^^^^^^^^ -The `HASH_ITER` macro is a deletion-safe iteration construct which expands -to a simple 'for' loop. - -.Delete all items from a hash ----------------------------------------------------------------------- -void delete_all() { - struct my_struct *current_user, *tmp; - - HASH_ITER(hh, users, current_user, tmp) { - HASH_DEL(users,current_user); /* delete; users advances to next */ - free(current_user); /* optional- if you want to free */ - } -} ----------------------------------------------------------------------- - -All-at-once deletion -^^^^^^^^^^^^^^^^^^^^ -If you only want to delete all the items, but not free them or do any -per-element clean up, you can do this more efficiently in a single operation: - - HASH_CLEAR(hh,users); - -Afterward, the list head (here, `users`) will be set to `NULL`. - -Count items -~~~~~~~~~~~ - -The number of items in the hash table can be obtained using `HASH_COUNT`: - -.Count of items in the hash table ----------------------------------------------------------------------- -unsigned int num_users; -num_users = HASH_COUNT(users); -printf("there are %u users\n", num_users); ----------------------------------------------------------------------- - -Incidentally, this works even the list (`users`, here) is `NULL`, in -which case the count is 0. - -Iterating and sorting -~~~~~~~~~~~~~~~~~~~~~ - -You can loop over the items in the hash by starting from the beginning and -following the `hh.next` pointer. - -.Iterating over all the items in a hash ----------------------------------------------------------------------- -void print_users() { - struct my_struct *s; - - for(s=users; s != NULL; s=s->hh.next) { - printf("user id %d: name %s\n", s->id, s->name); - } -} ----------------------------------------------------------------------- - -There is also an `hh.prev` pointer you could use to iterate backwards through -the hash, starting from any known item. - -[[deletesafe]] -Deletion-safe iteration -^^^^^^^^^^^^^^^^^^^^^^^ -In the example above, it would not be safe to delete and free `s` in the body -of the 'for' loop, (because `s` is derefenced each time the loop iterates). -This is easy to rewrite correctly (by copying the `s->hh.next` pointer to a -temporary variable 'before' freeing `s`), but it comes up often enough that a -deletion-safe iteration macro, `HASH_ITER`, is included. It expands to a -`for`-loop header. Here is how it could be used to rewrite the last example: - - struct my_struct *s, *tmp; - - HASH_ITER(hh, users, s, tmp) { - printf("user id %d: name %s\n", s->id, s->name); - /* ... it is safe to delete and free s here */ - } - -.A hash is also a doubly-linked list. -******************************************************************************* -Iterating backward and forward through the items in the hash is possible -because of the `hh.prev` and `hh.next` fields. All the items in the hash can -be reached by repeatedly following these pointers, thus the hash is also a -doubly-linked list. -******************************************************************************* - -If you're using uthash in a C++ program, you need an extra cast on the `for` -iterator, e.g., `s=(struct my_struct*)s->hh.next`. - -Sorted iteration -^^^^^^^^^^^^^^^^ -The items in the hash are, by default, traversed in the order they were added -("insertion order") when you follow the `hh.next` pointer. But you can sort -the items into a new order using `HASH_SORT`. E.g., - - HASH_SORT( users, name_sort ); - -The second argument is a pointer to a comparison function. It must accept two -arguments which are pointers to two items to compare. Its return value should -be less than zero, zero, or greater than zero, if the first item sorts before, -equal to, or after the second item, respectively. (Just like `strcmp`). - -.Sorting the items in the hash ----------------------------------------------------------------------- -int name_sort(struct my_struct *a, struct my_struct *b) { - return strcmp(a->name,b->name); -} - -int id_sort(struct my_struct *a, struct my_struct *b) { - return (a->id - b->id); -} - -void sort_by_name() { - HASH_SORT(users, name_sort); -} - -void sort_by_id() { - HASH_SORT(users, id_sort); -} ----------------------------------------------------------------------- - -When the items in the hash are sorted, the first item may change position. In -the example above, `users` may point to a different structure after calling -`HASH_SORT`. - -A complete example -~~~~~~~~~~~~~~~~~~ - -We'll repeat all the code and embellish it with a `main()` function to form a -working example. - -If this code was placed in a file called `example.c` in the same directory as -`uthash.h`, it could be compiled and run like this: - - cc -o example example.c - ./example - -Follow the prompts to try the program. - -.A complete program ----------------------------------------------------------------------- -#include /* gets */ -#include /* atoi, malloc */ -#include /* strcpy */ -#include "uthash.h" - -struct my_struct { - int id; /* key */ - char name[10]; - UT_hash_handle hh; /* makes this structure hashable */ -}; - -struct my_struct *users = NULL; - -void add_user(int user_id, char *name) { - struct my_struct *s; - - s = (struct my_struct*)malloc(sizeof(struct my_struct)); - s->id = user_id; - strcpy(s->name, name); - HASH_ADD_INT( users, id, s ); /* id: name of key field */ -} - -struct my_struct *find_user(int user_id) { - struct my_struct *s; - - HASH_FIND_INT( users, &user_id, s ); /* s: output pointer */ - return s; -} - -void delete_user(struct my_struct *user) { - HASH_DEL( users, user); /* user: pointer to deletee */ - free(user); -} - -void delete_all() { - struct my_struct *current_user, *tmp; - - HASH_ITER(hh, users, current_user, tmp) { - HASH_DEL(users,current_user); /* delete it (users advances to next) */ - free(current_user); /* free it */ - } -} - -void print_users() { - struct my_struct *s; - - for(s=users; s != NULL; s=(struct my_struct*)(s->hh.next)) { - printf("user id %d: name %s\n", s->id, s->name); - } -} - -int name_sort(struct my_struct *a, struct my_struct *b) { - return strcmp(a->name,b->name); -} - -int id_sort(struct my_struct *a, struct my_struct *b) { - return (a->id - b->id); -} - -void sort_by_name() { - HASH_SORT(users, name_sort); -} - -void sort_by_id() { - HASH_SORT(users, id_sort); -} - -int main(int argc, char *argv[]) { - char in[10]; - int id=1, running=1; - struct my_struct *s; - unsigned num_users; - - while (running) { - printf("1. add user\n"); - printf("2. find user\n"); - printf("3. delete user\n"); - printf("4. delete all users\n"); - printf("5. sort items by name\n"); - printf("6. sort items by id\n"); - printf("7. print users\n"); - printf("8. count users\n"); - printf("9. quit\n"); - gets(in); - switch(atoi(in)) { - case 1: - printf("name?\n"); - add_user(id++, gets(in)); - break; - case 2: - printf("id?\n"); - s = find_user(atoi(gets(in))); - printf("user: %s\n", s ? s->name : "unknown"); - break; - case 3: - printf("id?\n"); - s = find_user(atoi(gets(in))); - if (s) delete_user(s); - else printf("id unknown\n"); - break; - case 4: - delete_all(); - break; - case 5: - sort_by_name(); - break; - case 6: - sort_by_id(); - break; - case 7: - print_users(); - break; - case 8: - num_users=HASH_COUNT(users); - printf("there are %u users\n", num_users); - break; - case 9: - running=0; - break; - } - } - - delete_all(); /* free any structures */ - return 0; -} ----------------------------------------------------------------------- - -This program is included in the distribution in `tests/example.c`. You can run -`make example` in that directory to compile it easily. - -Standard key types ------------------- -This section goes into specifics of how to work with different kinds of keys. -You can use nearly any type of key-- integers, strings, pointers, structures, etc. - -[NOTE] -.A note about float -================================================================================ -You can use floating point keys. This comes with the same caveats as with any -program that tests floating point equality. In other words, even the tiniest -difference in two floating point numbers makes them distinct keys. -================================================================================ - -Integer keys -~~~~~~~~~~~~ -The preceding examples demonstrated use of integer keys. To recap, use the -convenience macros `HASH_ADD_INT` and `HASH_FIND_INT` for structures with -integer keys. (The other operations such as `HASH_DELETE` and `HASH_SORT` are -the same for all types of keys). - -String keys -~~~~~~~~~~~ -If your structure has a string key, the operations to use depend on whether your -structure 'points to' the key (`char *`) or the string resides `within` the -structure (`char a[10]`). *This distinction is important*. As we'll see below, -you need to use `HASH_ADD_KEYPTR` when your structure 'points' to a key (that is, -the key itself is 'outside' of the structure); in contrast, use `HASH_ADD_STR` -for a string key that is contained *within* your structure. - -[NOTE] -.char[ ] vs. char* -================================================================================ -The string is 'within' the structure in the first example below-- `name` is a -`char[10]` field. In the second example, the key is 'outside' of the -structure-- `name` is a `char *`. So the first example uses `HASH_ADD_STR` but -the second example uses `HASH_ADD_KEYPTR`. For information on this macro, see -the <>. -================================================================================ - -String 'within' structure -^^^^^^^^^^^^^^^^^^^^^^^^^ - -.A string-keyed hash (string within structure) ----------------------------------------------------------------------- -#include /* strcpy */ -#include /* malloc */ -#include /* printf */ -#include "uthash.h" - -struct my_struct { - char name[10]; /* key (string is WITHIN the structure) */ - int id; - UT_hash_handle hh; /* makes this structure hashable */ -}; - - -int main(int argc, char *argv[]) { - const char **n, *names[] = { "joe", "bob", "betty", NULL }; - struct my_struct *s, *tmp, *users = NULL; - int i=0; - - for (n = names; *n != NULL; n++) { - s = (struct my_struct*)malloc(sizeof(struct my_struct)); - strncpy(s->name, *n,10); - s->id = i++; - HASH_ADD_STR( users, name, s ); - } - - HASH_FIND_STR( users, "betty", s); - if (s) printf("betty's id is %d\n", s->id); - - /* free the hash table contents */ - HASH_ITER(hh, users, s, tmp) { - HASH_DEL(users, s); - free(s); - } - return 0; -} ----------------------------------------------------------------------- - -This example is included in the distribution in `tests/test15.c`. It prints: - - betty's id is 2 - -String 'pointer' in structure -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -Now, here is the same example but using a `char *` key instead of `char [ ]`: - -.A string-keyed hash (structure points to string) ----------------------------------------------------------------------- -#include /* strcpy */ -#include /* malloc */ -#include /* printf */ -#include "uthash.h" - -struct my_struct { - const char *name; /* key */ - int id; - UT_hash_handle hh; /* makes this structure hashable */ -}; - - -int main(int argc, char *argv[]) { - const char **n, *names[] = { "joe", "bob", "betty", NULL }; - struct my_struct *s, *tmp, *users = NULL; - int i=0; - - for (n = names; *n != NULL; n++) { - s = (struct my_struct*)malloc(sizeof(struct my_struct)); - s->name = *n; - s->id = i++; - HASH_ADD_KEYPTR( hh, users, s->name, strlen(s->name), s ); - } - - HASH_FIND_STR( users, "betty", s); - if (s) printf("betty's id is %d\n", s->id); - - /* free the hash table contents */ - HASH_ITER(hh, users, s, tmp) { - HASH_DEL(users, s); - free(s); - } - return 0; -} ----------------------------------------------------------------------- - -This example is included in `tests/test40.c`. - -Pointer keys -~~~~~~~~~~~~ -Your key can be a pointer. To be very clear, this means the 'pointer itself' -can be the key (in contrast, if the thing 'pointed to' is the key, this is a -different use case handled by `HASH_ADD_KEYPTR`). - -Here is a simple example where a structure has a pointer member, called `key`. - -.A pointer key ----------------------------------------------------------------------- -#include -#include -#include "uthash.h" - -typedef struct { - void *key; - int i; - UT_hash_handle hh; -} el_t; - -el_t *hash = NULL; -char *someaddr = NULL; - -int main() { - el_t *d; - el_t *e = (el_t*)malloc(sizeof(el_t)); - if (!e) return -1; - e->key = (void*)someaddr; - e->i = 1; - HASH_ADD_PTR(hash,key,e); - HASH_FIND_PTR(hash, &someaddr, d); - if (d) printf("found\n"); - - /* release memory */ - HASH_DEL(hash,e); - free(e); - return 0; -} ----------------------------------------------------------------------- - -This example is included in `tests/test57.c`. Note that the end of the program -deletes the element out of the hash, (and since no more elements remain in the -hash), uthash releases its internal memory. - -Structure keys -~~~~~~~~~~~~~~ -Your key field can have any data type. To uthash, it is just a sequence of -bytes. Therefore, even a nested structure can be used as a key. We'll use the -general macros `HASH_ADD` and `HASH_FIND` to demonstrate. - -NOTE: Structures contain padding (wasted internal space used to fulfill -alignment requirements for the members of the structure). These padding bytes -'must be zeroed' before adding an item to the hash or looking up an item. -Therefore always zero the whole structure before setting the members of -interest. The example below does this-- see the two calls to `memset`. - -.A key which is a structure ----------------------------------------------------------------------- -#include -#include -#include "uthash.h" - -typedef struct { - char a; - int b; -} record_key_t; - -typedef struct { - record_key_t key; - /* ... other data ... */ - UT_hash_handle hh; -} record_t; - -int main(int argc, char *argv[]) { - record_t l, *p, *r, *tmp, *records = NULL; - - r = (record_t*)malloc( sizeof(record_t) ); - memset(r, 0, sizeof(record_t)); - r->key.a = 'a'; - r->key.b = 1; - HASH_ADD(hh, records, key, sizeof(record_key_t), r); - - memset(&l, 0, sizeof(record_t)); - l.key.a = 'a'; - l.key.b = 1; - HASH_FIND(hh, records, &l.key, sizeof(record_key_t), p); - - if (p) printf("found %c %d\n", p->key.a, p->key.b); - - HASH_ITER(hh, records, p, tmp) { - HASH_DEL(records, p); - free(p); - } - return 0; -} - ----------------------------------------------------------------------- - -This usage is nearly the same as use of a compound key explained below. - -Note that the general macros require the name of the `UT_hash_handle` to be -passed as the first argument (here, this is `hh`). The general macros are -documented in <>. - -Advanced Topics ---------------- - -Compound keys -~~~~~~~~~~~~~ -Your key can even comprise multiple contiguous fields. - -.A multi-field key ----------------------------------------------------------------------- -#include /* malloc */ -#include /* offsetof */ -#include /* printf */ -#include /* memset */ -#include "uthash.h" - -#define UTF32 1 - -typedef struct { - UT_hash_handle hh; - int len; - char encoding; /* these two fields */ - int text[]; /* comprise the key */ -} msg_t; - -typedef struct { - char encoding; - int text[]; -} lookup_key_t; - -int main(int argc, char *argv[]) { - unsigned keylen; - msg_t *msg, *tmp, *msgs = NULL; - lookup_key_t *lookup_key; - - int beijing[] = {0x5317, 0x4eac}; /* UTF-32LE for 北京 */ - - /* allocate and initialize our structure */ - msg = (msg_t*)malloc( sizeof(msg_t) + sizeof(beijing) ); - memset(msg, 0, sizeof(msg_t)+sizeof(beijing)); /* zero fill */ - msg->len = sizeof(beijing); - msg->encoding = UTF32; - memcpy(msg->text, beijing, sizeof(beijing)); - - /* calculate the key length including padding, using formula */ - keylen = offsetof(msg_t, text) /* offset of last key field */ - + sizeof(beijing) /* size of last key field */ - - offsetof(msg_t, encoding); /* offset of first key field */ - - /* add our structure to the hash table */ - HASH_ADD( hh, msgs, encoding, keylen, msg); - - /* look it up to prove that it worked :-) */ - msg=NULL; - - lookup_key = (lookup_key_t*)malloc(sizeof(*lookup_key) + sizeof(beijing)); - memset(lookup_key, 0, sizeof(*lookup_key) + sizeof(beijing)); - lookup_key->encoding = UTF32; - memcpy(lookup_key->text, beijing, sizeof(beijing)); - HASH_FIND( hh, msgs, &lookup_key->encoding, keylen, msg ); - if (msg) printf("found \n"); - free(lookup_key); - - HASH_ITER(hh, msgs, msg, tmp) { - HASH_DEL(msgs, msg); - free(msg); - } - return 0; -} ----------------------------------------------------------------------- - -This example is included in the distribution in `tests/test22.c`. - -If you use multi-field keys, recognize that the compiler pads adjacent fields -(by inserting unused space between them) in order to fulfill the alignment -requirement of each field. For example a structure containing a `char` followed -by an `int` will normally have 3 "wasted" bytes of padding after the char, in -order to make the `int` field start on a multiple-of-4 address (4 is the length -of the int). - -.Calculating the length of a multi-field key: -******************************************************************************* -To determine the key length when using a multi-field key, you must include any -intervening structure padding the compiler adds for alignment purposes. - -An easy way to calculate the key length is to use the `offsetof` macro from -``. The formula is: - - key length = offsetof(last_key_field) - + sizeof(last_key_field) - - offsetof(first_key_field) - -In the example above, the `keylen` variable is set using this formula. -******************************************************************************* - -When dealing with a multi-field key, you must zero-fill your structure before -`HASH_ADD`'ing it to a hash table, or using its fields in a `HASH_FIND` key. - -In the previous example, `memset` is used to initialize the structure by -zero-filling it. This zeroes out any padding between the key fields. If we -didn't zero-fill the structure, this padding would contain random values. The -random values would lead to `HASH_FIND` failures; as two "identical" keys will -appear to mismatch if there are any differences within their padding. - -[[multilevel]] -Multi-level hash tables -~~~~~~~~~~~~~~~~~~~~~~~ -A multi-level hash table arises when each element of a hash table contains its -own secondary hash table. There can be any number of levels. In a scripting -language you might see: - - $items{bob}{age}=37 - -The C program below builds this example in uthash: the hash table is called -`items`. It contains one element (`bob`) whose own hash table contains one -element (`age`) with value 37. No special functions are necessary to build -a multi-level hash table. - -While this example represents both levels (`bob` and `age`) using the same -structure, it would also be fine to use two different structure definitions. -It would also be fine if there were three or more levels instead of two. - -.Multi-level hash table ----------------------------------------------------------------------- -#include -#include -#include -#include "uthash.h" - -/* hash of hashes */ -typedef struct item { - char name[10]; - struct item *sub; - int val; - UT_hash_handle hh; -} item_t; - -item_t *items=NULL; - -int main(int argc, char *argvp[]) { - item_t *item1, *item2, *tmp1, *tmp2; - - /* make initial element */ - item_t *i = malloc(sizeof(*i)); - strcpy(i->name, "bob"); - i->sub = NULL; - i->val = 0; - HASH_ADD_STR(items, name, i); - - /* add a sub hash table off this element */ - item_t *s = malloc(sizeof(*s)); - strcpy(s->name, "age"); - s->sub = NULL; - s->val = 37; - HASH_ADD_STR(i->sub, name, s); - - /* iterate over hash elements */ - HASH_ITER(hh, items, item1, tmp1) { - HASH_ITER(hh, item1->sub, item2, tmp2) { - printf("$items{%s}{%s} = %d\n", item1->name, item2->name, item2->val); - } - } - - /* clean up both hash tables */ - HASH_ITER(hh, items, item1, tmp1) { - HASH_ITER(hh, item1->sub, item2, tmp2) { - HASH_DEL(item1->sub, item2); - free(item2); - } - HASH_DEL(items, item1); - free(item1); - } - - return 0; -} ----------------------------------------------------------------------- -The example above is included in `tests/test59.c`. - -[[multihash]] -Items in several hash tables -~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -A structure can be added to more than one hash table. A few reasons you might do -this include: - -- each hash table may use an alternative key; -- each hash table may have its own sort order; -- or you might simply use multiple hash tables for grouping purposes. E.g., - you could have users in an `admin_users` and a `users` hash table. - -Your structure needs to have a `UT_hash_handle` field for each hash table to -which it might be added. You can name them anything. E.g., - - UT_hash_handle hh1, hh2; - -Items with alternative keys -~~~~~~~~~~~~~~~~~~~~~~~~~~~ -You might create a hash table keyed on an ID field, and another hash table keyed -on username (if usernames are unique). You can add the same user structure to -both hash tables (without duplication of the structure), allowing lookup of a -user structure by their name or ID. The way to achieve this is to have a -separate `UT_hash_handle` for each hash to which the structure may be added. - -.A structure with two alternative keys ----------------------------------------------------------------------- -struct my_struct { - int id; /* usual key */ - char username[10]; /* alternative key */ - UT_hash_handle hh1; /* handle for first hash table */ - UT_hash_handle hh2; /* handle for second hash table */ -}; ----------------------------------------------------------------------- - -In the example above, the structure can now be added to two separate hash -tables. In one hash, `id` is its key, while in the other hash, `username` is -its key. (There is no requirement that the two hashes have different key -fields. They could both use the same key, such as `id`). - -Notice the structure has two hash handles (`hh1` and `hh2`). In the code -below, notice that each hash handle is used exclusively with a particular hash -table. (`hh1` is always used with the `users_by_id` hash, while `hh2` is -always used with the `users_by_name` hash table). - -.Two keys on a structure ----------------------------------------------------------------------- - struct my_struct *users_by_id = NULL, *users_by_name = NULL, *s; - int i; - char *name; - - s = malloc(sizeof(struct my_struct)); - s->id = 1; - strcpy(s->username, "thanson"); - - /* add the structure to both hash tables */ - HASH_ADD(hh1, users_by_id, id, sizeof(int), s); - HASH_ADD(hh2, users_by_name, username, strlen(s->username), s); - - /* lookup user by ID in the "users_by_id" hash table */ - i=1; - HASH_FIND(hh1, users_by_id, &i, sizeof(int), s); - if (s) printf("found id %d: %s\n", i, s->username); - - /* lookup user by username in the "users_by_name" hash table */ - name = "thanson"; - HASH_FIND(hh2, users_by_name, name, strlen(name), s); - if (s) printf("found user %s: %d\n", name, s->id); ----------------------------------------------------------------------- - - -Several sort orders -~~~~~~~~~~~~~~~~~~~ -It comes as no surprise that two hash tables can have different sort orders, but -this fact can also be used advantageously to sort the 'same items' in several -ways. This is based on the ability to store a structure in several hash tables. - -Extending the previous example, suppose we have many users. We have added each -user structure to the `users_by_id` hash table and the `users_by_name` hash table. -(To reiterate, this is done without the need to have two copies of each structure). -Now we can define two sort functions, then use `HASH_SRT`. - - int sort_by_id(struct my_struct *a, struct my_struct *b) { - if (a->id == b->id) return 0; - return (a->id < b->id) ? -1 : 1; - } - - int sort_by_name(struct my_struct *a, struct my_struct *b) { - return strcmp(a->username,b->username); - } - - HASH_SRT(hh1, users_by_id, sort_by_id); - HASH_SRT(hh2, users_by_name, sort_by_name); - -Now iterating over the items in `users_by_id` will traverse them in id-order -while, naturally, iterating over `users_by_name` will traverse them in -name-order. The items are fully forward-and-backward linked in each order. -So even for one set of users, we might store them in two hash tables to provide -easy iteration in two different sort orders. - -Bloom filter (faster misses) -~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -Programs that generate a fair miss rate (`HASH_FIND` that result in `NULL`) may -benefit from the built-in Bloom filter support. This is disabled by default, -because programs that generate only hits would incur a slight penalty from it. -Also, programs that do deletes should not use the Bloom filter. While the -program would operate correctly, deletes diminish the benefit of the filter. -To enable the Bloom filter, simply compile with `-DHASH_BLOOM=n` like: - - -DHASH_BLOOM=27 - -where the number can be any value up to 32 which determines the amount of memory -used by the filter, as shown below. Using more memory makes the filter more -accurate and has the potential to speed up your program by making misses bail -out faster. - -.Bloom filter sizes for selected values of n -[width="50%",cols="10m,30",grid="none",options="header"] -|===================================================================== -| n | Bloom filter size (per hash table) -| 16 | 8 kilobytes -| 20 | 128 kilobytes -| 24 | 2 megabytes -| 28 | 32 megabytes -| 32 | 512 megabytes -|===================================================================== - -Bloom filters are only a performance feature; they do not change the results of -hash operations in any way. The only way to gauge whether or not a Bloom filter -is right for your program is to test it. Reasonable values for the size of the -Bloom filter are 16-32 bits. - -Select -~~~~~~ -An experimental 'select' operation is provided that inserts those items from a -source hash that satisfy a given condition into a destination hash. This -insertion is done with somewhat more efficiency than if this were using -`HASH_ADD`, namely because the hash function is not recalculated for keys of the -selected items. This operation does not remove any items from the source hash. -Rather the selected items obtain dual presence in both hashes. The destination -hash may already have items in it; the selected items are added to it. In order -for a structure to be usable with `HASH_SELECT`, it must have two or more hash -handles. (As described <>, a structure can exist in many -hash tables at the same time; it must have a separate hash handle for each one). - - user_t *users=NULL, *admins=NULL; /* two hash tables */ - - typedef struct { - int id; - UT_hash_handle hh; /* handle for users hash */ - UT_hash_handle ah; /* handle for admins hash */ - } user_t; - -Now suppose we have added some users, and want to select just the administrator -users who have id's less than 1024. - - #define is_admin(x) (((user_t*)x)->id < 1024) - HASH_SELECT(ah,admins,hh,users,is_admin); - -The first two parameters are the 'destination' hash handle and hash table, the -second two parameters are the 'source' hash handle and hash table, and the last -parameter is the 'select condition'. Here we used a macro `is_admin()` but we -could just as well have used a function. - - int is_admin(void *userv) { - user_t *user = (user_t*)userv; - return (user->id < 1024) ? 1 : 0; - } - -If the select condition always evaluates to true, this operation is -essentially a 'merge' of the source hash into the destination hash. Of course, -the source hash remains unchanged under any use of `HASH_SELECT`. It only adds -items to the destination hash selectively. - -The two hash handles must differ. An example of using `HASH_SELECT` is included -in `tests/test36.c`. - - -[[hash_functions]] -Built-in hash functions -~~~~~~~~~~~~~~~~~~~~~~~ -Internally, a hash function transforms a key into a bucket number. You don't -have to take any action to use the default hash function, currently Jenkin's. - -Some programs may benefit from using another of the built-in hash functions. -There is a simple analysis utility included with uthash to help you determine -if another hash function will give you better performance. - -You can use a different hash function by compiling your program with -`-DHASH_FUNCTION=HASH_xyz` where `xyz` is one of the symbolic names listed -below. E.g., - - cc -DHASH_FUNCTION=HASH_BER -o program program.c - -.Built-in hash functions -[width="50%",cols="^5m,20",grid="none",options="header"] -|=============================================================================== -|Symbol | Name -|JEN | Jenkins (default) -|BER | Bernstein -|SAX | Shift-Add-Xor -|OAT | One-at-a-time -|FNV | Fowler/Noll/Vo -|SFH | Paul Hsieh -|MUR | MurmurHash v3 (see note) -|=============================================================================== - -[NOTE] -.MurmurHash -================================================================================ -A special symbol must be defined if you intend to use MurmurHash. To use it, add -`-DHASH_USING_NO_STRICT_ALIASING` to your `CFLAGS`. And, if you are using -the gcc compiler with optimization, add `-fno-strict-aliasing` to your `CFLAGS`. -================================================================================ - -Which hash function is best? -^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -You can easily determine the best hash function for your key domain. To do so, -you'll need to run your program once in a data-collection pass, and then run -the collected data through an included analysis utility. - -First you must build the analysis utility. From the top-level directory, - - cd tests/ - make - -We'll use `test14.c` to demonstrate the data-collection and analysis steps -(here using `sh` syntax to redirect file descriptor 3 to a file): - -.Using keystats --------------------------------------------------------------------------------- -% cc -DHASH_EMIT_KEYS=3 -I../src -o test14 test14.c -% ./test14 3>test14.keys -% ./keystats test14.keys -fcn ideal% #items #buckets dup% fl add_usec find_usec del-all usec ---- ------ ---------- ---------- ----- -- ---------- ---------- ------------ -SFH 91.6% 1219 256 0% ok 92 131 25 -FNV 90.3% 1219 512 0% ok 107 97 31 -SAX 88.7% 1219 512 0% ok 111 109 32 -OAT 87.2% 1219 256 0% ok 99 138 26 -JEN 86.7% 1219 256 0% ok 87 130 27 -BER 86.2% 1219 256 0% ok 121 129 27 --------------------------------------------------------------------------------- - -[NOTE] -The number 3 in `-DHASH_EMIT_KEYS=3` is a file descriptor. Any file descriptor -that your program doesn't use for its own purposes can be used instead of 3. -The data-collection mode enabled by `-DHASH_EMIT_KEYS=x` should not be used in -production code. - -Usually, you should just pick the first hash function that is listed. Here, this -is `SFH`. This is the function that provides the most even distribution for -your keys. If several have the same `ideal%`, then choose the fastest one -according to the `find_usec` column. - -keystats column reference -^^^^^^^^^^^^^^^^^^^^^^^^^ -fcn:: - symbolic name of hash function -ideal%:: - The percentage of items in the hash table which can be looked up within an - ideal number of steps. (Further explained below). -#items:: - the number of keys that were read in from the emitted key file -#buckets:: - the number of buckets in the hash after all the keys were added -dup%:: - the percent of duplicate keys encountered in the emitted key file. - Duplicates keys are filtered out to maintain key uniqueness. (Duplicates - are normal. For example, if the application adds an item to a hash, - deletes it, then re-adds it, the key is written twice to the emitted file.) -flags:: - this is either `ok`, or `nx` (noexpand) if the expansion inhibited flag is - set, described in <>. It is not recommended - to use a hash function that has the `noexpand` flag set. -add_usec:: - the clock time in microseconds required to add all the keys to a hash -find_usec:: - the clock time in microseconds required to look up every key in the hash -del-all usec:: - the clock time in microseconds required to delete every item in the hash - -[[ideal]] -ideal% -^^^^^^ - -.What is ideal%? -***************************************************************************** -The 'n' items in a hash are distributed into 'k' buckets. Ideally each bucket -would contain an equal share '(n/k)' of the items. In other words, the maximum -linear position of any item in a bucket chain would be 'n/k' if every bucket is -equally used. If some buckets are overused and others are underused, the -overused buckets will contain items whose linear position surpasses 'n/k'. -Such items are considered non-ideal. - -As you might guess, `ideal%` is the percentage of ideal items in the hash. These -items have favorable linear positions in their bucket chains. As `ideal%` -approaches 100%, the hash table approaches constant-time lookup performance. -***************************************************************************** - -[[hashscan]] -hashscan -~~~~~~~~ -NOTE: This utility is only available on Linux, and on FreeBSD (8.1 and up). - -A utility called `hashscan` is included in the `tests/` directory. It -is built automatically when you run `make` in that directory. This tool -examines a running process and reports on the uthash tables that it finds in -that program's memory. It can also save the keys from each table in a format -that can be fed into `keystats`. - -Here is an example of using `hashscan`. First ensure that it is built: - - cd tests/ - make - -Since `hashscan` needs a running program to inspect, we'll start up a simple -program that makes a hash table and then sleeps as our test subject: - - ./test_sleep & - pid: 9711 - -Now that we have a test program, let's run `hashscan` on it: - - ./hashscan 9711 - Address ideal items buckets mc fl bloom/sat fcn keys saved to - ------------------ ----- -------- -------- -- -- --------- --- ------------- - 0x862e038 81% 10000 4096 11 ok 16 14% JEN - -If we wanted to copy out all its keys for external analysis using `keystats`, -add the `-k` flag: - - ./hashscan -k 9711 - Address ideal items buckets mc fl bloom/sat fcn keys saved to - ------------------ ----- -------- -------- -- -- --------- --- ------------- - 0x862e038 81% 10000 4096 11 ok 16 14% JEN /tmp/9711-0.key - -Now we could run `./keystats /tmp/9711-0.key` to analyze which hash function -has the best characteristics on this set of keys. - -hashscan column reference -^^^^^^^^^^^^^^^^^^^^^^^^^ -Address:: - virtual address of the hash table -ideal:: - The percentage of items in the table which can be looked up within an ideal - number of steps. See <> in the `keystats` section. -items:: - number of items in the hash table -buckets:: - number of buckets in the hash table -mc:: - the maximum chain length found in the hash table (uthash usually tries to - keep fewer than 10 items in each bucket, or in some cases a multiple of 10) -fl:: - flags (either `ok`, or `NX` if the expansion-inhibited flag is set) -bloom/sat:: - if the hash table uses a Bloom filter, this is the size (as a power of two) - of the filter (e.g. 16 means the filter is 2^16 bits in size). The second - number is the "saturation" of the bits expressed as a percentage. The lower - the percentage, the more potential benefit to identify cache misses quickly. -fcn:: - symbolic name of hash function -keys saved to:: - file to which keys were saved, if any - -.How hashscan works -***************************************************************************** -When hashscan runs, it attaches itself to the target process, which suspends -the target process momentarily. During this brief suspension, it scans the -target's virtual memory for the signature of a uthash hash table. It then -checks if a valid hash table structure accompanies the signature and reports -what it finds. When it detaches, the target process resumes running normally. -The hashscan is performed "read-only"-- the target process is not modified. -Since hashscan is analyzing a momentary snapshot of a running process, it may -return different results from one run to another. -***************************************************************************** - -[[expansion]] -Expansion internals -~~~~~~~~~~~~~~~~~~~ -Internally this hash manages the number of buckets, with the goal of having -enough buckets so that each one contains only a small number of items. - -.Why does the number of buckets matter? -******************************************************************************** -When looking up an item by its key, this hash scans linearly through the items -in the appropriate bucket. In order for the linear scan to run in constant -time, the number of items in each bucket must be bounded. This is accomplished -by increasing the number of buckets as needed. -******************************************************************************** - -Normal expansion -^^^^^^^^^^^^^^^^ -This hash attempts to keep fewer than 10 items in each bucket. When an item is -added that would cause a bucket to exceed this number, the number of buckets in -the hash is doubled and the items are redistributed into the new buckets. In an -ideal world, each bucket will then contain half as many items as it did before. - -Bucket expansion occurs automatically and invisibly as needed. There is -no need for the application to know when it occurs. - -Per-bucket expansion threshold -++++++++++++++++++++++++++++++ -Normally all buckets share the same threshold (10 items) at which point bucket -expansion is triggered. During the process of bucket expansion, uthash can -adjust this expansion-trigger threshold on a per-bucket basis if it sees that -certain buckets are over-utilized. - -When this threshold is adjusted, it goes from 10 to a multiple of 10 (for that -particular bucket). The multiple is based on how many times greater the actual -chain length is than the ideal length. It is a practical measure to reduce -excess bucket expansion in the case where a hash function over-utilizes a few -buckets but has good overall distribution. However, if the overall distribution -gets too bad, uthash changes tactics. - -Inhibited expansion -^^^^^^^^^^^^^^^^^^^ -You usually don't need to know or worry about this, particularly if you used -the `keystats` utility during development to select a good hash for your keys. - -A hash function may yield an uneven distribution of items across the buckets. -In moderation this is not a problem. Normal bucket expansion takes place as -the chain lengths grow. But when significant imbalance occurs (because the hash -function is not well suited to the key domain), bucket expansion may be -ineffective at reducing the chain lengths. - -Imagine a very bad hash function which always puts every item in bucket 0. No -matter how many times the number of buckets is doubled, the chain length of -bucket 0 stays the same. In a situation like this, the best behavior is to -stop expanding, and accept O(n) lookup performance. This is what uthash -does. It degrades gracefully if the hash function is ill-suited to the keys. - -If two consecutive bucket expansions yield `ideal%` values below 50%, uthash -inhibits expansion for that hash table. Once set, the 'bucket expansion -inhibited' flag remains in effect as long as the hash has items in it. -Inhibited expansion may cause `HASH_FIND` to exhibit worse than constant-time -performance. - -Hooks -~~~~~ -You don't need to use these hooks- they are only here if you want to modify -the behavior of uthash. Hooks can be used to change how uthash allocates -memory, and to run code in response to certain internal events. - -malloc/free -^^^^^^^^^^^ -By default this hash implementation uses `malloc` and `free` to manage memory. -If your application uses its own custom allocator, this hash can use them too. - -.Specifying alternate memory management functions ----------------------------------------------------------------------------- -#include "uthash.h" - -/* undefine the defaults */ -#undef uthash_malloc -#undef uthash_free - -/* re-define, specifying alternate functions */ -#define uthash_malloc(sz) my_malloc(sz) -#define uthash_free(ptr,sz) my_free(ptr) - -... ----------------------------------------------------------------------------- - -Notice that `uthash_free` receives two parameters. The `sz` parameter is for -convenience on embedded platforms that manage their own memory. - -Out of memory -^^^^^^^^^^^^^ -If memory allocation fails (i.e., the malloc function returned `NULL`), the -default behavior is to terminate the process by calling `exit(-1)`. This can -be modified by re-defining the `uthash_fatal` macro. - - #undef uthash_fatal - #define uthash_fatal(msg) my_fatal_function(msg); - -The fatal function should terminate the process or `longjmp` back to a safe -place. Uthash does not support "returning a failure" if memory cannot be -allocated. - -Internal events -^^^^^^^^^^^^^^^ -There is no need for the application to set these hooks or take action in -response to these events. They are mainly for diagnostic purposes. - -These two hooks are "notification" hooks which get executed if uthash is -expanding buckets, or setting the 'bucket expansion inhibited' flag. Normally -both of these hooks are undefined and thus compile away to nothing. - -Expansion -+++++++++ -There is a hook for the bucket expansion event. - -.Bucket expansion hook ----------------------------------------------------------------------------- -#include "uthash.h" - -#undef uthash_expand_fyi -#define uthash_expand_fyi(tbl) printf("expanded to %d buckets\n", tbl->num_buckets) - -... ----------------------------------------------------------------------------- - -Expansion-inhibition -++++++++++++++++++++ -This hook can be defined to code to execute in the event that uthash decides to -set the 'bucket expansion inhibited' flag. - -.Bucket expansion inhibited hook ----------------------------------------------------------------------------- -#include "uthash.h" - -#undef uthash_noexpand_fyi -#define uthash_noexpand_fyi printf("warning: bucket expansion inhibited\n"); - -... ----------------------------------------------------------------------------- - - -Debug mode -~~~~~~~~~~ -If a program that uses this hash is compiled with `-DHASH_DEBUG=1`, a special -internal consistency-checking mode is activated. In this mode, the integrity -of the whole hash is checked following every add or delete operation. This is -for debugging the uthash software only, not for use in production code. - -In the `tests/` directory, running `make debug` will run all the tests in -this mode. - -In this mode, any internal errors in the hash data structure will cause a -message to be printed to `stderr` and the program to exit. - -The `UT_hash_handle` data structure includes `next`, `prev`, `hh_next` and -`hh_prev` fields. The former two fields determine the "application" ordering -(that is, insertion order-- the order the items were added). The latter two -fields determine the "bucket chain" order. These link the `UT_hash_handles` -together in a doubly-linked list that is a bucket chain. - -Checks performed in `-DHASH_DEBUG=1` mode: - -- the hash is walked in its entirety twice: once in 'bucket' order and a - second time in 'application' order -- the total number of items encountered in both walks is checked against the - stored number -- during the walk in 'bucket' order, each item's `hh_prev` pointer is compared - for equality with the last visited item -- during the walk in 'application' order, each item's `prev` pointer is compared - for equality with the last visited item - -.Macro debugging: -******************************************************************************** -Sometimes it's difficult to interpret a compiler warning on a line which -contains a macro call. In the case of uthash, one macro can expand to dozens of -lines. In this case, it is helpful to expand the macros and then recompile. -By doing so, the warning message will refer to the exact line within the macro. - -Here is an example of how to expand the macros and then recompile. This uses the -`test1.c` program in the `tests/` subdirectory. - - gcc -E -I../src test1.c > /tmp/a.c - egrep -v '^#' /tmp/a.c > /tmp/b.c - indent /tmp/b.c - gcc -o /tmp/b /tmp/b.c - -The last line compiles the original program (test1.c) with all macros expanded. -If there was a warning, the referenced line number can be checked in `/tmp/b.c`. -******************************************************************************** - -Thread safety -~~~~~~~~~~~~~ -You can use uthash in a threaded program. But you must do the locking. Use a -read-write lock to protect against concurrent writes. It is ok to have -concurrent readers (since uthash 1.5). - -For example using pthreads you can create an rwlock like this: - - pthread_rwlock_t lock; - if (pthread_rwlock_init(&lock,NULL) != 0) fatal("can't create rwlock"); - -Then, readers must acquire the read lock before doing any `HASH_FIND` calls or -before iterating over the hash elements: - - if (pthread_rwlock_rdlock(&lock) != 0) fatal("can't get rdlock"); - HASH_FIND_INT(elts, &i, e); - pthread_rwlock_unlock(&lock); - -Writers must acquire the exclusive write lock before doing any update. Add, -delete, and sort are all updates that must be locked. - - if (pthread_rwlock_wrlock(&lock) != 0) fatal("can't get wrlock"); - HASH_DEL(elts, e); - pthread_rwlock_unlock(&lock); - -If you prefer, you can use a mutex instead of a read-write lock, but this will -reduce reader concurrency to a single thread at a time. - -An example program using uthash with a read-write lock is included in -`tests/threads/test1.c`. - -[[Macro_reference]] -Macro reference ---------------- - -Convenience macros -~~~~~~~~~~~~~~~~~~ -The convenience macros do the same thing as the generalized macros, but -require fewer arguments. - -In order to use the convenience macros, - -1. the structure's `UT_hash_handle` field must be named `hh`, and -2. for add or find, the key field must be of type `int` or `char[]` or pointer - -.Convenience macros -[width="90%",cols="10m,30m",grid="none",options="header"] -|=============================================================================== -|macro | arguments -|HASH_ADD_INT | (head, keyfield_name, item_ptr) -|HASH_FIND_INT | (head, key_ptr, item_ptr) -|HASH_ADD_STR | (head, keyfield_name, item_ptr) -|HASH_FIND_STR | (head, key_ptr, item_ptr) -|HASH_ADD_PTR | (head, keyfield_name, item_ptr) -|HASH_FIND_PTR | (head, key_ptr, item_ptr) -|HASH_DEL | (head, item_ptr) -|HASH_SORT | (head, cmp) -|HASH_COUNT | (head) -|=============================================================================== - -General macros -~~~~~~~~~~~~~~ - -These macros add, find, delete and sort the items in a hash. You need to -use the general macros if your `UT_hash_handle` is named something other -than `hh`, or if your key's data type isn't `int` or `char[]`. - -.General macros -[width="90%",cols="10m,30m",grid="none",options="header"] -|=============================================================================== -|macro | arguments -|HASH_ADD | (hh_name, head, keyfield_name, key_len, item_ptr) -|HASH_ADD_KEYPTR| (hh_name, head, key_ptr, key_len, item_ptr) -|HASH_FIND | (hh_name, head, key_ptr, key_len, item_ptr) -|HASH_DELETE | (hh_name, head, item_ptr) -|HASH_SRT | (hh_name, head, cmp) -|HASH_CNT | (hh_name, head) -|HASH_CLEAR | (hh_name, head) -|HASH_SELECT | (dst_hh_name, dst_head, src_hh_name, src_head, condition) -|HASH_ITER | (hh_name, head, item_ptr, tmp_item_ptr) -|=============================================================================== - -[NOTE] -`HASH_ADD_KEYPTR` is used when the structure contains a pointer to the -key, rather than the key itself. - - -Argument descriptions -^^^^^^^^^^^^^^^^^^^^^ -hh_name:: - name of the `UT_hash_handle` field in the structure. Conventionally called - `hh`. -head:: - the structure pointer variable which acts as the "head" of the hash. So - named because it initially points to the first item that is added to the hash. -keyfield_name:: - the name of the key field in the structure. (In the case of a multi-field - key, this is the first field of the key). If you're new to macros, it - might seem strange to pass the name of a field as a parameter. See - <>. -key_len:: - the length of the key field in bytes. E.g. for an integer key, this is - `sizeof(int)`, while for a string key it's `strlen(key)`. (For a - multi-field key, see the notes in this guide on calculating key length). -key_ptr:: - for `HASH_FIND`, this is a pointer to the key to look up in the hash - (since it's a pointer, you can't directly pass a literal value here). For - `HASH_ADD_KEYPTR`, this is the address of the key of the item being added. -item_ptr:: - pointer to the structure being added, deleted, or looked up, or the current - pointer during iteration. This is an input parameter for `HASH_ADD` and - `HASH_DELETE` macros, and an output parameter for `HASH_FIND` and - `HASH_ITER`. (When using `HASH_ITER` to iterate, `tmp_item_ptr` - is another variable of the same type as `item_ptr`, used internally). -cmp:: - pointer to comparison function which accepts two arguments (pointers to - items to compare) and returns an int specifying whether the first item - should sort before, equal to, or after the second item (like `strcmp`). -condition:: - a function or macro which accepts a single argument-- a void pointer to a - structure, which needs to be cast to the appropriate structure type. The - function or macro should return (or evaluate to) a non-zero value if the - structure should be "selected" for addition to the destination hash. - -// vim: set tw=80 wm=2 syntax=asciidoc: - diff --git a/src/lib/systemlib/uthash/doc/utlist.txt b/src/lib/systemlib/uthash/doc/utlist.txt deleted file mode 100644 index fbf961c27a..0000000000 --- a/src/lib/systemlib/uthash/doc/utlist.txt +++ /dev/null @@ -1,219 +0,0 @@ -utlist: linked list macros for C structures -=========================================== -Troy D. Hanson -v1.9.5, November 2011 - -include::sflogo.txt[] -include::topnav_utlist.txt[] - -Introduction ------------- -include::toc.txt[] - -A set of general-purpose 'linked list' macros for C structures are included with -uthash in `utlist.h`. To use these macros in your own C program, just -copy `utlist.h` into your source directory and use it in your programs. - - #include "utlist.h" - -These macros support the basic linked list operations: adding and deleting -elements, sorting them and iterating over them. - -Download -~~~~~~~~ -To download the `utlist.h` header file, follow the link on the -http://uthash.sourceforge.net[uthash home page]. - -BSD licensed -~~~~~~~~~~~~ -This software is made available under the -link:license.html[revised BSD license]. -It is free and open source. - -Platforms -~~~~~~~~~ -The 'utlist' macros have been tested on: - - * Linux, - * Mac OS X, and - * Windows, using Visual Studio 2008, Visual Studio 2010, or Cygwin/MinGW. - -Using utlist ------------- - -Types of lists -~~~~~~~~~~~~~~ -Three types of linked lists are supported: - -- *singly-linked* lists, -- *doubly-linked* lists, and -- *circular, doubly-linked* lists - -Efficiency -^^^^^^^^^^ -For all types of lists, prepending elements and deleting elements are -constant-time operations. Appending to a singly-linked list is an 'O(n)' -operation but appending to a doubly-linked list is constant time using these -macros. (This is because, in the utlist implementation of the doubly-linked -list, the head element's `prev` member points back to the list tail, even when -the list is non-circular). Sorting is an 'O(n log(n))' operation. Iteration -and searching are `O(n)` for all list types. - -List elements -~~~~~~~~~~~~~ -You can use any structure with these macros, as long as the structure -contains a `next` pointer. If you want to make a doubly-linked list, -the element also needs to have a `prev` pointer. - - typedef struct element { - char *name; - struct element *prev; /* needed for a doubly-linked list only */ - struct element *next; /* needed for singly- or doubly-linked lists */ - } element; - -You can name your structure anything. In the example above it is called `element`. -Within a particular list, all elements must be of the same type. - -List head -~~~~~~~~~ -The list head is simply a pointer to your element structure. You can name it -anything. *It must be initialized to `NULL`*. - - element *head = NULL; - -List operations -~~~~~~~~~~~~~~~ -The lists support inserting or deleting elements, sorting the elements and -iterating over them. - -[width="100%",cols="10 -#include -#include -#include "utlist.h" - -#define BUFLEN 20 - -typedef struct el { - char bname[BUFLEN]; - struct el *next, *prev; -} el; - -int namecmp(el *a, el *b) { - return strcmp(a->bname,b->bname); -} - -el *head = NULL; /* important- initialize to NULL! */ - -int main(int argc, char *argv[]) { - el *name, *elt, *tmp, etmp; - - char linebuf[BUFLEN]; - FILE *file; - - if ( (file = fopen( "test11.dat", "r" )) == NULL ) { - perror("can't open: "); - exit(-1); - } - - while (fgets(linebuf,BUFLEN,file) != NULL) { - if ( (name = (el*)malloc(sizeof(el))) == NULL) exit(-1); - strncpy(name->bname,linebuf,BUFLEN); - DL_APPEND(head, name); - } - DL_SORT(head, namecmp); - DL_FOREACH(head,elt) printf("%s", elt->bname); - - memcpy(&etmp.bname, "WES\n", 5); - DL_SEARCH(head,elt,&etmp,namecmp); - if (elt) printf("found %s\n", elt->bname); - - /* now delete each element, use the safe iterator */ - DL_FOREACH_SAFE(head,elt,tmp) { - DL_DELETE(head,elt); - } - - fclose(file); - - return 0; -} --------------------------------------------------------------------------------- - -// vim: set nowrap syntax=asciidoc: - diff --git a/src/lib/systemlib/uthash/doc/utstring.txt b/src/lib/systemlib/uthash/doc/utstring.txt deleted file mode 100644 index abfdcd1074..0000000000 --- a/src/lib/systemlib/uthash/doc/utstring.txt +++ /dev/null @@ -1,178 +0,0 @@ -utstring: dynamic string macros for C -===================================== -Troy D. Hanson -v1.9.5, November 2011 - -include::sflogo.txt[] -include::topnav_utstring.txt[] - -Introduction ------------- -include::toc.txt[] - -A set of very basic dynamic string macros for C programs are included with -uthash in `utstring.h`. To use these macros in your own C program, just -copy `utstring.h` into your source directory and use it in your programs. - - #include "utstring.h" - -The dynamic string supports basic operations such as inserting data (including -binary data-- despite its name, utstring is not limited to string content), -concatenation, getting the length and content, and clearing it. The string -<> are listed below. - -Download -~~~~~~~~ -To download the `utstring.h` header file, follow the link on the -http://uthash.sourceforge.net[uthash home page]. - -BSD licensed -~~~~~~~~~~~~ -This software is made available under the -link:license.html[revised BSD license]. -It is free and open source. - -Platforms -~~~~~~~~~ -The 'utstring' macros have been tested on: - - * Linux, - * Windows, using Visual Studio 2008 and Visual Studio 2010 - -Usage ------ - -Declaration -~~~~~~~~~~~ - -The dynamic string itself has the data type `UT_string`. It is declared like, - - UT_string *str; - -New and free -~~~~~~~~~~~~ -The next step is to create the string using `utstring_new`. Later when you're -done with it, `utstring_free` will free it and all its content. - -Manipulation -~~~~~~~~~~~~ -The `utstring_printf` or `utstring_bincpy` operations insert (copy) data into -the string. To concatenate one utstring to another, use `utstring_concat`. To -clear the content of the string, use `utstring_clear`. The length of the string -is available from `utstring_len`, and its content from `utstring_body`. This -evaluates to a `char*`. The buffer it points to is always null-terminated. -So, it can be used directly with external functions that expect a string. -This automatic null terminator is not counted in the length of the string. - -Samples -~~~~~~~ - -These examples show how to use utstring. - -.Sample 1 -------------------------------------------------------------------------------- -#include -#include "utstring.h" - -int main() { - UT_string *s; - - utstring_new(s); - utstring_printf(s, "hello world!" ); - printf("%s\n", utstring_body(s)); - - utstring_free(s); - return 0; -} -------------------------------------------------------------------------------- - -The next example is meant to demonstrate that printf 'appends' to the string. -It also shows concatenation. - -.Sample 2 -------------------------------------------------------------------------------- -#include -#include "utstring.h" - -int main() { - UT_string *s, *t; - - utstring_new(s); - utstring_new(t); - - utstring_printf(s, "hello " ); - utstring_printf(s, "world " ); - - utstring_printf(t, "hi " ); - utstring_printf(t, "there " ); - - utstring_concat(s, t); - printf("length: %u\n", utstring_len(s)); - printf("%s\n", utstring_body(s)); - - utstring_free(s); - utstring_free(t); - return 0; -} -------------------------------------------------------------------------------- - -The last example shows how binary data can be inserted into the string. It also -clears the string and prints new data into it. - -.Sample 3 -------------------------------------------------------------------------------- -#include -#include "utstring.h" - -int main() { - UT_string *s; - char binary[] = "\xff\xff"; - - utstring_new(s); - utstring_bincpy(s, binary, sizeof(binary)); - printf("length is %u\n", utstring_len(s)); - - utstring_clear(s); - utstring_printf(s,"number %d", 10); - printf("%s\n", utstring_body(s)); - - utstring_free(s); - return 0; -} -------------------------------------------------------------------------------- - -[[operations]] -Reference ---------- -These are the utstring operations. - -Operations -~~~~~~~~~~ - -[width="100%",cols="50 /* memcmp,strlen */ -#include /* ptrdiff_t */ -#include /* exit() */ - -/* These macros use decltype or the earlier __typeof GNU extension. - As decltype is only available in newer compilers (VS2010 or gcc 4.3+ - when compiling c++ source) this code uses whatever method is needed - or, for VS2008 where neither is available, uses casting workarounds. */ -#ifdef _MSC_VER /* MS compiler */ -#if _MSC_VER >= 1600 && defined(__cplusplus) /* VS2010 or newer in C++ mode */ -#define DECLTYPE(x) (decltype(x)) -#else /* VS2008 or older (or VS2010 in C mode) */ -#define NO_DECLTYPE -#define DECLTYPE(x) -#endif -#else /* GNU, Sun and other compilers */ -#define DECLTYPE(x) (__typeof(x)) -#endif - -#ifdef NO_DECLTYPE -#define DECLTYPE_ASSIGN(dst,src) \ -do { \ - char **_da_dst = (char**)(&(dst)); \ - *_da_dst = (char*)(src); \ -} while(0) -#else -#define DECLTYPE_ASSIGN(dst,src) \ -do { \ - (dst) = DECLTYPE(dst)(src); \ -} while(0) -#endif - -/* a number of the hash function use uint32_t which isn't defined on win32 */ -#ifdef _MSC_VER -typedef unsigned int uint32_t; -typedef unsigned char uint8_t; -#else -#include /* uint32_t */ -#endif - -#define UTHASH_VERSION 1.9.6 - -#ifndef uthash_fatal -#define uthash_fatal(msg) exit(-1) /* fatal error (out of memory,etc) */ -#endif -#ifndef uthash_malloc -#define uthash_malloc(sz) malloc(sz) /* malloc fcn */ -#endif -#ifndef uthash_free -#define uthash_free(ptr,sz) free(ptr) /* free fcn */ -#endif - -#ifndef uthash_noexpand_fyi -#define uthash_noexpand_fyi(tbl) /* can be defined to log noexpand */ -#endif -#ifndef uthash_expand_fyi -#define uthash_expand_fyi(tbl) /* can be defined to log expands */ -#endif - -/* initial number of buckets */ -#define HASH_INITIAL_NUM_BUCKETS 32 /* initial number of buckets */ -#define HASH_INITIAL_NUM_BUCKETS_LOG2 5 /* lg2 of initial number of buckets */ -#define HASH_BKT_CAPACITY_THRESH 10 /* expand when bucket count reaches */ - -/* calculate the element whose hash handle address is hhe */ -#define ELMT_FROM_HH(tbl,hhp) ((void*)(((char*)(hhp)) - ((tbl)->hho))) - -#define HASH_FIND(hh,head,keyptr,keylen,out) \ -do { \ - unsigned _hf_bkt,_hf_hashv; \ - out=NULL; \ - if (head) { \ - HASH_FCN(keyptr,keylen, (head)->hh.tbl->num_buckets, _hf_hashv, _hf_bkt); \ - if (HASH_BLOOM_TEST((head)->hh.tbl, _hf_hashv)) { \ - HASH_FIND_IN_BKT((head)->hh.tbl, hh, (head)->hh.tbl->buckets[ _hf_bkt ], \ - keyptr,keylen,out); \ - } \ - } \ -} while (0) - -#ifdef HASH_BLOOM -#define HASH_BLOOM_BITLEN (1ULL << HASH_BLOOM) -#define HASH_BLOOM_BYTELEN (HASH_BLOOM_BITLEN/8) + ((HASH_BLOOM_BITLEN%8) ? 1:0) -#define HASH_BLOOM_MAKE(tbl) \ -do { \ - (tbl)->bloom_nbits = HASH_BLOOM; \ - (tbl)->bloom_bv = (uint8_t*)uthash_malloc(HASH_BLOOM_BYTELEN); \ - if (!((tbl)->bloom_bv)) { uthash_fatal( "out of memory"); } \ - memset((tbl)->bloom_bv, 0, HASH_BLOOM_BYTELEN); \ - (tbl)->bloom_sig = HASH_BLOOM_SIGNATURE; \ -} while (0) - -#define HASH_BLOOM_FREE(tbl) \ -do { \ - uthash_free((tbl)->bloom_bv, HASH_BLOOM_BYTELEN); \ -} while (0) - -#define HASH_BLOOM_BITSET(bv,idx) (bv[(idx)/8] |= (1U << ((idx)%8))) -#define HASH_BLOOM_BITTEST(bv,idx) (bv[(idx)/8] & (1U << ((idx)%8))) - -#define HASH_BLOOM_ADD(tbl,hashv) \ - HASH_BLOOM_BITSET((tbl)->bloom_bv, (hashv & (uint32_t)((1ULL << (tbl)->bloom_nbits) - 1))) - -#define HASH_BLOOM_TEST(tbl,hashv) \ - HASH_BLOOM_BITTEST((tbl)->bloom_bv, (hashv & (uint32_t)((1ULL << (tbl)->bloom_nbits) - 1))) - -#else -#define HASH_BLOOM_MAKE(tbl) -#define HASH_BLOOM_FREE(tbl) -#define HASH_BLOOM_ADD(tbl,hashv) -#define HASH_BLOOM_TEST(tbl,hashv) (1) -#endif - -#define HASH_MAKE_TABLE(hh,head) \ -do { \ - (head)->hh.tbl = (UT_hash_table*)uthash_malloc( \ - sizeof(UT_hash_table)); \ - if (!((head)->hh.tbl)) { uthash_fatal( "out of memory"); } \ - memset((head)->hh.tbl, 0, sizeof(UT_hash_table)); \ - (head)->hh.tbl->tail = &((head)->hh); \ - (head)->hh.tbl->num_buckets = HASH_INITIAL_NUM_BUCKETS; \ - (head)->hh.tbl->log2_num_buckets = HASH_INITIAL_NUM_BUCKETS_LOG2; \ - (head)->hh.tbl->hho = (char*)(&(head)->hh) - (char*)(head); \ - (head)->hh.tbl->buckets = (UT_hash_bucket*)uthash_malloc( \ - HASH_INITIAL_NUM_BUCKETS*sizeof(struct UT_hash_bucket)); \ - if (! (head)->hh.tbl->buckets) { uthash_fatal( "out of memory"); } \ - memset((head)->hh.tbl->buckets, 0, \ - HASH_INITIAL_NUM_BUCKETS*sizeof(struct UT_hash_bucket)); \ - HASH_BLOOM_MAKE((head)->hh.tbl); \ - (head)->hh.tbl->signature = HASH_SIGNATURE; \ -} while(0) - -#define HASH_ADD(hh,head,fieldname,keylen_in,add) \ - HASH_ADD_KEYPTR(hh,head,&((add)->fieldname),keylen_in,add) - -#define HASH_ADD_KEYPTR(hh,head,keyptr,keylen_in,add) \ -do { \ - unsigned _ha_bkt; \ - (add)->hh.next = NULL; \ - (add)->hh.key = (char*)keyptr; \ - (add)->hh.keylen = (unsigned)keylen_in; \ - if (!(head)) { \ - head = (add); \ - (head)->hh.prev = NULL; \ - HASH_MAKE_TABLE(hh,head); \ - } else { \ - (head)->hh.tbl->tail->next = (add); \ - (add)->hh.prev = ELMT_FROM_HH((head)->hh.tbl, (head)->hh.tbl->tail); \ - (head)->hh.tbl->tail = &((add)->hh); \ - } \ - (head)->hh.tbl->num_items++; \ - (add)->hh.tbl = (head)->hh.tbl; \ - HASH_FCN(keyptr,keylen_in, (head)->hh.tbl->num_buckets, \ - (add)->hh.hashv, _ha_bkt); \ - HASH_ADD_TO_BKT((head)->hh.tbl->buckets[_ha_bkt],&(add)->hh); \ - HASH_BLOOM_ADD((head)->hh.tbl,(add)->hh.hashv); \ - HASH_EMIT_KEY(hh,head,keyptr,keylen_in); \ - HASH_FSCK(hh,head); \ -} while(0) - -#define HASH_TO_BKT( hashv, num_bkts, bkt ) \ -do { \ - bkt = ((hashv) & ((num_bkts) - 1)); \ -} while(0) - -/* delete "delptr" from the hash table. - * "the usual" patch-up process for the app-order doubly-linked-list. - * The use of _hd_hh_del below deserves special explanation. - * These used to be expressed using (delptr) but that led to a bug - * if someone used the same symbol for the head and deletee, like - * HASH_DELETE(hh,users,users); - * We want that to work, but by changing the head (users) below - * we were forfeiting our ability to further refer to the deletee (users) - * in the patch-up process. Solution: use scratch space to - * copy the deletee pointer, then the latter references are via that - * scratch pointer rather than through the repointed (users) symbol. - */ -#define HASH_DELETE(hh,head,delptr) \ -do { \ - unsigned _hd_bkt; \ - struct UT_hash_handle *_hd_hh_del; \ - if ( ((delptr)->hh.prev == NULL) && ((delptr)->hh.next == NULL) ) { \ - uthash_free((head)->hh.tbl->buckets, \ - (head)->hh.tbl->num_buckets*sizeof(struct UT_hash_bucket) ); \ - HASH_BLOOM_FREE((head)->hh.tbl); \ - uthash_free((head)->hh.tbl, sizeof(UT_hash_table)); \ - head = NULL; \ - } else { \ - _hd_hh_del = &((delptr)->hh); \ - if ((delptr) == ELMT_FROM_HH((head)->hh.tbl,(head)->hh.tbl->tail)) { \ - (head)->hh.tbl->tail = \ - (UT_hash_handle*)((char*)((delptr)->hh.prev) + \ - (head)->hh.tbl->hho); \ - } \ - if ((delptr)->hh.prev) { \ - ((UT_hash_handle*)((char*)((delptr)->hh.prev) + \ - (head)->hh.tbl->hho))->next = (delptr)->hh.next; \ - } else { \ - DECLTYPE_ASSIGN(head,(delptr)->hh.next); \ - } \ - if (_hd_hh_del->next) { \ - ((UT_hash_handle*)((char*)_hd_hh_del->next + \ - (head)->hh.tbl->hho))->prev = \ - _hd_hh_del->prev; \ - } \ - HASH_TO_BKT( _hd_hh_del->hashv, (head)->hh.tbl->num_buckets, _hd_bkt); \ - HASH_DEL_IN_BKT(hh,(head)->hh.tbl->buckets[_hd_bkt], _hd_hh_del); \ - (head)->hh.tbl->num_items--; \ - } \ - HASH_FSCK(hh,head); \ -} while (0) - - -/* convenience forms of HASH_FIND/HASH_ADD/HASH_DEL */ -#define HASH_FIND_STR(head,findstr,out) \ - HASH_FIND(hh,head,findstr,strlen(findstr),out) -#define HASH_ADD_STR(head,strfield,add) \ - HASH_ADD(hh,head,strfield,strlen(add->strfield),add) -#define HASH_FIND_INT(head,findint,out) \ - HASH_FIND(hh,head,findint,sizeof(int),out) -#define HASH_ADD_INT(head,intfield,add) \ - HASH_ADD(hh,head,intfield,sizeof(int),add) -#define HASH_FIND_PTR(head,findptr,out) \ - HASH_FIND(hh,head,findptr,sizeof(void *),out) -#define HASH_ADD_PTR(head,ptrfield,add) \ - HASH_ADD(hh,head,ptrfield,sizeof(void *),add) -#define HASH_DEL(head,delptr) \ - HASH_DELETE(hh,head,delptr) - -/* HASH_FSCK checks hash integrity on every add/delete when HASH_DEBUG is defined. - * This is for uthash developer only; it compiles away if HASH_DEBUG isn't defined. - */ -#ifdef HASH_DEBUG -#define HASH_OOPS(...) do { fprintf(stderr,__VA_ARGS__); exit(-1); } while (0) -#define HASH_FSCK(hh,head) \ -do { \ - unsigned _bkt_i; \ - unsigned _count, _bkt_count; \ - char *_prev; \ - struct UT_hash_handle *_thh; \ - if (head) { \ - _count = 0; \ - for( _bkt_i = 0; _bkt_i < (head)->hh.tbl->num_buckets; _bkt_i++) { \ - _bkt_count = 0; \ - _thh = (head)->hh.tbl->buckets[_bkt_i].hh_head; \ - _prev = NULL; \ - while (_thh) { \ - if (_prev != (char*)(_thh->hh_prev)) { \ - HASH_OOPS("invalid hh_prev %p, actual %p\n", \ - _thh->hh_prev, _prev ); \ - } \ - _bkt_count++; \ - _prev = (char*)(_thh); \ - _thh = _thh->hh_next; \ - } \ - _count += _bkt_count; \ - if ((head)->hh.tbl->buckets[_bkt_i].count != _bkt_count) { \ - HASH_OOPS("invalid bucket count %d, actual %d\n", \ - (head)->hh.tbl->buckets[_bkt_i].count, _bkt_count); \ - } \ - } \ - if (_count != (head)->hh.tbl->num_items) { \ - HASH_OOPS("invalid hh item count %d, actual %d\n", \ - (head)->hh.tbl->num_items, _count ); \ - } \ - /* traverse hh in app order; check next/prev integrity, count */ \ - _count = 0; \ - _prev = NULL; \ - _thh = &(head)->hh; \ - while (_thh) { \ - _count++; \ - if (_prev !=(char*)(_thh->prev)) { \ - HASH_OOPS("invalid prev %p, actual %p\n", \ - _thh->prev, _prev ); \ - } \ - _prev = (char*)ELMT_FROM_HH((head)->hh.tbl, _thh); \ - _thh = ( _thh->next ? (UT_hash_handle*)((char*)(_thh->next) + \ - (head)->hh.tbl->hho) : NULL ); \ - } \ - if (_count != (head)->hh.tbl->num_items) { \ - HASH_OOPS("invalid app item count %d, actual %d\n", \ - (head)->hh.tbl->num_items, _count ); \ - } \ - } \ -} while (0) -#else -#define HASH_FSCK(hh,head) -#endif - -/* When compiled with -DHASH_EMIT_KEYS, length-prefixed keys are emitted to - * the descriptor to which this macro is defined for tuning the hash function. - * The app can #include to get the prototype for write(2). */ -#ifdef HASH_EMIT_KEYS -#define HASH_EMIT_KEY(hh,head,keyptr,fieldlen) \ -do { \ - unsigned _klen = fieldlen; \ - write(HASH_EMIT_KEYS, &_klen, sizeof(_klen)); \ - write(HASH_EMIT_KEYS, keyptr, fieldlen); \ -} while (0) -#else -#define HASH_EMIT_KEY(hh,head,keyptr,fieldlen) -#endif - -/* default to Jenkin's hash unless overridden e.g. DHASH_FUNCTION=HASH_SAX */ -#ifdef HASH_FUNCTION -#define HASH_FCN HASH_FUNCTION -#else -#define HASH_FCN HASH_JEN -#endif - -/* The Bernstein hash function, used in Perl prior to v5.6 */ -#define HASH_BER(key,keylen,num_bkts,hashv,bkt) \ -do { \ - unsigned _hb_keylen=keylen; \ - char *_hb_key=(char*)(key); \ - (hashv) = 0; \ - while (_hb_keylen--) { (hashv) = ((hashv) * 33) + *_hb_key++; } \ - bkt = (hashv) & (num_bkts-1); \ -} while (0) - - -/* SAX/FNV/OAT/JEN hash functions are macro variants of those listed at - * http://eternallyconfuzzled.com/tuts/algorithms/jsw_tut_hashing.aspx */ -#define HASH_SAX(key,keylen,num_bkts,hashv,bkt) \ -do { \ - unsigned _sx_i; \ - char *_hs_key=(char*)(key); \ - hashv = 0; \ - for(_sx_i=0; _sx_i < keylen; _sx_i++) \ - hashv ^= (hashv << 5) + (hashv >> 2) + _hs_key[_sx_i]; \ - bkt = hashv & (num_bkts-1); \ -} while (0) - -#define HASH_FNV(key,keylen,num_bkts,hashv,bkt) \ -do { \ - unsigned _fn_i; \ - char *_hf_key=(char*)(key); \ - hashv = 2166136261UL; \ - for(_fn_i=0; _fn_i < keylen; _fn_i++) \ - hashv = (hashv * 16777619) ^ _hf_key[_fn_i]; \ - bkt = hashv & (num_bkts-1); \ -} while(0) - -#define HASH_OAT(key,keylen,num_bkts,hashv,bkt) \ -do { \ - unsigned _ho_i; \ - char *_ho_key=(char*)(key); \ - hashv = 0; \ - for(_ho_i=0; _ho_i < keylen; _ho_i++) { \ - hashv += _ho_key[_ho_i]; \ - hashv += (hashv << 10); \ - hashv ^= (hashv >> 6); \ - } \ - hashv += (hashv << 3); \ - hashv ^= (hashv >> 11); \ - hashv += (hashv << 15); \ - bkt = hashv & (num_bkts-1); \ -} while(0) - -#define HASH_JEN_MIX(a,b,c) \ -do { \ - a -= b; a -= c; a ^= ( c >> 13 ); \ - b -= c; b -= a; b ^= ( a << 8 ); \ - c -= a; c -= b; c ^= ( b >> 13 ); \ - a -= b; a -= c; a ^= ( c >> 12 ); \ - b -= c; b -= a; b ^= ( a << 16 ); \ - c -= a; c -= b; c ^= ( b >> 5 ); \ - a -= b; a -= c; a ^= ( c >> 3 ); \ - b -= c; b -= a; b ^= ( a << 10 ); \ - c -= a; c -= b; c ^= ( b >> 15 ); \ -} while (0) - -#define HASH_JEN(key,keylen,num_bkts,hashv,bkt) \ -do { \ - unsigned _hj_i,_hj_j,_hj_k; \ - char *_hj_key=(char*)(key); \ - hashv = 0xfeedbeef; \ - _hj_i = _hj_j = 0x9e3779b9; \ - _hj_k = (unsigned)keylen; \ - while (_hj_k >= 12) { \ - _hj_i += (_hj_key[0] + ( (unsigned)_hj_key[1] << 8 ) \ - + ( (unsigned)_hj_key[2] << 16 ) \ - + ( (unsigned)_hj_key[3] << 24 ) ); \ - _hj_j += (_hj_key[4] + ( (unsigned)_hj_key[5] << 8 ) \ - + ( (unsigned)_hj_key[6] << 16 ) \ - + ( (unsigned)_hj_key[7] << 24 ) ); \ - hashv += (_hj_key[8] + ( (unsigned)_hj_key[9] << 8 ) \ - + ( (unsigned)_hj_key[10] << 16 ) \ - + ( (unsigned)_hj_key[11] << 24 ) ); \ - \ - HASH_JEN_MIX(_hj_i, _hj_j, hashv); \ - \ - _hj_key += 12; \ - _hj_k -= 12; \ - } \ - hashv += keylen; \ - switch ( _hj_k ) { \ - case 11: hashv += ( (unsigned)_hj_key[10] << 24 ); \ - case 10: hashv += ( (unsigned)_hj_key[9] << 16 ); \ - case 9: hashv += ( (unsigned)_hj_key[8] << 8 ); \ - case 8: _hj_j += ( (unsigned)_hj_key[7] << 24 ); \ - case 7: _hj_j += ( (unsigned)_hj_key[6] << 16 ); \ - case 6: _hj_j += ( (unsigned)_hj_key[5] << 8 ); \ - case 5: _hj_j += _hj_key[4]; \ - case 4: _hj_i += ( (unsigned)_hj_key[3] << 24 ); \ - case 3: _hj_i += ( (unsigned)_hj_key[2] << 16 ); \ - case 2: _hj_i += ( (unsigned)_hj_key[1] << 8 ); \ - case 1: _hj_i += _hj_key[0]; \ - } \ - HASH_JEN_MIX(_hj_i, _hj_j, hashv); \ - bkt = hashv & (num_bkts-1); \ -} while(0) - -/* The Paul Hsieh hash function */ -#undef get16bits -#if (defined(__GNUC__) && defined(__i386__)) || defined(__WATCOMC__) \ - || defined(_MSC_VER) || defined (__BORLANDC__) || defined (__TURBOC__) -#define get16bits(d) (*((const uint16_t *) (d))) -#endif - -#if !defined (get16bits) -#define get16bits(d) ((((uint32_t)(((const uint8_t *)(d))[1])) << 8) \ - +(uint32_t)(((const uint8_t *)(d))[0]) ) -#endif -#define HASH_SFH(key,keylen,num_bkts,hashv,bkt) \ -do { \ - char *_sfh_key=(char*)(key); \ - uint32_t _sfh_tmp, _sfh_len = keylen; \ - \ - int _sfh_rem = _sfh_len & 3; \ - _sfh_len >>= 2; \ - hashv = 0xcafebabe; \ - \ - /* Main loop */ \ - for (;_sfh_len > 0; _sfh_len--) { \ - hashv += get16bits (_sfh_key); \ - _sfh_tmp = (get16bits (_sfh_key+2) << 11) ^ hashv; \ - hashv = (hashv << 16) ^ _sfh_tmp; \ - _sfh_key += 2*sizeof (uint16_t); \ - hashv += hashv >> 11; \ - } \ - \ - /* Handle end cases */ \ - switch (_sfh_rem) { \ - case 3: hashv += get16bits (_sfh_key); \ - hashv ^= hashv << 16; \ - hashv ^= _sfh_key[sizeof (uint16_t)] << 18; \ - hashv += hashv >> 11; \ - break; \ - case 2: hashv += get16bits (_sfh_key); \ - hashv ^= hashv << 11; \ - hashv += hashv >> 17; \ - break; \ - case 1: hashv += *_sfh_key; \ - hashv ^= hashv << 10; \ - hashv += hashv >> 1; \ - } \ - \ - /* Force "avalanching" of final 127 bits */ \ - hashv ^= hashv << 3; \ - hashv += hashv >> 5; \ - hashv ^= hashv << 4; \ - hashv += hashv >> 17; \ - hashv ^= hashv << 25; \ - hashv += hashv >> 6; \ - bkt = hashv & (num_bkts-1); \ -} while(0) - -#ifdef HASH_USING_NO_STRICT_ALIASING -/* The MurmurHash exploits some CPU's (x86,x86_64) tolerance for unaligned reads. - * For other types of CPU's (e.g. Sparc) an unaligned read causes a bus error. - * MurmurHash uses the faster approach only on CPU's where we know it's safe. - * - * Note the preprocessor built-in defines can be emitted using: - * - * gcc -m64 -dM -E - < /dev/null (on gcc) - * cc -## a.c (where a.c is a simple test file) (Sun Studio) - */ -#if (defined(__i386__) || defined(__x86_64__)) -#define MUR_GETBLOCK(p,i) p[i] -#else /* non intel */ -#define MUR_PLUS0_ALIGNED(p) (((unsigned long)p & 0x3) == 0) -#define MUR_PLUS1_ALIGNED(p) (((unsigned long)p & 0x3) == 1) -#define MUR_PLUS2_ALIGNED(p) (((unsigned long)p & 0x3) == 2) -#define MUR_PLUS3_ALIGNED(p) (((unsigned long)p & 0x3) == 3) -#define WP(p) ((uint32_t*)((unsigned long)(p) & ~3UL)) -#if (defined(__BIG_ENDIAN__) || defined(SPARC) || defined(__ppc__) || defined(__ppc64__)) -#define MUR_THREE_ONE(p) ((((*WP(p))&0x00ffffff) << 8) | (((*(WP(p)+1))&0xff000000) >> 24)) -#define MUR_TWO_TWO(p) ((((*WP(p))&0x0000ffff) <<16) | (((*(WP(p)+1))&0xffff0000) >> 16)) -#define MUR_ONE_THREE(p) ((((*WP(p))&0x000000ff) <<24) | (((*(WP(p)+1))&0xffffff00) >> 8)) -#else /* assume little endian non-intel */ -#define MUR_THREE_ONE(p) ((((*WP(p))&0xffffff00) >> 8) | (((*(WP(p)+1))&0x000000ff) << 24)) -#define MUR_TWO_TWO(p) ((((*WP(p))&0xffff0000) >>16) | (((*(WP(p)+1))&0x0000ffff) << 16)) -#define MUR_ONE_THREE(p) ((((*WP(p))&0xff000000) >>24) | (((*(WP(p)+1))&0x00ffffff) << 8)) -#endif -#define MUR_GETBLOCK(p,i) (MUR_PLUS0_ALIGNED(p) ? ((p)[i]) : \ - (MUR_PLUS1_ALIGNED(p) ? MUR_THREE_ONE(p) : \ - (MUR_PLUS2_ALIGNED(p) ? MUR_TWO_TWO(p) : \ - MUR_ONE_THREE(p)))) -#endif -#define MUR_ROTL32(x,r) (((x) << (r)) | ((x) >> (32 - (r)))) -#define MUR_FMIX(_h) \ -do { \ - _h ^= _h >> 16; \ - _h *= 0x85ebca6b; \ - _h ^= _h >> 13; \ - _h *= 0xc2b2ae35l; \ - _h ^= _h >> 16; \ -} while(0) - -#define HASH_MUR(key,keylen,num_bkts,hashv,bkt) \ -do { \ - const uint8_t *_mur_data = (const uint8_t*)(key); \ - const int _mur_nblocks = (keylen) / 4; \ - uint32_t _mur_h1 = 0xf88D5353; \ - uint32_t _mur_c1 = 0xcc9e2d51; \ - uint32_t _mur_c2 = 0x1b873593; \ - const uint32_t *_mur_blocks = (const uint32_t*)(_mur_data+_mur_nblocks*4); \ - int _mur_i; \ - for(_mur_i = -_mur_nblocks; _mur_i; _mur_i++) { \ - uint32_t _mur_k1 = MUR_GETBLOCK(_mur_blocks,_mur_i); \ - _mur_k1 *= _mur_c1; \ - _mur_k1 = MUR_ROTL32(_mur_k1,15); \ - _mur_k1 *= _mur_c2; \ - \ - _mur_h1 ^= _mur_k1; \ - _mur_h1 = MUR_ROTL32(_mur_h1,13); \ - _mur_h1 = _mur_h1*5+0xe6546b64; \ - } \ - const uint8_t *_mur_tail = (const uint8_t*)(_mur_data + _mur_nblocks*4); \ - uint32_t _mur_k1=0; \ - switch((keylen) & 3) { \ - case 3: _mur_k1 ^= _mur_tail[2] << 16; \ - case 2: _mur_k1 ^= _mur_tail[1] << 8; \ - case 1: _mur_k1 ^= _mur_tail[0]; \ - _mur_k1 *= _mur_c1; \ - _mur_k1 = MUR_ROTL32(_mur_k1,15); \ - _mur_k1 *= _mur_c2; \ - _mur_h1 ^= _mur_k1; \ - } \ - _mur_h1 ^= (keylen); \ - MUR_FMIX(_mur_h1); \ - hashv = _mur_h1; \ - bkt = hashv & (num_bkts-1); \ -} while(0) -#endif /* HASH_USING_NO_STRICT_ALIASING */ - -/* key comparison function; return 0 if keys equal */ -#define HASH_KEYCMP(a,b,len) memcmp(a,b,len) - -/* iterate over items in a known bucket to find desired item */ -#define HASH_FIND_IN_BKT(tbl,hh,head,keyptr,keylen_in,out) \ -do { \ - if (head.hh_head) DECLTYPE_ASSIGN(out,ELMT_FROM_HH(tbl,head.hh_head)); \ - else out=NULL; \ - while (out) { \ - if ((out)->hh.keylen == keylen_in) { \ - if ((HASH_KEYCMP((out)->hh.key,keyptr,keylen_in)) == 0) break; \ - } \ - if ((out)->hh.hh_next) DECLTYPE_ASSIGN(out,ELMT_FROM_HH(tbl,(out)->hh.hh_next)); \ - else out = NULL; \ - } \ -} while(0) - -/* add an item to a bucket */ -#define HASH_ADD_TO_BKT(head,addhh) \ -do { \ - head.count++; \ - (addhh)->hh_next = head.hh_head; \ - (addhh)->hh_prev = NULL; \ - if (head.hh_head) { (head).hh_head->hh_prev = (addhh); } \ - (head).hh_head=addhh; \ - if (head.count >= ((head.expand_mult+1) * HASH_BKT_CAPACITY_THRESH) \ - && (addhh)->tbl->noexpand != 1) { \ - HASH_EXPAND_BUCKETS((addhh)->tbl); \ - } \ -} while(0) - -/* remove an item from a given bucket */ -#define HASH_DEL_IN_BKT(hh,head,hh_del) \ - (head).count--; \ - if ((head).hh_head == hh_del) { \ - (head).hh_head = hh_del->hh_next; \ - } \ - if (hh_del->hh_prev) { \ - hh_del->hh_prev->hh_next = hh_del->hh_next; \ - } \ - if (hh_del->hh_next) { \ - hh_del->hh_next->hh_prev = hh_del->hh_prev; \ - } - -/* Bucket expansion has the effect of doubling the number of buckets - * and redistributing the items into the new buckets. Ideally the - * items will distribute more or less evenly into the new buckets - * (the extent to which this is true is a measure of the quality of - * the hash function as it applies to the key domain). - * - * With the items distributed into more buckets, the chain length - * (item count) in each bucket is reduced. Thus by expanding buckets - * the hash keeps a bound on the chain length. This bounded chain - * length is the essence of how a hash provides constant time lookup. - * - * The calculation of tbl->ideal_chain_maxlen below deserves some - * explanation. First, keep in mind that we're calculating the ideal - * maximum chain length based on the *new* (doubled) bucket count. - * In fractions this is just n/b (n=number of items,b=new num buckets). - * Since the ideal chain length is an integer, we want to calculate - * ceil(n/b). We don't depend on floating point arithmetic in this - * hash, so to calculate ceil(n/b) with integers we could write - * - * ceil(n/b) = (n/b) + ((n%b)?1:0) - * - * and in fact a previous version of this hash did just that. - * But now we have improved things a bit by recognizing that b is - * always a power of two. We keep its base 2 log handy (call it lb), - * so now we can write this with a bit shift and logical AND: - * - * ceil(n/b) = (n>>lb) + ( (n & (b-1)) ? 1:0) - * - */ -#define HASH_EXPAND_BUCKETS(tbl) \ -do { \ - unsigned _he_bkt; \ - unsigned _he_bkt_i; \ - struct UT_hash_handle *_he_thh, *_he_hh_nxt; \ - UT_hash_bucket *_he_new_buckets, *_he_newbkt; \ - _he_new_buckets = (UT_hash_bucket*)uthash_malloc( \ - 2 * tbl->num_buckets * sizeof(struct UT_hash_bucket)); \ - if (!_he_new_buckets) { uthash_fatal( "out of memory"); } \ - memset(_he_new_buckets, 0, \ - 2 * tbl->num_buckets * sizeof(struct UT_hash_bucket)); \ - tbl->ideal_chain_maxlen = \ - (tbl->num_items >> (tbl->log2_num_buckets+1)) + \ - ((tbl->num_items & ((tbl->num_buckets*2)-1)) ? 1 : 0); \ - tbl->nonideal_items = 0; \ - for(_he_bkt_i = 0; _he_bkt_i < tbl->num_buckets; _he_bkt_i++) \ - { \ - _he_thh = tbl->buckets[ _he_bkt_i ].hh_head; \ - while (_he_thh) { \ - _he_hh_nxt = _he_thh->hh_next; \ - HASH_TO_BKT( _he_thh->hashv, tbl->num_buckets*2, _he_bkt); \ - _he_newbkt = &(_he_new_buckets[ _he_bkt ]); \ - if (++(_he_newbkt->count) > tbl->ideal_chain_maxlen) { \ - tbl->nonideal_items++; \ - _he_newbkt->expand_mult = _he_newbkt->count / \ - tbl->ideal_chain_maxlen; \ - } \ - _he_thh->hh_prev = NULL; \ - _he_thh->hh_next = _he_newbkt->hh_head; \ - if (_he_newbkt->hh_head) _he_newbkt->hh_head->hh_prev = \ - _he_thh; \ - _he_newbkt->hh_head = _he_thh; \ - _he_thh = _he_hh_nxt; \ - } \ - } \ - uthash_free( tbl->buckets, tbl->num_buckets*sizeof(struct UT_hash_bucket) ); \ - tbl->num_buckets *= 2; \ - tbl->log2_num_buckets++; \ - tbl->buckets = _he_new_buckets; \ - tbl->ineff_expands = (tbl->nonideal_items > (tbl->num_items >> 1)) ? \ - (tbl->ineff_expands+1) : 0; \ - if (tbl->ineff_expands > 1) { \ - tbl->noexpand=1; \ - uthash_noexpand_fyi(tbl); \ - } \ - uthash_expand_fyi(tbl); \ -} while(0) - - -/* This is an adaptation of Simon Tatham's O(n log(n)) mergesort */ -/* Note that HASH_SORT assumes the hash handle name to be hh. - * HASH_SRT was added to allow the hash handle name to be passed in. */ -#define HASH_SORT(head,cmpfcn) HASH_SRT(hh,head,cmpfcn) -#define HASH_SRT(hh,head,cmpfcn) \ -do { \ - unsigned _hs_i; \ - unsigned _hs_looping,_hs_nmerges,_hs_insize,_hs_psize,_hs_qsize; \ - struct UT_hash_handle *_hs_p, *_hs_q, *_hs_e, *_hs_list, *_hs_tail; \ - if (head) { \ - _hs_insize = 1; \ - _hs_looping = 1; \ - _hs_list = &((head)->hh); \ - while (_hs_looping) { \ - _hs_p = _hs_list; \ - _hs_list = NULL; \ - _hs_tail = NULL; \ - _hs_nmerges = 0; \ - while (_hs_p) { \ - _hs_nmerges++; \ - _hs_q = _hs_p; \ - _hs_psize = 0; \ - for ( _hs_i = 0; _hs_i < _hs_insize; _hs_i++ ) { \ - _hs_psize++; \ - _hs_q = (UT_hash_handle*)((_hs_q->next) ? \ - ((void*)((char*)(_hs_q->next) + \ - (head)->hh.tbl->hho)) : NULL); \ - if (! (_hs_q) ) break; \ - } \ - _hs_qsize = _hs_insize; \ - while ((_hs_psize > 0) || ((_hs_qsize > 0) && _hs_q )) { \ - if (_hs_psize == 0) { \ - _hs_e = _hs_q; \ - _hs_q = (UT_hash_handle*)((_hs_q->next) ? \ - ((void*)((char*)(_hs_q->next) + \ - (head)->hh.tbl->hho)) : NULL); \ - _hs_qsize--; \ - } else if ( (_hs_qsize == 0) || !(_hs_q) ) { \ - _hs_e = _hs_p; \ - _hs_p = (UT_hash_handle*)((_hs_p->next) ? \ - ((void*)((char*)(_hs_p->next) + \ - (head)->hh.tbl->hho)) : NULL); \ - _hs_psize--; \ - } else if (( \ - cmpfcn(DECLTYPE(head)(ELMT_FROM_HH((head)->hh.tbl,_hs_p)), \ - DECLTYPE(head)(ELMT_FROM_HH((head)->hh.tbl,_hs_q))) \ - ) <= 0) { \ - _hs_e = _hs_p; \ - _hs_p = (UT_hash_handle*)((_hs_p->next) ? \ - ((void*)((char*)(_hs_p->next) + \ - (head)->hh.tbl->hho)) : NULL); \ - _hs_psize--; \ - } else { \ - _hs_e = _hs_q; \ - _hs_q = (UT_hash_handle*)((_hs_q->next) ? \ - ((void*)((char*)(_hs_q->next) + \ - (head)->hh.tbl->hho)) : NULL); \ - _hs_qsize--; \ - } \ - if ( _hs_tail ) { \ - _hs_tail->next = ((_hs_e) ? \ - ELMT_FROM_HH((head)->hh.tbl,_hs_e) : NULL); \ - } else { \ - _hs_list = _hs_e; \ - } \ - _hs_e->prev = ((_hs_tail) ? \ - ELMT_FROM_HH((head)->hh.tbl,_hs_tail) : NULL); \ - _hs_tail = _hs_e; \ - } \ - _hs_p = _hs_q; \ - } \ - _hs_tail->next = NULL; \ - if ( _hs_nmerges <= 1 ) { \ - _hs_looping=0; \ - (head)->hh.tbl->tail = _hs_tail; \ - DECLTYPE_ASSIGN(head,ELMT_FROM_HH((head)->hh.tbl, _hs_list)); \ - } \ - _hs_insize *= 2; \ - } \ - HASH_FSCK(hh,head); \ - } \ -} while (0) - -/* This function selects items from one hash into another hash. - * The end result is that the selected items have dual presence - * in both hashes. There is no copy of the items made; rather - * they are added into the new hash through a secondary hash - * hash handle that must be present in the structure. */ -#define HASH_SELECT(hh_dst, dst, hh_src, src, cond) \ -do { \ - unsigned _src_bkt, _dst_bkt; \ - void *_last_elt=NULL, *_elt; \ - UT_hash_handle *_src_hh, *_dst_hh, *_last_elt_hh=NULL; \ - ptrdiff_t _dst_hho = ((char*)(&(dst)->hh_dst) - (char*)(dst)); \ - if (src) { \ - for(_src_bkt=0; _src_bkt < (src)->hh_src.tbl->num_buckets; _src_bkt++) { \ - for(_src_hh = (src)->hh_src.tbl->buckets[_src_bkt].hh_head; \ - _src_hh; \ - _src_hh = _src_hh->hh_next) { \ - _elt = ELMT_FROM_HH((src)->hh_src.tbl, _src_hh); \ - if (cond(_elt)) { \ - _dst_hh = (UT_hash_handle*)(((char*)_elt) + _dst_hho); \ - _dst_hh->key = _src_hh->key; \ - _dst_hh->keylen = _src_hh->keylen; \ - _dst_hh->hashv = _src_hh->hashv; \ - _dst_hh->prev = _last_elt; \ - _dst_hh->next = NULL; \ - if (_last_elt_hh) { _last_elt_hh->next = _elt; } \ - if (!dst) { \ - DECLTYPE_ASSIGN(dst,_elt); \ - HASH_MAKE_TABLE(hh_dst,dst); \ - } else { \ - _dst_hh->tbl = (dst)->hh_dst.tbl; \ - } \ - HASH_TO_BKT(_dst_hh->hashv, _dst_hh->tbl->num_buckets, _dst_bkt); \ - HASH_ADD_TO_BKT(_dst_hh->tbl->buckets[_dst_bkt],_dst_hh); \ - (dst)->hh_dst.tbl->num_items++; \ - _last_elt = _elt; \ - _last_elt_hh = _dst_hh; \ - } \ - } \ - } \ - } \ - HASH_FSCK(hh_dst,dst); \ -} while (0) - -#define HASH_CLEAR(hh,head) \ -do { \ - if (head) { \ - uthash_free((head)->hh.tbl->buckets, \ - (head)->hh.tbl->num_buckets*sizeof(struct UT_hash_bucket)); \ - HASH_BLOOM_FREE((head)->hh.tbl); \ - uthash_free((head)->hh.tbl, sizeof(UT_hash_table)); \ - (head)=NULL; \ - } \ -} while(0) - -#ifdef NO_DECLTYPE -#define HASH_ITER(hh,head,el,tmp) \ -for((el)=(head), (*(char**)(&(tmp)))=(char*)((head)?(head)->hh.next:NULL); \ - el; (el)=(tmp),(*(char**)(&(tmp)))=(char*)((tmp)?(tmp)->hh.next:NULL)) -#else -#define HASH_ITER(hh,head,el,tmp) \ -for((el)=(head),(tmp)=DECLTYPE(el)((head)?(head)->hh.next:NULL); \ - el; (el)=(tmp),(tmp)=DECLTYPE(el)((tmp)?(tmp)->hh.next:NULL)) -#endif - -/* obtain a count of items in the hash */ -#define HASH_COUNT(head) HASH_CNT(hh,head) -#define HASH_CNT(hh,head) ((head)?((head)->hh.tbl->num_items):0) - -typedef struct UT_hash_bucket { - struct UT_hash_handle *hh_head; - unsigned count; - - /* expand_mult is normally set to 0. In this situation, the max chain length - * threshold is enforced at its default value, HASH_BKT_CAPACITY_THRESH. (If - * the bucket's chain exceeds this length, bucket expansion is triggered). - * However, setting expand_mult to a non-zero value delays bucket expansion - * (that would be triggered by additions to this particular bucket) - * until its chain length reaches a *multiple* of HASH_BKT_CAPACITY_THRESH. - * (The multiplier is simply expand_mult+1). The whole idea of this - * multiplier is to reduce bucket expansions, since they are expensive, in - * situations where we know that a particular bucket tends to be overused. - * It is better to let its chain length grow to a longer yet-still-bounded - * value, than to do an O(n) bucket expansion too often. - */ - unsigned expand_mult; - -} UT_hash_bucket; - -/* random signature used only to find hash tables in external analysis */ -#define HASH_SIGNATURE 0xa0111fe1 -#define HASH_BLOOM_SIGNATURE 0xb12220f2 - -typedef struct UT_hash_table { - UT_hash_bucket *buckets; - unsigned num_buckets, log2_num_buckets; - unsigned num_items; - struct UT_hash_handle *tail; /* tail hh in app order, for fast append */ - ptrdiff_t hho; /* hash handle offset (byte pos of hash handle in element */ - - /* in an ideal situation (all buckets used equally), no bucket would have - * more than ceil(#items/#buckets) items. that's the ideal chain length. */ - unsigned ideal_chain_maxlen; - - /* nonideal_items is the number of items in the hash whose chain position - * exceeds the ideal chain maxlen. these items pay the penalty for an uneven - * hash distribution; reaching them in a chain traversal takes >ideal steps */ - unsigned nonideal_items; - - /* ineffective expands occur when a bucket doubling was performed, but - * afterward, more than half the items in the hash had nonideal chain - * positions. If this happens on two consecutive expansions we inhibit any - * further expansion, as it's not helping; this happens when the hash - * function isn't a good fit for the key domain. When expansion is inhibited - * the hash will still work, albeit no longer in constant time. */ - unsigned ineff_expands, noexpand; - - uint32_t signature; /* used only to find hash tables in external analysis */ -#ifdef HASH_BLOOM - uint32_t bloom_sig; /* used only to test bloom exists in external analysis */ - uint8_t *bloom_bv; - char bloom_nbits; -#endif - -} UT_hash_table; - -typedef struct UT_hash_handle { - struct UT_hash_table *tbl; - void *prev; /* prev element in app order */ - void *next; /* next element in app order */ - struct UT_hash_handle *hh_prev; /* previous hh in bucket order */ - struct UT_hash_handle *hh_next; /* next hh in bucket order */ - void *key; /* ptr to enclosing struct's key */ - unsigned keylen; /* enclosing struct's key len */ - unsigned hashv; /* result of hash-fcn(key) */ -} UT_hash_handle; - -#endif /* UTHASH_H */ diff --git a/src/lib/systemlib/uthash/utlist.h b/src/lib/systemlib/uthash/utlist.h deleted file mode 100644 index 1578acad2f..0000000000 --- a/src/lib/systemlib/uthash/utlist.h +++ /dev/null @@ -1,522 +0,0 @@ -/* -Copyright (c) 2007-2012, Troy D. Hanson http://uthash.sourceforge.net -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS -IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED -TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A -PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER -OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, -EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, -PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR -PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF -LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING -NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#ifndef UTLIST_H -#define UTLIST_H - -#define UTLIST_VERSION 1.9.6 - -#include - -/* - * This file contains macros to manipulate singly and doubly-linked lists. - * - * 1. LL_ macros: singly-linked lists. - * 2. DL_ macros: doubly-linked lists. - * 3. CDL_ macros: circular doubly-linked lists. - * - * To use singly-linked lists, your structure must have a "next" pointer. - * To use doubly-linked lists, your structure must "prev" and "next" pointers. - * Either way, the pointer to the head of the list must be initialized to NULL. - * - * ----------------.EXAMPLE ------------------------- - * struct item { - * int id; - * struct item *prev, *next; - * } - * - * struct item *list = NULL: - * - * int main() { - * struct item *item; - * ... allocate and populate item ... - * DL_APPEND(list, item); - * } - * -------------------------------------------------- - * - * For doubly-linked lists, the append and delete macros are O(1) - * For singly-linked lists, append and delete are O(n) but prepend is O(1) - * The sort macro is O(n log(n)) for all types of single/double/circular lists. - */ - -/* These macros use decltype or the earlier __typeof GNU extension. - As decltype is only available in newer compilers (VS2010 or gcc 4.3+ - when compiling c++ code), this code uses whatever method is needed - or, for VS2008 where neither is available, uses casting workarounds. */ -#ifdef _MSC_VER /* MS compiler */ -#if _MSC_VER >= 1600 && defined(__cplusplus) /* VS2010 or newer in C++ mode */ -#define LDECLTYPE(x) decltype(x) -#else /* VS2008 or older (or VS2010 in C mode) */ -#define NO_DECLTYPE -#define LDECLTYPE(x) char* -#endif -#else /* GNU, Sun and other compilers */ -#define LDECLTYPE(x) __typeof(x) -#endif - -/* for VS2008 we use some workarounds to get around the lack of decltype, - * namely, we always reassign our tmp variable to the list head if we need - * to dereference its prev/next pointers, and save/restore the real head.*/ -#ifdef NO_DECLTYPE -#define _SV(elt,list) _tmp = (char*)(list); {char **_alias = (char**)&(list); *_alias = (elt); } -#define _NEXT(elt,list) ((char*)((list)->next)) -#define _NEXTASGN(elt,list,to) { char **_alias = (char**)&((list)->next); *_alias=(char*)(to); } -#define _PREV(elt,list) ((char*)((list)->prev)) -#define _PREVASGN(elt,list,to) { char **_alias = (char**)&((list)->prev); *_alias=(char*)(to); } -#define _RS(list) { char **_alias = (char**)&(list); *_alias=_tmp; } -#define _CASTASGN(a,b) { char **_alias = (char**)&(a); *_alias=(char*)(b); } -#else -#define _SV(elt,list) -#define _NEXT(elt,list) ((elt)->next) -#define _NEXTASGN(elt,list,to) ((elt)->next)=(to) -#define _PREV(elt,list) ((elt)->prev) -#define _PREVASGN(elt,list,to) ((elt)->prev)=(to) -#define _RS(list) -#define _CASTASGN(a,b) (a)=(b) -#endif - -/****************************************************************************** - * The sort macro is an adaptation of Simon Tatham's O(n log(n)) mergesort * - * Unwieldy variable names used here to avoid shadowing passed-in variables. * - *****************************************************************************/ -#define LL_SORT(list, cmp) \ -do { \ - LDECLTYPE(list) _ls_p; \ - LDECLTYPE(list) _ls_q; \ - LDECLTYPE(list) _ls_e; \ - LDECLTYPE(list) _ls_tail; \ - LDECLTYPE(list) _ls_oldhead; \ - LDECLTYPE(list) _tmp; \ - int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ - if (list) { \ - _ls_insize = 1; \ - _ls_looping = 1; \ - while (_ls_looping) { \ - _CASTASGN(_ls_p,list); \ - _CASTASGN(_ls_oldhead,list); \ - list = NULL; \ - _ls_tail = NULL; \ - _ls_nmerges = 0; \ - while (_ls_p) { \ - _ls_nmerges++; \ - _ls_q = _ls_p; \ - _ls_psize = 0; \ - for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ - _ls_psize++; \ - _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); \ - if (!_ls_q) break; \ - } \ - _ls_qsize = _ls_insize; \ - while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ - if (_ls_psize == 0) { \ - _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \ - } else if (_ls_qsize == 0 || !_ls_q) { \ - _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \ - } else if (cmp(_ls_p,_ls_q) <= 0) { \ - _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \ - } else { \ - _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \ - } \ - if (_ls_tail) { \ - _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,_ls_e); _RS(list); \ - } else { \ - _CASTASGN(list,_ls_e); \ - } \ - _ls_tail = _ls_e; \ - } \ - _ls_p = _ls_q; \ - } \ - _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,NULL); _RS(list); \ - if (_ls_nmerges <= 1) { \ - _ls_looping=0; \ - } \ - _ls_insize *= 2; \ - } \ - } else _tmp=NULL; /* quiet gcc unused variable warning */ \ -} while (0) - -#define DL_SORT(list, cmp) \ -do { \ - LDECLTYPE(list) _ls_p; \ - LDECLTYPE(list) _ls_q; \ - LDECLTYPE(list) _ls_e; \ - LDECLTYPE(list) _ls_tail; \ - LDECLTYPE(list) _ls_oldhead; \ - LDECLTYPE(list) _tmp; \ - int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ - if (list) { \ - _ls_insize = 1; \ - _ls_looping = 1; \ - while (_ls_looping) { \ - _CASTASGN(_ls_p,list); \ - _CASTASGN(_ls_oldhead,list); \ - list = NULL; \ - _ls_tail = NULL; \ - _ls_nmerges = 0; \ - while (_ls_p) { \ - _ls_nmerges++; \ - _ls_q = _ls_p; \ - _ls_psize = 0; \ - for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ - _ls_psize++; \ - _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); \ - if (!_ls_q) break; \ - } \ - _ls_qsize = _ls_insize; \ - while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ - if (_ls_psize == 0) { \ - _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \ - } else if (_ls_qsize == 0 || !_ls_q) { \ - _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \ - } else if (cmp(_ls_p,_ls_q) <= 0) { \ - _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \ - } else { \ - _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \ - } \ - if (_ls_tail) { \ - _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,_ls_e); _RS(list); \ - } else { \ - _CASTASGN(list,_ls_e); \ - } \ - _SV(_ls_e,list); _PREVASGN(_ls_e,list,_ls_tail); _RS(list); \ - _ls_tail = _ls_e; \ - } \ - _ls_p = _ls_q; \ - } \ - _CASTASGN(list->prev, _ls_tail); \ - _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,NULL); _RS(list); \ - if (_ls_nmerges <= 1) { \ - _ls_looping=0; \ - } \ - _ls_insize *= 2; \ - } \ - } else _tmp=NULL; /* quiet gcc unused variable warning */ \ -} while (0) - -#define CDL_SORT(list, cmp) \ -do { \ - LDECLTYPE(list) _ls_p; \ - LDECLTYPE(list) _ls_q; \ - LDECLTYPE(list) _ls_e; \ - LDECLTYPE(list) _ls_tail; \ - LDECLTYPE(list) _ls_oldhead; \ - LDECLTYPE(list) _tmp; \ - LDECLTYPE(list) _tmp2; \ - int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \ - if (list) { \ - _ls_insize = 1; \ - _ls_looping = 1; \ - while (_ls_looping) { \ - _CASTASGN(_ls_p,list); \ - _CASTASGN(_ls_oldhead,list); \ - list = NULL; \ - _ls_tail = NULL; \ - _ls_nmerges = 0; \ - while (_ls_p) { \ - _ls_nmerges++; \ - _ls_q = _ls_p; \ - _ls_psize = 0; \ - for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \ - _ls_psize++; \ - _SV(_ls_q,list); \ - if (_NEXT(_ls_q,list) == _ls_oldhead) { \ - _ls_q = NULL; \ - } else { \ - _ls_q = _NEXT(_ls_q,list); \ - } \ - _RS(list); \ - if (!_ls_q) break; \ - } \ - _ls_qsize = _ls_insize; \ - while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \ - if (_ls_psize == 0) { \ - _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \ - if (_ls_q == _ls_oldhead) { _ls_q = NULL; } \ - } else if (_ls_qsize == 0 || !_ls_q) { \ - _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \ - if (_ls_p == _ls_oldhead) { _ls_p = NULL; } \ - } else if (cmp(_ls_p,_ls_q) <= 0) { \ - _ls_e = _ls_p; _SV(_ls_p,list); _ls_p = _NEXT(_ls_p,list); _RS(list); _ls_psize--; \ - if (_ls_p == _ls_oldhead) { _ls_p = NULL; } \ - } else { \ - _ls_e = _ls_q; _SV(_ls_q,list); _ls_q = _NEXT(_ls_q,list); _RS(list); _ls_qsize--; \ - if (_ls_q == _ls_oldhead) { _ls_q = NULL; } \ - } \ - if (_ls_tail) { \ - _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,_ls_e); _RS(list); \ - } else { \ - _CASTASGN(list,_ls_e); \ - } \ - _SV(_ls_e,list); _PREVASGN(_ls_e,list,_ls_tail); _RS(list); \ - _ls_tail = _ls_e; \ - } \ - _ls_p = _ls_q; \ - } \ - _CASTASGN(list->prev,_ls_tail); \ - _CASTASGN(_tmp2,list); \ - _SV(_ls_tail,list); _NEXTASGN(_ls_tail,list,_tmp2); _RS(list); \ - if (_ls_nmerges <= 1) { \ - _ls_looping=0; \ - } \ - _ls_insize *= 2; \ - } \ - } else _tmp=NULL; /* quiet gcc unused variable warning */ \ -} while (0) - -/****************************************************************************** - * singly linked list macros (non-circular) * - *****************************************************************************/ -#define LL_PREPEND(head,add) \ -do { \ - (add)->next = head; \ - head = add; \ -} while (0) - -#define LL_CONCAT(head1,head2) \ -do { \ - LDECLTYPE(head1) _tmp; \ - if (head1) { \ - _tmp = head1; \ - while (_tmp->next) { _tmp = _tmp->next; } \ - _tmp->next=(head2); \ - } else { \ - (head1)=(head2); \ - } \ -} while (0) - -#define LL_APPEND(head,add) \ -do { \ - LDECLTYPE(head) _tmp; \ - (add)->next=NULL; \ - if (head) { \ - _tmp = head; \ - while (_tmp->next) { _tmp = _tmp->next; } \ - _tmp->next=(add); \ - } else { \ - (head)=(add); \ - } \ -} while (0) - -#define LL_DELETE(head,del) \ -do { \ - LDECLTYPE(head) _tmp; \ - if ((head) == (del)) { \ - (head)=(head)->next; \ - } else { \ - _tmp = head; \ - while (_tmp->next && (_tmp->next != (del))) { \ - _tmp = _tmp->next; \ - } \ - if (_tmp->next) { \ - _tmp->next = ((del)->next); \ - } \ - } \ -} while (0) - -/* Here are VS2008 replacements for LL_APPEND and LL_DELETE */ -#define LL_APPEND_VS2008(head,add) \ -do { \ - if (head) { \ - (add)->next = head; /* use add->next as a temp variable */ \ - while ((add)->next->next) { (add)->next = (add)->next->next; } \ - (add)->next->next=(add); \ - } else { \ - (head)=(add); \ - } \ - (add)->next=NULL; \ -} while (0) - -#define LL_DELETE_VS2008(head,del) \ -do { \ - if ((head) == (del)) { \ - (head)=(head)->next; \ - } else { \ - char *_tmp = (char*)(head); \ - while ((head)->next && ((head)->next != (del))) { \ - head = (head)->next; \ - } \ - if ((head)->next) { \ - (head)->next = ((del)->next); \ - } \ - { \ - char **_head_alias = (char**)&(head); \ - *_head_alias = _tmp; \ - } \ - } \ -} while (0) -#ifdef NO_DECLTYPE -#undef LL_APPEND -#define LL_APPEND LL_APPEND_VS2008 -#undef LL_DELETE -#define LL_DELETE LL_DELETE_VS2008 -#undef LL_CONCAT /* no LL_CONCAT_VS2008 */ -#undef DL_CONCAT /* no DL_CONCAT_VS2008 */ -#endif -/* end VS2008 replacements */ - -#define LL_FOREACH(head,el) \ - for(el=head;el;el=(el)->next) - -#define LL_FOREACH_SAFE(head,el,tmp) \ - for((el)=(head);(el) && (tmp = (el)->next, 1); (el) = tmp) - -#define LL_SEARCH_SCALAR(head,out,field,val) \ -do { \ - LL_FOREACH(head,out) { \ - if ((out)->field == (val)) break; \ - } \ -} while(0) - -#define LL_SEARCH(head,out,elt,cmp) \ -do { \ - LL_FOREACH(head,out) { \ - if ((cmp(out,elt))==0) break; \ - } \ -} while(0) - -/****************************************************************************** - * doubly linked list macros (non-circular) * - *****************************************************************************/ -#define DL_PREPEND(head,add) \ -do { \ - (add)->next = head; \ - if (head) { \ - (add)->prev = (head)->prev; \ - (head)->prev = (add); \ - } else { \ - (add)->prev = (add); \ - } \ - (head) = (add); \ -} while (0) - -#define DL_APPEND(head,add) \ -do { \ - if (head) { \ - (add)->prev = (head)->prev; \ - (head)->prev->next = (add); \ - (head)->prev = (add); \ - (add)->next = NULL; \ - } else { \ - (head)=(add); \ - (head)->prev = (head); \ - (head)->next = NULL; \ - } \ -} while (0) - -#define DL_CONCAT(head1,head2) \ -do { \ - LDECLTYPE(head1) _tmp; \ - if (head2) { \ - if (head1) { \ - _tmp = (head2)->prev; \ - (head2)->prev = (head1)->prev; \ - (head1)->prev->next = (head2); \ - (head1)->prev = _tmp; \ - } else { \ - (head1)=(head2); \ - } \ - } \ -} while (0) - -#define DL_DELETE(head,del) \ -do { \ - assert((del)->prev != NULL); \ - if ((del)->prev == (del)) { \ - (head)=NULL; \ - } else if ((del)==(head)) { \ - (del)->next->prev = (del)->prev; \ - (head) = (del)->next; \ - } else { \ - (del)->prev->next = (del)->next; \ - if ((del)->next) { \ - (del)->next->prev = (del)->prev; \ - } else { \ - (head)->prev = (del)->prev; \ - } \ - } \ -} while (0) - - -#define DL_FOREACH(head,el) \ - for(el=head;el;el=(el)->next) - -/* this version is safe for deleting the elements during iteration */ -#define DL_FOREACH_SAFE(head,el,tmp) \ - for((el)=(head);(el) && (tmp = (el)->next, 1); (el) = tmp) - -/* these are identical to their singly-linked list counterparts */ -#define DL_SEARCH_SCALAR LL_SEARCH_SCALAR -#define DL_SEARCH LL_SEARCH - -/****************************************************************************** - * circular doubly linked list macros * - *****************************************************************************/ -#define CDL_PREPEND(head,add) \ -do { \ - if (head) { \ - (add)->prev = (head)->prev; \ - (add)->next = (head); \ - (head)->prev = (add); \ - (add)->prev->next = (add); \ - } else { \ - (add)->prev = (add); \ - (add)->next = (add); \ - } \ -(head)=(add); \ -} while (0) - -#define CDL_DELETE(head,del) \ -do { \ - if ( ((head)==(del)) && ((head)->next == (head))) { \ - (head) = 0L; \ - } else { \ - (del)->next->prev = (del)->prev; \ - (del)->prev->next = (del)->next; \ - if ((del) == (head)) (head)=(del)->next; \ - } \ -} while (0) - -#define CDL_FOREACH(head,el) \ - for(el=head;el;el=((el)->next==head ? 0L : (el)->next)) - -#define CDL_FOREACH_SAFE(head,el,tmp1,tmp2) \ - for((el)=(head), ((tmp1)=(head)?((head)->prev):NULL); \ - (el) && ((tmp2)=(el)->next, 1); \ - ((el) = (((el)==(tmp1)) ? 0L : (tmp2)))) - -#define CDL_SEARCH_SCALAR(head,out,field,val) \ -do { \ - CDL_FOREACH(head,out) { \ - if ((out)->field == (val)) break; \ - } \ -} while(0) - -#define CDL_SEARCH(head,out,elt,cmp) \ -do { \ - CDL_FOREACH(head,out) { \ - if ((cmp(out,elt))==0) break; \ - } \ -} while(0) - -#endif /* UTLIST_H */ - diff --git a/src/lib/systemlib/uthash/utstring.h b/src/lib/systemlib/uthash/utstring.h deleted file mode 100644 index a181ad7785..0000000000 --- a/src/lib/systemlib/uthash/utstring.h +++ /dev/null @@ -1,148 +0,0 @@ -/* -Copyright (c) 2008-2012, Troy D. Hanson http://uthash.sourceforge.net -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS -IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED -TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A -PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER -OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, -EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, -PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR -PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF -LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING -NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -/* a dynamic string implementation using macros - * see http://uthash.sourceforge.net/utstring - */ -#ifndef UTSTRING_H -#define UTSTRING_H - -#define UTSTRING_VERSION 1.9.6 - -#ifdef __GNUC__ -#define _UNUSED_ __attribute__ ((__unused__)) -#else -#define _UNUSED_ -#endif - -#include -#include -#include -#define oom() exit(-1) - -typedef struct { - char *d; - size_t n; /* allocd size */ - size_t i; /* index of first unused byte */ -} UT_string; - -#define utstring_reserve(s,amt) \ -do { \ - if (((s)->n - (s)->i) < (size_t)(amt)) { \ - (s)->d = (char*)realloc((s)->d, (s)->n + amt); \ - if ((s)->d == NULL) oom(); \ - (s)->n += amt; \ - } \ -} while(0) - -#define utstring_init(s) \ -do { \ - (s)->n = 0; (s)->i = 0; (s)->d = NULL; \ - utstring_reserve(s,100); \ - (s)->d[0] = '\0'; \ -} while(0) - -#define utstring_done(s) \ -do { \ - if ((s)->d != NULL) free((s)->d); \ - (s)->n = 0; \ -} while(0) - -#define utstring_free(s) \ -do { \ - utstring_done(s); \ - free(s); \ -} while(0) - -#define utstring_new(s) \ -do { \ - s = (UT_string*)calloc(sizeof(UT_string),1); \ - if (!s) oom(); \ - utstring_init(s); \ -} while(0) - -#define utstring_renew(s) \ -do { \ - if (s) { \ - utstring_clear(s); \ - } else { \ - utstring_new(s); \ - } \ -} while(0) - -#define utstring_clear(s) \ -do { \ - (s)->i = 0; \ - (s)->d[0] = '\0'; \ -} while(0) - -#define utstring_bincpy(s,b,l) \ -do { \ - utstring_reserve((s),(l)+1); \ - if (l) memcpy(&(s)->d[(s)->i], b, l); \ - (s)->i += l; \ - (s)->d[(s)->i]='\0'; \ -} while(0) - -#define utstring_concat(dst,src) \ -do { \ - utstring_reserve((dst),((src)->i)+1); \ - if ((src)->i) memcpy(&(dst)->d[(dst)->i], (src)->d, (src)->i); \ - (dst)->i += (src)->i; \ - (dst)->d[(dst)->i]='\0'; \ -} while(0) - -#define utstring_len(s) ((unsigned)((s)->i)) - -#define utstring_body(s) ((s)->d) - -_UNUSED_ static void utstring_printf_va(UT_string *s, const char *fmt, va_list ap) { - int n; - va_list cp; - while (1) { -#ifdef _WIN32 - cp = ap; -#else - va_copy(cp, ap); -#endif - n = vsnprintf (&s->d[s->i], s->n-s->i, fmt, cp); - va_end(cp); - - if ((n > -1) && (n < (int)(s->n-s->i))) { - s->i += n; - return; - } - - /* Else try again with more space. */ - if (n > -1) utstring_reserve(s,n+1); /* exact */ - else utstring_reserve(s,(s->n)*2); /* 2x */ - } -} -_UNUSED_ static void utstring_printf(UT_string *s, const char *fmt, ...) { - va_list ap; - va_start(ap,fmt); - utstring_printf_va(s,fmt,ap); - va_end(ap); -} - -#endif /* UTSTRING_H */ diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 981fce9033..daf3aa0126 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -60,7 +60,7 @@ #include #include #include -#include +#include using namespace time_literals; @@ -103,7 +103,7 @@ private: }; uORB::Publication _airspeed_validated_pub {ORB_ID(airspeed_validated)}; /**< airspeed validated topic*/ - uORB::PublicationMulti _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {{ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */ + uORB::PublicationMulti _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {{ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */ orb_advert_t _mavlink_log_pub {nullptr}; /**< mavlink log topic*/ uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -129,7 +129,7 @@ private: vtol_vehicle_status_s _vtol_vehicle_status {}; WindEstimator _wind_estimator_sideslip; /**< wind estimator instance only fusing sideslip */ - wind_estimate_s _wind_estimate_sideslip {}; /**< wind estimate message for wind estimator instance only fusing sideslip */ + airspeed_wind_s _wind_estimate_sideslip {}; /**< wind estimate message for wind estimator instance only fusing sideslip */ int32_t _number_of_airspeed_sensors{0}; /**< number of airspeed sensors in use (detected during initialization)*/ int32_t _prev_number_of_airspeed_sensors{0}; /**< number of airspeed sensors in previous loop (to detect a new added sensor)*/ @@ -489,7 +489,7 @@ void AirspeedModule::update_wind_estimator_sideslip() _wind_estimate_sideslip.beta_innov = _wind_estimator_sideslip.get_beta_innov(); _wind_estimate_sideslip.beta_innov_var = _wind_estimator_sideslip.get_beta_innov_var(); _wind_estimate_sideslip.tas_scale = _wind_estimator_sideslip.get_tas_scale(); - _wind_estimate_sideslip.source = wind_estimate_s::SOURCE_AS_BETA_ONLY; + _wind_estimate_sideslip.source = airspeed_wind_s::SOURCE_AS_BETA_ONLY; } void AirspeedModule::update_ground_minus_wind_airspeed() @@ -602,16 +602,16 @@ void AirspeedModule::select_airspeed_and_publish() // publish the wind estimator states from all airspeed validators for (int i = 0; i < _number_of_airspeed_sensors; i++) { - wind_estimate_s wind_est = _airspeed_validator[i].get_wind_estimator_states(_time_now_usec); + airspeed_wind_s wind_est = _airspeed_validator[i].get_wind_estimator_states(_time_now_usec); if (i == 0) { - wind_est.source = wind_estimate_s::SOURCE_AS_SENSOR_1; + wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_1; } else if (i == 1) { - wind_est.source = wind_estimate_s::SOURCE_AS_SENSOR_2; + wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_2; } else { - wind_est.source = wind_estimate_s::SOURCE_AS_SENSOR_3; + wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_3; } _wind_est_pub[i + 1].publish(wind_est); diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp index aa5d2ed8bd..06e2f4afab 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp @@ -242,7 +242,7 @@ bool PreFlightCheck::ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bo // check accelerometer bias estimates if (bias.accel_bias_valid) { - const float ekf_ab_test_limit = 0.5f * bias.accel_bias_limit; + const float ekf_ab_test_limit = 0.75f * bias.accel_bias_limit; for (uint8_t axis_index = 0; axis_index < 3; axis_index++) { // allow for higher uncertainty in estimates for axes that are less observable to prevent false positives @@ -263,7 +263,7 @@ bool PreFlightCheck::ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bo // check gyro bias estimates if (bias.gyro_bias_valid) { - const float ekf_gb_test_limit = 0.5f * bias.gyro_bias_limit; + const float ekf_gb_test_limit = 0.75f * bias.gyro_bias_limit; for (uint8_t axis_index = 0; axis_index < 3; axis_index++) { // allow for higher uncertainty in estimates for axes that are less observable to prevent false positives diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp index d0a16fda86..f725654966 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp @@ -94,7 +94,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st } } - if (arm_requirements.global_position) { + if (arm_requirements.global_position && !status_flags.circuit_breaker_engaged_posfailure_check) { if (!status_flags.condition_global_position_valid) { if (prearm_ok) { diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 69cf6e66f1..38797b8545 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1258,6 +1258,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL: case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_MODE: case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_ZOOM: + case vehicle_command_s::VEHICLE_CMD_SET_CAMERA_FOCUS: case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED: case vehicle_command_s::VEHICLE_CMD_DO_LAND_START: case vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND: @@ -1944,11 +1945,12 @@ Commander::run() } /* start mission result check */ - const auto prev_mission_instance_count = _mission_result_sub.get().instance_count; - - if (_mission_result_sub.update()) { + if (_mission_result_sub.updated()) { const mission_result_s &mission_result = _mission_result_sub.get(); + const auto prev_mission_instance_count = mission_result.instance_count; + _mission_result_sub.update(); + // if mission_result is valid for the current mission const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp) && (mission_result.instance_count > 0); @@ -1956,7 +1958,6 @@ Commander::run() _status_flags.condition_auto_mission_available = mission_result_ok && mission_result.valid; if (mission_result_ok) { - if (_status.mission_failure != mission_result.failure) { _status.mission_failure = mission_result.failure; _status_changed = true; @@ -1984,6 +1985,20 @@ Commander::run() } } } + + // Transition main state to loiter or auto-mission after takeoff is completed. + if (_armed.armed && !_land_detector.landed + && (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF) + && (mission_result.timestamp >= _status.nav_state_timestamp) + && mission_result.finished) { + + if ((_param_takeoff_finished_action.get() == 1) && _status_flags.condition_auto_mission_available) { + main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, &_internal_state); + + } else { + main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state); + } + } } // update manual_control_setpoint before geofence (which might check sticks or switches) @@ -2381,25 +2396,6 @@ Commander::run() } } - /* Reset main state to loiter or auto-mission after takeoff is completed. - * Sometimes, the mission result topic is outdated and the mission is still signaled - * as finished even though we only just started with the takeoff. Therefore, we also - * check the timestamp of the mission_result topic. */ - if (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF - && (_mission_result_sub.get().timestamp >= _internal_state.timestamp) - && _mission_result_sub.get().finished) { - - const bool mission_available = (_mission_result_sub.get().timestamp > _boot_timestamp) - && (_mission_result_sub.get().instance_count > 0) && _mission_result_sub.get().valid; - - if ((_param_takeoff_finished_action.get() == 1) && mission_available) { - main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, &_internal_state); - - } else { - main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state); - } - } - /* check if we are disarmed and there is a better mode to wait in */ if (!_armed.armed) { /* if there is no radio control but GPS lock the user might want to fly using diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index bf4babe9ee..f0b4eb1605 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -307,7 +307,8 @@ PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f); * The default allows to arm the vehicle without GPS signal. * * @group Commander - * @boolean + * @value 0 Allow arming without GPS + * @value 1 Require GPS lock to arm */ PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 1); @@ -685,7 +686,7 @@ PARAM_DEFINE_INT32(COM_POSCTL_NAVL, 0); /** * Require arm authorization to arm * - * The default allows to arm the vehicle without a arm authorization. + * By default off. The default allows to arm the vehicle without a arm authorization. * * @group Commander * @boolean diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 9d34954b20..2cfdc6f689 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -55,6 +55,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _local_position_pub(multi_mode ? ORB_ID(estimator_local_position) : ORB_ID(vehicle_local_position)), _global_position_pub(multi_mode ? ORB_ID(estimator_global_position) : ORB_ID(vehicle_global_position)), _odometry_pub(multi_mode ? ORB_ID(estimator_odometry) : ORB_ID(vehicle_odometry)), + _wind_pub(multi_mode ? ORB_ID(estimator_wind) : ORB_ID(wind)), _params(_ekf.getParamHandle()), _param_ekf2_min_obs_dt(_params->sensor_interval_min_ms), _param_ekf2_mag_delay(_params->mag_delay_ms), @@ -707,13 +708,11 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) lpos.heading = Eulerf(_ekf.getQuaternion()).psi(); lpos.delta_heading = Eulerf(delta_q_reset).psi(); - lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid(); - - float terrain_vpos = _ekf.getTerrainVertPos(); - // Distance to bottom surface (ground) in meters // constrain the distance to ground to _rng_gnd_clearance - lpos.dist_bottom = math::max(terrain_vpos - lpos.z, _param_ekf2_min_rng.get()); + lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, _param_ekf2_min_rng.get()); + lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid(); + lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield(); if (!_had_valid_terrain) { _had_valid_terrain = lpos.dist_bottom_valid; @@ -966,6 +965,13 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) _ekf.get_ekf_soln_status(&status.solution_status_flags); _ekf.getImuVibrationMetrics().copyTo(status.vibe); + // reset counters + status.reset_count_vel_ne = _ekf.state_reset_status().velNE_counter; + status.reset_count_vel_d = _ekf.state_reset_status().velD_counter; + status.reset_count_pos_ne = _ekf.state_reset_status().posNE_counter; + status.reset_count_pod_d = _ekf.state_reset_status().posD_counter; + status.reset_count_quat = _ekf.state_reset_status().quat_counter; + status.time_slip = _last_time_slip_us * 1e-6f; status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed(); @@ -1107,25 +1113,23 @@ void EKF2::PublishWindEstimate(const hrt_abstime ×tamp) { if (_ekf.get_wind_status()) { // Publish wind estimate only if ekf declares them valid - wind_estimate_s wind_estimate{}; - wind_estimate.timestamp_sample = timestamp; + wind_s wind{}; + wind.timestamp_sample = timestamp; const Vector2f wind_vel = _ekf.getWindVelocity(); const Vector2f wind_vel_var = _ekf.getWindVelocityVariance(); - _ekf.getAirspeedInnov(wind_estimate.tas_innov); - _ekf.getAirspeedInnovVar(wind_estimate.tas_innov_var); - _ekf.getBetaInnov(wind_estimate.beta_innov); - _ekf.getBetaInnovVar(wind_estimate.beta_innov_var); + _ekf.getAirspeedInnov(wind.tas_innov); + _ekf.getAirspeedInnovVar(wind.tas_innov_var); + _ekf.getBetaInnov(wind.beta_innov); + _ekf.getBetaInnovVar(wind.beta_innov_var); - wind_estimate.windspeed_north = wind_vel(0); - wind_estimate.windspeed_east = wind_vel(1); - wind_estimate.variance_north = wind_vel_var(0); - wind_estimate.variance_east = wind_vel_var(1); - wind_estimate.tas_scale = 0.0f; //leave at 0 as scale is not estimated in ekf - wind_estimate.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); - wind_estimate.source = wind_estimate_s::SOURCE_EKF; + wind.windspeed_north = wind_vel(0); + wind.windspeed_east = wind_vel(1); + wind.variance_north = wind_vel_var(0); + wind.variance_east = wind_vel_var(1); + wind.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); - _wind_pub.publish(wind_estimate); + _wind_pub.publish(wind); } } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 42a88e4590..1952630a09 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -85,7 +85,7 @@ #include #include #include -#include +#include #include #include "Utility/PreFlightChecker.hpp" @@ -254,13 +254,14 @@ private: uORB::PublicationMulti _estimator_status_flags_pub{ORB_ID(estimator_status_flags)}; uORB::PublicationMulti _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)}; uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; - uORB::PublicationMulti _wind_pub{ORB_ID(wind_estimate)}; // publications with topic dependent on multi-mode uORB::PublicationMulti _attitude_pub; uORB::PublicationMulti _local_position_pub; uORB::PublicationMulti _global_position_pub; uORB::PublicationMulti _odometry_pub; + uORB::PublicationMulti _wind_pub; + PreFlightChecker _preflt_checker; diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index c79616bd0e..898dbc87f5 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -122,6 +122,7 @@ bool EKF2Selector::SelectInstance(uint8_t ekf_instance) PublishVehicleAttitude(true); PublishVehicleLocalPosition(true); PublishVehicleGlobalPosition(true); + PublishWindEstimate(true); return true; } @@ -498,6 +499,25 @@ void EKF2Selector::PublishVehicleGlobalPosition(bool reset) } } +void EKF2Selector::PublishWindEstimate(bool reset) +{ + wind_s wind; + + if (_instance[_selected_instance].estimator_wind_sub.copy(&wind)) { + if (reset) { + // ensure monotonically increasing timestamp_sample through reset + wind.timestamp_sample = max(wind.timestamp_sample, _wind_last.timestamp_sample); + } + + // save last primary wind + _wind_last = wind; + + // republish with current timestamp + wind.timestamp = hrt_absolute_time(); + _wind_pub.publish(wind); + } +} + void EKF2Selector::Run() { // re-schedule as backup timeout @@ -651,6 +671,11 @@ void EKF2Selector::Run() PublishVehicleGlobalPosition(); } + // selected estimator_wind -> wind + if (_instance[_selected_instance].estimator_wind_sub.updated()) { + PublishWindEstimate(); + } + // selected estimator_odometry -> vehicle_odometry if (_instance[_selected_instance].estimator_odometry_sub.updated()) { vehicle_odometry_s vehicle_odometry; diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp index 38cce2859e..fc63297f36 100644 --- a/src/modules/ekf2/EKF2Selector.hpp +++ b/src/modules/ekf2/EKF2Selector.hpp @@ -52,6 +52,7 @@ #include #include #include +#include #if CONSTRAINED_MEMORY # define EKF2_MAX_INSTANCES 2 @@ -80,6 +81,7 @@ private: void PublishVehicleAttitude(bool reset = false); void PublishVehicleLocalPosition(bool reset = false); void PublishVehicleGlobalPosition(bool reset = false); + void PublishWindEstimate(bool reset = false); bool SelectInstance(uint8_t instance); // Update the error scores for all available instances @@ -94,6 +96,7 @@ private: estimator_local_position_sub{ORB_ID(estimator_local_position), i}, estimator_global_position_sub{ORB_ID(estimator_global_position), i}, estimator_odometry_sub{ORB_ID(estimator_odometry), i}, + estimator_wind_sub{ORB_ID(estimator_wind), i}, instance(i) {} @@ -103,6 +106,7 @@ private: uORB::Subscription estimator_local_position_sub; uORB::Subscription estimator_global_position_sub; uORB::Subscription estimator_odometry_sub; + uORB::Subscription estimator_wind_sub; uint64_t timestamp_sample_last{0}; @@ -195,6 +199,9 @@ private: uint8_t _lat_lon_reset_counter{0}; uint8_t _alt_reset_counter{0}; + // wind estimate + wind_s _wind_last{}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _sensors_status_imu{ORB_ID(sensors_status_imu)}; @@ -205,6 +212,7 @@ private: uORB::Publication _vehicle_global_position_pub{ORB_ID(vehicle_global_position)}; uORB::Publication _vehicle_local_position_pub{ORB_ID(vehicle_local_position)}; uORB::Publication _vehicle_odometry_pub{ORB_ID(vehicle_odometry)}; + uORB::Publication _wind_pub{ORB_ID(wind)}; DEFINE_PARAMETERS( (ParamFloat) _param_ekf2_sel_err_red, diff --git a/src/modules/events/CMakeLists.txt b/src/modules/events/CMakeLists.txt index a886cd5f4f..8c54833d4e 100644 --- a/src/modules/events/CMakeLists.txt +++ b/src/modules/events/CMakeLists.txt @@ -40,6 +40,4 @@ px4_add_module( send_event.cpp set_leds.cpp status_display.cpp - DEPENDS - modules__uORB ) diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index a0a6fa4118..29fe3e37ac 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -62,6 +62,7 @@ void LoggedTopics::add_default_topics() add_topic("cpuload"); add_topic("esc_status", 250); add_topic("generator_status"); + add_topic("heater_status"); add_topic("home_position"); add_topic("hover_thrust_estimate", 100); add_topic("input_rc", 500); @@ -112,6 +113,7 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_status"); add_topic("vehicle_status_flags"); add_topic("vtol_vehicle_status", 200); + add_topic("wind", 1000); // Control allocation topics add_topic("vehicle_angular_acceleration_setpoint", 20); @@ -122,6 +124,7 @@ void LoggedTopics::add_default_topics() // multi topics add_topic_multi("actuator_outputs", 100, 3); + add_topic_multi("airspeed_wind", 1000); add_topic_multi("logger_status", 0, 2); add_topic_multi("multirotor_motor_limits", 1000, 2); add_topic_multi("rate_ctrl_status", 200, 2); @@ -148,8 +151,8 @@ void LoggedTopics::add_default_topics() add_topic_multi("estimator_status", 200, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_visual_odometry_aligned", 200, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_wind", 1000, MAX_ESTIMATOR_INSTANCES); add_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("wind_estimate", 1000); // published by both ekf2 and airspeed_selector // log all raw sensors at minimal rate (at least 1 Hz) add_topic_multi("battery_status", 300, 2); diff --git a/src/modules/logger/params.c b/src/modules/logger/params.c index 0cbfb2c749..8220aa8fcf 100644 --- a/src/modules/logger/params.c +++ b/src/modules/logger/params.c @@ -108,8 +108,8 @@ PARAM_DEFINE_INT32(SDLOG_MISSION, 0); * Logging topic profile (integer bitmask). * * This integer bitmask controls the set and rates of logged topics. - * The default allows for general log analysis and estimator replay, while - * keeping the log file size reasonably small. + * The default allows for general log analysis while keeping the + * log file size reasonably small. * * Enabling multiple sets leads to higher bandwidth requirements and larger log * files. diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index d9c1a66958..e4811f5628 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -55,6 +55,12 @@ #define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer #define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status +#if !defined(CONSTRAINED_MEMORY) +# define MAVLINK_COMM_NUM_BUFFERS 6 +# define MAVLINK_COMM_4 static_cast(4) +# define MAVLINK_COMM_5 static_cast(5) +#endif + #include #include diff --git a/src/modules/mavlink/mavlink_command_sender.cpp b/src/modules/mavlink/mavlink_command_sender.cpp index b076115e8e..85578b1335 100644 --- a/src/modules/mavlink/mavlink_command_sender.cpp +++ b/src/modules/mavlink/mavlink_command_sender.cpp @@ -151,7 +151,7 @@ void MavlinkCommandSender::check_timeout(mavlink_channel_t channel) // as some channels might be lagging behind and will end up putting the same command into the buffer. bool dropped_command = false; - for (unsigned i = 0; i < MAX_MAVLINK_CHANNEL; ++i) { + for (unsigned i = 0; i < MAVLINK_COMM_NUM_BUFFERS; ++i) { if (item->num_sent_per_channel[i] == -2) { _commands.drop_current(); dropped_command = true; @@ -176,7 +176,7 @@ void MavlinkCommandSender::check_timeout(mavlink_channel_t channel) int8_t max_sent = 0; int8_t min_sent = INT8_MAX; - for (unsigned i = 0; i < MAX_MAVLINK_CHANNEL; ++i) { + for (unsigned i = 0; i < MAVLINK_COMM_NUM_BUFFERS; ++i) { if (item->num_sent_per_channel[i] > max_sent) { max_sent = item->num_sent_per_channel[i]; } diff --git a/src/modules/mavlink/mavlink_command_sender.h b/src/modules/mavlink/mavlink_command_sender.h index 2454055802..9d5eb82bee 100644 --- a/src/modules/mavlink/mavlink_command_sender.h +++ b/src/modules/mavlink/mavlink_command_sender.h @@ -49,7 +49,6 @@ #include "timestamped_list.h" #include "mavlink_bridge_header.h" -#include /** * @class MavlinkCommandSender @@ -102,14 +101,18 @@ private: static MavlinkCommandSender *_instance; static px4_sem_t _lock; - // There are MAVLINK_COMM_0 to MAVLINK_COMM_3, so it should be 4. - static const unsigned MAX_MAVLINK_CHANNEL = 4; - struct command_item_s { mavlink_command_long_t command = {}; hrt_abstime timestamp_us = 0; hrt_abstime last_time_sent_us = 0; - int8_t num_sent_per_channel[MAX_MAVLINK_CHANNEL] = {-1, -1, -1, -1}; // -1: channel did not request this command to be sent, -2: channel got an ack for this command + // -1: channel did not request this command to be sent, -2: channel got an ack for this command +#if MAVLINK_COMM_NUM_BUFFERS == 4 + int8_t num_sent_per_channel[MAVLINK_COMM_NUM_BUFFERS] {-1, -1, -1, -1}; +#elif MAVLINK_COMM_NUM_BUFFERS == 6 + int8_t num_sent_per_channel[MAVLINK_COMM_NUM_BUFFERS] {-1, -1, -1, -1, -1, -1}; +#else +# error Unknown number of MAVLINK_COMM_NUM_BUFFERS +#endif }; TimestampedList _commands{3}; diff --git a/src/modules/mavlink/mavlink_log_handler.h b/src/modules/mavlink/mavlink_log_handler.h index dc4843eee4..8993bf4100 100644 --- a/src/modules/mavlink/mavlink_log_handler.h +++ b/src/modules/mavlink/mavlink_log_handler.h @@ -41,9 +41,10 @@ #include #include #include -#include #include +#include "mavlink_bridge_header.h" + class Mavlink; // MAVLink LOG_* Message Handler diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index a8fbf7454f..049bb7a249 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -48,6 +48,7 @@ #include #endif +#include #include #include #include @@ -76,84 +77,15 @@ #define MAX_DATA_RATE 10000000 ///< max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate -static Mavlink *_mavlink_instances = nullptr; +static pthread_mutex_t mavlink_module_mutex = PTHREAD_MUTEX_INITIALIZER; -/** - * Mavlink app start / stop handling function. - * - * @ingroup apps - */ -extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); +Mavlink *mavlink_module_instances[MAVLINK_COMM_NUM_BUFFERS] {}; -void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length) -{ - Mavlink *m = Mavlink::get_instance(chan); - - if (m != nullptr) { - m->send_bytes(ch, length); -#ifdef MAVLINK_PRINT_PACKETS - - for (unsigned i = 0; i < length; i++) { - printf("%02x", (unsigned char)ch[i]); - } - -#endif - } -} - -void mavlink_start_uart_send(mavlink_channel_t chan, int length) -{ - Mavlink *m = Mavlink::get_instance(chan); - - if (m != nullptr) { - m->send_start(length); -#ifdef MAVLINK_PRINT_PACKETS - printf("START PACKET (%u): ", (unsigned)chan); -#endif - } -} - -void mavlink_end_uart_send(mavlink_channel_t chan, int length) -{ - Mavlink *m = Mavlink::get_instance(chan); - - if (m != nullptr) { - m->send_finish(); -#ifdef MAVLINK_PRINT_PACKETS - printf("\n"); -#endif - } -} - -/* - * Internal function to give access to the channel status for each channel - */ -mavlink_status_t *mavlink_get_channel_status(uint8_t channel) -{ - Mavlink *m = Mavlink::get_instance(channel); - - if (m != nullptr) { - return m->get_status(); - - } else { - return nullptr; - } -} - -/* - * Internal function to give access to the channel buffer for each channel - */ -mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel) -{ - Mavlink *m = Mavlink::get_instance(channel); - - if (m != nullptr) { - return m->get_buffer(); - - } else { - return nullptr; - } -} +void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length) { mavlink_module_instances[chan]->send_bytes(ch, length); } +void mavlink_start_uart_send(mavlink_channel_t chan, int length) { mavlink_module_instances[chan]->send_start(length); } +void mavlink_end_uart_send(mavlink_channel_t chan, int length) { mavlink_module_instances[chan]->send_finish(); } +mavlink_status_t *mavlink_get_channel_status(uint8_t channel) { return mavlink_module_instances[channel]->get_status(); } +mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel) { return mavlink_module_instances[channel]->get_buffer(); } static void usage(); @@ -278,16 +210,26 @@ Mavlink::set_channel() #endif default: - PX4_WARN("instance ID is out of range"); + PX4_WARN("instance ID %d is out of range", _instance_id); px4_task_exit(1); break; } } -void +bool Mavlink::set_instance_id() { - _instance_id = Mavlink::instance_count(); + LockGuard lg{mavlink_module_mutex}; + + for (int instance_id = 0; instance_id < MAVLINK_COMM_NUM_BUFFERS; instance_id++) { + if (mavlink_module_instances[instance_id] == nullptr) { + mavlink_module_instances[instance_id] = this; + _instance_id = instance_id; + return true; + } + } + + return false; } void @@ -308,36 +250,25 @@ Mavlink::set_proto_version(unsigned version) int Mavlink::instance_count() { + LockGuard lg{mavlink_module_mutex}; size_t inst_index = 0; - Mavlink *inst; - LL_FOREACH(::_mavlink_instances, inst) { - inst_index++; + for (Mavlink *inst : mavlink_module_instances) { + if (inst != nullptr) { + inst_index++; + } } return inst_index; } -Mavlink * -Mavlink::get_instance(int instance) -{ - Mavlink *inst; - LL_FOREACH(::_mavlink_instances, inst) { - if (instance == inst->get_instance_id()) { - return inst; - } - } - - return nullptr; -} - Mavlink * Mavlink::get_instance_for_device(const char *device_name) { - Mavlink *inst; + LockGuard lg{mavlink_module_mutex}; - LL_FOREACH(::_mavlink_instances, inst) { - if ((inst->_protocol == Protocol::SERIAL) && (strcmp(inst->_device_name, device_name) == 0)) { + for (Mavlink *inst : mavlink_module_instances) { + if (inst && (inst->_protocol == Protocol::SERIAL) && (strcmp(inst->_device_name, device_name) == 0)) { return inst; } } @@ -349,10 +280,10 @@ Mavlink::get_instance_for_device(const char *device_name) Mavlink * Mavlink::get_instance_for_network_port(unsigned long port) { - Mavlink *inst; + LockGuard lg{mavlink_module_mutex}; - LL_FOREACH(::_mavlink_instances, inst) { - if ((inst->_protocol == Protocol::UDP) && (inst->_network_port == port)) { + for (Mavlink *inst : mavlink_module_instances) { + if (inst && (inst->_protocol == Protocol::UDP) && (inst->_network_port == port)) { return inst; } } @@ -364,40 +295,36 @@ Mavlink::get_instance_for_network_port(unsigned long port) int Mavlink::destroy_all_instances() { - /* start deleting from the end */ - Mavlink *inst_to_del = nullptr; - Mavlink *next_inst = ::_mavlink_instances; - + LockGuard lg{mavlink_module_mutex}; unsigned iterations = 0; PX4_INFO("waiting for instances to stop"); - while (next_inst != nullptr) { - inst_to_del = next_inst; - next_inst = inst_to_del->next; + for (Mavlink *inst_to_del : mavlink_module_instances) { + if (inst_to_del != nullptr) { + /* set flag to stop thread and wait for all threads to finish */ + inst_to_del->_task_should_exit = true; - /* set flag to stop thread and wait for all threads to finish */ - inst_to_del->_task_should_exit = true; + while (inst_to_del->_task_running) { + printf("."); + fflush(stdout); + px4_usleep(10000); + iterations++; - while (inst_to_del->_task_running) { - printf("."); - fflush(stdout); - px4_usleep(10000); - iterations++; - - if (iterations > 1000) { - PX4_ERR("Couldn't stop all mavlink instances."); - return PX4_ERROR; + if (iterations > 1000) { + PX4_ERR("Couldn't stop all mavlink instances."); + return PX4_ERROR; + } } } - } //we know all threads have exited, so it's safe to manipulate the linked list and delete objects. - while (_mavlink_instances) { - inst_to_del = _mavlink_instances; - LL_DELETE(_mavlink_instances, inst_to_del); - delete inst_to_del; + for (Mavlink *inst_to_del : mavlink_module_instances) { + if (inst_to_del != nullptr) { + delete inst_to_del; + inst_to_del = nullptr; + } } printf("\n"); @@ -408,24 +335,22 @@ Mavlink::destroy_all_instances() int Mavlink::get_status_all_instances(bool show_streams_status) { - Mavlink *inst = ::_mavlink_instances; - + LockGuard lg{mavlink_module_mutex}; unsigned iterations = 0; - while (inst != nullptr) { + for (Mavlink *inst : mavlink_module_instances) { + if (inst != nullptr) { + printf("\ninstance #%u:\n", iterations); - printf("\ninstance #%u:\n", iterations); + if (show_streams_status) { + inst->display_status_streams(); - if (show_streams_status) { - inst->display_status_streams(); + } else { + inst->display_status(); + } - } else { - inst->display_status(); + iterations++; } - - /* move on */ - inst = inst->next; - iterations++; } /* return an error if there are no instances */ @@ -435,16 +360,13 @@ Mavlink::get_status_all_instances(bool show_streams_status) bool Mavlink::serial_instance_exists(const char *device_name, Mavlink *self) { - Mavlink *inst = ::_mavlink_instances; - - while (inst != nullptr) { + LockGuard lg{mavlink_module_mutex}; + for (Mavlink *inst : mavlink_module_instances) { /* don't compare with itself and with non serial instances*/ - if ((inst != self) && (inst->get_protocol() == Protocol::SERIAL) && !strcmp(device_name, inst->_device_name)) { + if (inst && (inst != self) && (inst->get_protocol() == Protocol::SERIAL) && !strcmp(device_name, inst->_device_name)) { return true; } - - inst = inst->next; } return false; @@ -453,9 +375,10 @@ Mavlink::serial_instance_exists(const char *device_name, Mavlink *self) void Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) { - Mavlink *inst; - LL_FOREACH(_mavlink_instances, inst) { - if (inst != self) { + LockGuard lg{mavlink_module_mutex}; + + for (Mavlink *inst : mavlink_module_instances) { + if (inst && (inst != self)) { const mavlink_msg_entry_t *meta = mavlink_get_msg_entry(msg->msgid); int target_system_id = 0; @@ -1129,6 +1052,11 @@ Mavlink::handle_message(const mavlink_message_t *msg) /* forward any messages to other mavlink instances */ Mavlink::forward_message(msg, this); } + + // Special case for gimbals that need to forward GIMBAL_DEVICE_ATTITUDE_STATUS. + else if (msg->msgid == MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS) { + Mavlink::forward_message(msg, this); + } } void @@ -1548,6 +1476,9 @@ Mavlink::update_radio_status(const radio_status_s &radio_status) /* this indicates spare bandwidth, increase by 2.5% */ _radio_status_mult *= 1.025f; } + + /* Constrain radio status multiplier between 1% and 100% to allow recovery */ + _radio_status_mult = math::constrain(_radio_status_mult, 0.01f, 1.0f); } } @@ -1587,6 +1518,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("ESTIMATOR_STATUS", 0.5f); configure_stream_local("EXTENDED_SYS_STATE", 1.0f); configure_stream_local("GLOBAL_POSITION_INT", 5.0f); + configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 1.0f); + configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f); + configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f); configure_stream_local("GPS2_RAW", 1.0f); configure_stream_local("GPS_RAW_INT", 1.0f); configure_stream_local("GPS_STATUS", 1.0f); @@ -1642,6 +1576,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("COLLISION", unlimited_rate); configure_stream_local("ESTIMATOR_STATUS", 1.0f); configure_stream_local("EXTENDED_SYS_STATE", 5.0f); + configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 1.0f); + configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f); + configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f); configure_stream_local("GLOBAL_POSITION_INT", 50.0f); configure_stream_local("GPS2_RAW", unlimited_rate); configure_stream_local("GPS_RAW_INT", unlimited_rate); @@ -1674,6 +1611,12 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) break; + case MAVLINK_MODE_GIMBAL: + // Note: streams requiring low latency come first + configure_stream_local("AUTOPILOT_STATE_FOR_GIMBAL_DEVICE", 20.0f); + configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 20.0f); + break; + case MAVLINK_MODE_EXTVISION: configure_stream_local("HIGHRES_IMU", unlimited_rate); // for VIO @@ -2038,6 +1981,9 @@ Mavlink::task_main(int argc, char *argv[]) } else if (strcmp(myoptarg, "extvisionmin") == 0) { _mode = MAVLINK_MODE_EXTVISIONMIN; + } else if (strcmp(myoptarg, "gimbal") == 0) { + _mode = MAVLINK_MODE_GIMBAL; + } else { PX4_ERR("invalid mode"); err_flag = true; @@ -2136,6 +2082,13 @@ Mavlink::task_main(int argc, char *argv[]) #endif // MAVLINK_UDP + if (!set_instance_id()) { + PX4_ERR("no instances available"); + return PX4_ERROR; + } + + set_channel(); + /* initialize send mutex */ pthread_mutex_init(&_send_mutex, nullptr); @@ -2193,13 +2146,6 @@ Mavlink::task_main(int argc, char *argv[]) _main_loop_delay = MAVLINK_MAX_INTERVAL; } - set_instance_id(); - - set_channel(); - - /* now the instance is fully initialized and we can bump the instance count */ - LL_APPEND(_mavlink_instances, this); - /* open the UART device after setting the instance, as it might block */ if (get_protocol() == Protocol::SERIAL) { _uart_fd = mavlink_open_uart(_baudrate, _device_name, _flow_control); @@ -2712,9 +2658,8 @@ Mavlink::start(int argc, char *argv[]) // before returning to the shell int ic = Mavlink::instance_count(); - if (ic == Mavlink::MAVLINK_MAX_INSTANCES) { - PX4_ERR("Maximum MAVLink instance count of %d reached.", - (int)Mavlink::MAVLINK_MAX_INSTANCES); + if (ic == MAVLINK_COMM_NUM_BUFFERS) { + PX4_ERR("Maximum MAVLink instance count of %d reached.", MAVLINK_COMM_NUM_BUFFERS); return 1; } @@ -3012,14 +2957,16 @@ Mavlink::set_boot_complete() _boot_complete = true; #if defined(MAVLINK_UDP) - Mavlink *inst; - LL_FOREACH(::_mavlink_instances, inst) { - if ((inst->get_mode() != MAVLINK_MODE_ONBOARD) && + LockGuard lg {mavlink_module_mutex}; + + for (Mavlink *inst : mavlink_module_instances) { + if (inst && (inst->get_mode() != MAVLINK_MODE_ONBOARD) && !inst->broadcast_enabled() && inst->get_protocol() == Protocol::UDP) { PX4_INFO("MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)"); } } + #endif // MAVLINK_UDP } @@ -3068,7 +3015,7 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50 PRINT_MODULE_USAGE_PARAM_STRING('t', "127.0.0.1", nullptr, "Partner IP (broadcasting can be enabled via MAV_BROADCAST param)", true); #endif - PRINT_MODULE_USAGE_PARAM_STRING('m', "normal", "custom|camera|onboard|osd|magic|config|iridium|minimal|extvsision", + PRINT_MODULE_USAGE_PARAM_STRING('m', "normal", "custom|camera|onboard|osd|magic|config|iridium|minimal|extvision|extvisionmin|gimbal", "Mode: sets default streams and rates", true); PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, "", "wifi/ethernet interface name", true); #if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) @@ -3098,7 +3045,7 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50 } -int mavlink_main(int argc, char *argv[]) +extern "C" __EXPORT int mavlink_main(int argc, char *argv[]) { if (argc < 2) { usage(); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index afa4743eea..4992cd8759 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -68,7 +68,6 @@ #include #include #include -#include #include #include #include @@ -103,7 +102,7 @@ enum class Protocol { using namespace time_literals; -class Mavlink : public ModuleParams +class Mavlink final : public ModuleParams { public: @@ -140,8 +139,6 @@ public: static Mavlink *new_instance(); - static Mavlink *get_instance(int instance); - static Mavlink *get_instance_for_device(const char *device_name); mavlink_message_t *get_buffer() { return &_mavlink_buffer; } @@ -194,7 +191,7 @@ public: MAVLINK_MODE_MINIMAL, MAVLINK_MODE_EXTVISION, MAVLINK_MODE_EXTVISIONMIN, - + MAVLINK_MODE_GIMBAL, MAVLINK_MODE_COUNT }; @@ -243,6 +240,9 @@ public: case MAVLINK_MODE_EXTVISIONMIN: return "ExtVisionMin"; + case MAVLINK_MODE_GIMBAL: + return "Gimbal"; + default: return "Unknown"; } @@ -518,8 +518,7 @@ public: static hrt_abstime &get_first_start_time() { return _first_start_time; } -protected: - Mavlink *next{nullptr}; + bool radio_status_critical() const { return _radio_status_critical; } private: int _instance_id{0}; @@ -540,7 +539,7 @@ private: bool _task_running{true}; static bool _boot_complete; - static constexpr int MAVLINK_MAX_INSTANCES{4}; + static constexpr int MAVLINK_MIN_INTERVAL{1500}; static constexpr int MAVLINK_MAX_INTERVAL{10000}; static constexpr float MAVLINK_MIN_MULTIPLIER{0.0005f}; @@ -743,7 +742,7 @@ private: void set_channel(); - void set_instance_id(); + bool set_instance_id(); /** * Main mavlink task. diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 170b83868f..904979ee30 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -69,6 +69,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include @@ -1478,6 +1482,112 @@ protected: } }; +#if !defined(CONSTRAINED_FLASH) +class MavlinkStreamAutopilotStateForGimbalDevice : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamAutopilotStateForGimbalDevice::get_name_static(); + } + + static constexpr const char *get_name_static() + { + return "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE"; + } + + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE; + } + + uint16_t get_id() override + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamAutopilotStateForGimbalDevice(mavlink); + } + + unsigned get_size() override + { + return _att_sub.advertised() ? MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _est_sub{ORB_ID(estimator_status)}; + uORB::Subscription _landed_sub{ORB_ID(vehicle_land_detected)}; + + /* do not allow top copying this class */ + MavlinkStreamAutopilotStateForGimbalDevice(MavlinkStreamAutopilotStateForGimbalDevice &) = delete; + MavlinkStreamAutopilotStateForGimbalDevice &operator = (const MavlinkStreamAutopilotStateForGimbalDevice &) = delete; + +protected: + explicit MavlinkStreamAutopilotStateForGimbalDevice(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send() override + { + if (_att_sub.advertised()) { + + mavlink_autopilot_state_for_gimbal_device_t msg{}; + + { + vehicle_attitude_s att{}; + _att_sub.copy(&att); + msg.time_boot_us = att.timestamp; + msg.q[0] = att.q[0]; + msg.q[1] = att.q[1]; + msg.q[2] = att.q[2]; + msg.q[3] = att.q[3]; + msg.q_estimated_delay_us = 0; // I don't know. + } + + { + vehicle_local_position_s lpos{}; + _lpos_sub.copy(&lpos); + msg.vx = lpos.vx; + msg.vy = lpos.vy; + msg.vz = lpos.vz; + msg.v_estimated_delay_us = 0; // I don't know. + } + + { + vehicle_attitude_setpoint_s att_sp{}; + _att_sp_sub.copy(&att_sp); + msg.feed_forward_angular_velocity_z = att_sp.yaw_sp_move_rate; + } + + { + estimator_status_s est{}; + _est_sub.copy(&est); + msg.estimator_status = est.solution_status_flags; + } + + { + vehicle_land_detected_s land_detected{}; + _landed_sub.copy(&land_detected); + + // Ignore take-off and landing states for now. + msg.landed_state = land_detected.landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR; + } + + mavlink_msg_autopilot_state_for_gimbal_device_send_struct(_mavlink->get_channel(), &msg); + + return true; + + } + + return false; + } +}; +#endif + class MavlinkStreamSystemTime : public MavlinkStream { public: @@ -1867,6 +1977,298 @@ protected: } }; +#if !defined(CONSTRAINED_FLASH) +class MavlinkStreamGimbalDeviceAttitudeStatus : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamGimbalDeviceAttitudeStatus::get_name_static(); + } + + static constexpr const char *get_name_static() + { + return "GIMBAL_DEVICE_ATTITUDE_STATUS"; + } + + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS; + } + + uint16_t get_id() override + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGimbalDeviceAttitudeStatus(mavlink); + } + + unsigned get_size() override + { + return _gimbal_device_attitude_status_sub.advertised() ? MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; + + /* do not allow top copying this class */ + MavlinkStreamGimbalDeviceAttitudeStatus(MavlinkStreamGimbalDeviceAttitudeStatus &) = delete; + MavlinkStreamGimbalDeviceAttitudeStatus &operator = (const MavlinkStreamGimbalDeviceAttitudeStatus &) = delete; + +protected: + explicit MavlinkStreamGimbalDeviceAttitudeStatus(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send() override + { + gimbal_device_attitude_status_s gimbal_device_attitude_status{}; + + if (_gimbal_device_attitude_status_sub.update(&gimbal_device_attitude_status)) { + mavlink_gimbal_device_attitude_status_t msg{}; + + msg.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000; + msg.q[0] = gimbal_device_attitude_status.q[0]; + msg.q[1] = gimbal_device_attitude_status.q[1]; + msg.q[2] = gimbal_device_attitude_status.q[2]; + msg.q[3] = gimbal_device_attitude_status.q[3]; + msg.angular_velocity_x = gimbal_device_attitude_status.angular_velocity_x; + msg.angular_velocity_y = gimbal_device_attitude_status.angular_velocity_y; + msg.angular_velocity_z = gimbal_device_attitude_status.angular_velocity_z; + msg.failure_flags = gimbal_device_attitude_status.failure_flags; + msg.flags = gimbal_device_attitude_status.device_flags; + + mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + +class MavlinkStreamGimbalManagerInformation : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamGimbalManagerInformation::get_name_static(); + } + + static constexpr const char *get_name_static() + { + return "GIMBAL_MANAGER_INFORMATION"; + } + + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION; + } + + uint16_t get_id() override + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGimbalManagerInformation(mavlink); + } + + unsigned get_size() override + { + return _gimbal_manager_information_sub.advertised() ? (MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + uORB::Subscription _gimbal_manager_information_sub{ORB_ID(gimbal_manager_information)}; + + /* do not allow top copying this class */ + MavlinkStreamGimbalManagerInformation(MavlinkStreamGimbalManagerInformation &) = delete; + MavlinkStreamGimbalManagerInformation &operator = (const MavlinkStreamGimbalManagerInformation &) = delete; + +protected: + explicit MavlinkStreamGimbalManagerInformation(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send() override + { + gimbal_manager_information_s gimbal_manager_information; + + if (_gimbal_manager_information_sub.advertised() && _gimbal_manager_information_sub.copy(&gimbal_manager_information)) { + // send out gimbal_manager_info with info from gimbal_manager_information + mavlink_gimbal_manager_information_t msg{}; + msg.time_boot_ms = gimbal_manager_information.timestamp / 1000; + msg.gimbal_device_id = 0; + msg.cap_flags = gimbal_manager_information.cap_flags; + + msg.roll_min = gimbal_manager_information.roll_min; + msg.roll_max = gimbal_manager_information.roll_max; + msg.pitch_min = gimbal_manager_information.pitch_min; + msg.pitch_max = gimbal_manager_information.pitch_max; + msg.yaw_min = gimbal_manager_information.yaw_min; + msg.yaw_max = gimbal_manager_information.yaw_max; + + mavlink_msg_gimbal_manager_information_send_struct(_mavlink->get_channel(), &msg); + + return true; + + } + + return false; + } +}; + +class MavlinkStreamGimbalManagerStatus : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamGimbalManagerStatus::get_name_static(); + } + + static constexpr const char *get_name_static() + { + return "GIMBAL_MANAGER_STATUS"; + } + + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS; + } + + uint16_t get_id() override + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGimbalManagerStatus(mavlink); + } + + unsigned get_size() override + { + return _gimbal_manager_status_sub.advertised() ? (MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + uORB::Subscription _gimbal_manager_status_sub{ORB_ID(gimbal_manager_status)}; + + /* do not allow top copying this class */ + MavlinkStreamGimbalManagerStatus(MavlinkStreamGimbalManagerStatus &) = delete; + MavlinkStreamGimbalManagerStatus &operator = (const MavlinkStreamGimbalManagerStatus &) = delete; + +protected: + explicit MavlinkStreamGimbalManagerStatus(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send() override + { + gimbal_manager_status_s gimbal_manager_status; + + if (_gimbal_manager_status_sub.advertised() && _gimbal_manager_status_sub.copy(&gimbal_manager_status)) { + mavlink_gimbal_manager_status_t msg{}; + msg.time_boot_ms = gimbal_manager_status.timestamp / 1000; + msg.gimbal_device_id = gimbal_manager_status.gimbal_device_id; + msg.primary_control_sysid = gimbal_manager_status.primary_control_sysid; + msg.primary_control_compid = gimbal_manager_status.primary_control_compid; + msg.secondary_control_sysid = gimbal_manager_status.secondary_control_sysid; + msg.secondary_control_compid = gimbal_manager_status.secondary_control_compid; + msg.flags = gimbal_manager_status.flags; + + mavlink_msg_gimbal_manager_status_send_struct(_mavlink->get_channel(), &msg); + + return true; + + } + + return false; + } +}; + + + +class MavlinkStreamGimbalDeviceSetAttitude : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamGimbalDeviceSetAttitude::get_name_static(); + } + + static constexpr const char *get_name_static() + { + return "GIMBAL_DEVICE_SET_ATTITUDE"; + } + + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE; + } + + uint16_t get_id() override + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamGimbalDeviceSetAttitude(mavlink); + } + + unsigned get_size() override + { + return _gimbal_device_set_attitude_sub.advertised() ? (MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + } + +private: + uORB::Subscription _gimbal_device_set_attitude_sub{ORB_ID(gimbal_device_set_attitude)}; + + /* do not allow top copying this class */ + MavlinkStreamGimbalDeviceSetAttitude(MavlinkStreamGimbalDeviceSetAttitude &) = delete; + MavlinkStreamGimbalDeviceSetAttitude &operator = (const MavlinkStreamGimbalDeviceSetAttitude &) = delete; + +protected: + explicit MavlinkStreamGimbalDeviceSetAttitude(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send() override + { + gimbal_device_set_attitude_s gimbal_device_set_attitude; + + if (_gimbal_device_set_attitude_sub.advertised() && _gimbal_device_set_attitude_sub.copy(&gimbal_device_set_attitude)) { + mavlink_gimbal_device_set_attitude_t msg{}; + msg.flags = gimbal_device_set_attitude.flags; + msg.target_system = gimbal_device_set_attitude.target_system; + msg.target_component = gimbal_device_set_attitude.target_component; + + msg.q[0] = gimbal_device_set_attitude.q[0]; + msg.q[1] = gimbal_device_set_attitude.q[1]; + msg.q[2] = gimbal_device_set_attitude.q[2]; + msg.q[3] = gimbal_device_set_attitude.q[3]; + + msg.angular_velocity_x = gimbal_device_set_attitude.angular_velocity_x; + msg.angular_velocity_y = gimbal_device_set_attitude.angular_velocity_y; + msg.angular_velocity_z = gimbal_device_set_attitude.angular_velocity_z; + + mavlink_msg_gimbal_device_set_attitude_send_struct(_mavlink->get_channel(), &msg); + + return true; + + } + + return false; + } +}; +#endif + class MavlinkStreamCameraTrigger : public MavlinkStream { public: @@ -3046,6 +3448,13 @@ static const StreamListItem streams_list[] = { create_stream_list_item(), create_stream_list_item(), create_stream_list_item(), +#if !defined(CONSTRAINED_FLASH) + create_stream_list_item(), + create_stream_list_item(), + create_stream_list_item(), + create_stream_list_item(), + create_stream_list_item(), +#endif create_stream_list_item(), create_stream_list_item >(), create_stream_list_item >(), diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 5d50dc2c29..5c038f0c50 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1338,16 +1338,8 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * break; case MAV_CMD_NAV_TAKEOFF: - - // reject takeoff item if minimum pitch (parameter 1) is set - if (PX4_ISFINITE(mavlink_mission_item->param1) && (fabsf(mavlink_mission_item->param1) > FLT_EPSILON)) { - _mavlink->send_statustext_critical("Takeoff rejected, remove deprecated minimum pitch"); - return MAV_MISSION_INVALID_PARAM1; - - } else { - mission_item->nav_cmd = NAV_CMD_TAKEOFF; - mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); - } + mission_item->nav_cmd = NAV_CMD_TAKEOFF; + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); break; @@ -1475,6 +1467,9 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * case MAV_CMD_DO_DIGICAM_CONTROL: case MAV_CMD_DO_MOUNT_CONFIGURE: case MAV_CMD_DO_MOUNT_CONTROL: + case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + case NAV_CMD_SET_CAMERA_FOCUS: + case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: case MAV_CMD_IMAGE_START_CAPTURE: case MAV_CMD_IMAGE_STOP_CAPTURE: case MAV_CMD_VIDEO_START_CAPTURE: @@ -1574,12 +1569,15 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * case NAV_CMD_DO_CONTROL_VIDEO: case NAV_CMD_DO_MOUNT_CONFIGURE: case NAV_CMD_DO_MOUNT_CONTROL: + case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: case NAV_CMD_DO_SET_ROI: case NAV_CMD_DO_SET_CAM_TRIGG_DIST: case NAV_CMD_OBLIQUE_SURVEY: case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL: case NAV_CMD_SET_CAMERA_MODE: case NAV_CMD_SET_CAMERA_ZOOM: + case NAV_CMD_SET_CAMERA_FOCUS: case NAV_CMD_DO_VTOL_TRANSITION: break; diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 6f356f71bc..4c1366eeff 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -329,7 +329,8 @@ MavlinkParametersManager::send() int i = 0; // Send while burst is not exceeded, we still have buffer space and still something to send - while ((i++ < max_num_to_send) && (_mavlink->get_free_tx_buf() >= get_size()) && send_params()) {} + while ((i++ < max_num_to_send) && (_mavlink->get_free_tx_buf() >= get_size()) && !_mavlink->radio_status_critical() + && send_params()) {} } bool @@ -390,7 +391,8 @@ MavlinkParametersManager::send_untransmitted() break; } } - } while ((_mavlink->get_free_tx_buf() >= get_size()) && (_param_update_index < (int) param_count())); + } while ((_mavlink->get_free_tx_buf() >= get_size()) && !_mavlink->radio_status_critical() + && (_param_update_index < (int) param_count())); // Flag work as done once all params have been sent if (_param_update_index >= (int) param_count()) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 56a63bac3b..6c8b00ed1f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -279,6 +279,18 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) break; #endif // !CONSTRAINED_FLASH + case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE: + handle_message_gimbal_manager_set_attitude(msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL: + handle_message_gimbal_manager_set_manual_control(msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION: + handle_message_gimbal_device_information(msg); + break; + default: break; } @@ -2938,6 +2950,94 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force) } } +void +MavlinkReceiver::handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg) +{ + mavlink_gimbal_manager_set_manual_control_t set_manual_control_msg; + mavlink_msg_gimbal_manager_set_manual_control_decode(msg, &set_manual_control_msg); + + gimbal_manager_set_manual_control_s set_manual_control{}; + set_manual_control.timestamp = hrt_absolute_time(); + set_manual_control.origin_sysid = msg->sysid; + set_manual_control.origin_compid = msg->compid; + set_manual_control.target_system = set_manual_control_msg.target_system; + set_manual_control.target_component = set_manual_control_msg.target_component; + set_manual_control.flags = set_manual_control_msg.flags; + set_manual_control.gimbal_device_id = set_manual_control_msg.gimbal_device_id; + + set_manual_control.pitch = set_manual_control_msg.pitch; + set_manual_control.yaw = set_manual_control_msg.yaw; + set_manual_control.pitch_rate = set_manual_control_msg.pitch_rate; + set_manual_control.yaw_rate = set_manual_control_msg.yaw_rate; + + _gimbal_manager_set_manual_control_pub.publish(set_manual_control); +} + +void +MavlinkReceiver::handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg) +{ + mavlink_gimbal_manager_set_attitude_t set_attitude_msg; + mavlink_msg_gimbal_manager_set_attitude_decode(msg, &set_attitude_msg); + + gimbal_manager_set_attitude_s gimbal_attitude{}; + gimbal_attitude.timestamp = hrt_absolute_time(); + gimbal_attitude.origin_sysid = msg->sysid; + gimbal_attitude.origin_compid = msg->compid; + gimbal_attitude.target_system = set_attitude_msg.target_system; + gimbal_attitude.target_component = set_attitude_msg.target_component; + gimbal_attitude.flags = set_attitude_msg.flags; + gimbal_attitude.gimbal_device_id = set_attitude_msg.gimbal_device_id; + + matrix::Quatf q(set_attitude_msg.q); + q.copyTo(gimbal_attitude.q); + + gimbal_attitude.angular_velocity_x = set_attitude_msg.angular_velocity_x; + gimbal_attitude.angular_velocity_y = set_attitude_msg.angular_velocity_y; + gimbal_attitude.angular_velocity_z = set_attitude_msg.angular_velocity_z; + + _gimbal_manager_set_attitude_pub.publish(gimbal_attitude); +} + +void +MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg) +{ + + mavlink_gimbal_device_information_t gimbal_device_info_msg; + mavlink_msg_gimbal_device_information_decode(msg, &gimbal_device_info_msg); + + gimbal_device_information_s gimbal_information{}; + gimbal_information.timestamp = hrt_absolute_time(); + + static_assert(sizeof(gimbal_information.vendor_name) == sizeof(gimbal_device_info_msg.vendor_name), + "vendor_name length doesn't match"); + static_assert(sizeof(gimbal_information.model_name) == sizeof(gimbal_device_info_msg.model_name), + "model_name length doesn't match"); + static_assert(sizeof(gimbal_information.custom_name) == sizeof(gimbal_device_info_msg.custom_name), + "custom_name length doesn't match"); + memcpy(gimbal_information.vendor_name, gimbal_device_info_msg.vendor_name, sizeof(gimbal_information.vendor_name)); + memcpy(gimbal_information.model_name, gimbal_device_info_msg.model_name, sizeof(gimbal_information.model_name)); + memcpy(gimbal_information.custom_name, gimbal_device_info_msg.custom_name, sizeof(gimbal_information.custom_name)); + gimbal_device_info_msg.vendor_name[sizeof(gimbal_device_info_msg.vendor_name) - 1] = '\0'; + gimbal_device_info_msg.model_name[sizeof(gimbal_device_info_msg.model_name) - 1] = '\0'; + gimbal_device_info_msg.custom_name[sizeof(gimbal_device_info_msg.custom_name) - 1] = '\0'; + + gimbal_information.firmware_version = gimbal_device_info_msg.firmware_version; + gimbal_information.hardware_version = gimbal_device_info_msg.hardware_version; + gimbal_information.cap_flags = gimbal_device_info_msg.cap_flags; + gimbal_information.custom_cap_flags = gimbal_device_info_msg.custom_cap_flags; + gimbal_information.uid = gimbal_device_info_msg.uid; + + gimbal_information.pitch_max = gimbal_device_info_msg.pitch_max; + gimbal_information.pitch_min = gimbal_device_info_msg.pitch_min; + + gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max; + gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min; + + gimbal_information.gimbal_device_compid = msg->compid; + + _gimbal_device_information_pub.publish(gimbal_information); +} + /** * Receive data from UART/UDP */ diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 1c8171054c..8922860d28 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -67,6 +67,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -201,6 +204,9 @@ private: void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg); void handle_message_utm_global_position(mavlink_message_t *msg); void handle_message_vision_position_estimate(mavlink_message_t *msg); + void handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg); + void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg); + void handle_message_gimbal_device_information(mavlink_message_t *msg); #if !defined(CONSTRAINED_FLASH) void handle_message_debug(mavlink_message_t *msg); @@ -352,6 +358,9 @@ private: uORB::Publication _collision_report_pub{ORB_ID(collision_report)}; uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::Publication _follow_target_pub{ORB_ID(follow_target)}; + uORB::Publication _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)}; + uORB::Publication _gimbal_manager_set_manual_control_pub{ORB_ID(gimbal_manager_set_manual_control)}; + uORB::Publication _gimbal_device_information_pub{ORB_ID(gimbal_device_information)}; uORB::Publication _irlock_report_pub{ORB_ID(irlock_report)}; uORB::Publication _landing_target_pose_pub{ORB_ID(landing_target_pose)}; uORB::Publication _log_message_pub{ORB_ID(log_message)}; diff --git a/src/modules/mavlink/module.yaml b/src/modules/mavlink/module.yaml index 7936df5a58..2924f8c21e 100644 --- a/src/modules/mavlink/module.yaml +++ b/src/modules/mavlink/module.yaml @@ -44,6 +44,8 @@ parameters: #6: Iridium # as the user does not need to configure this, hide it from the UI 7: Minimal 8: External Vision + #9: External Vision Minimal # hidden + 10: Gimbal reboot_required: true num_instances: *max_num_config_instances default: [0, 2, 0] diff --git a/src/modules/mavlink/streams/ALTITUDE.hpp b/src/modules/mavlink/streams/ALTITUDE.hpp index 78aba36aa4..45eb67c9f9 100644 --- a/src/modules/mavlink/streams/ALTITUDE.hpp +++ b/src/modules/mavlink/streams/ALTITUDE.hpp @@ -35,7 +35,7 @@ #define ALTITUDE_HPP #include -#include +#include class MavlinkStreamAltitude : public MavlinkStream { diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index a1e913f54a..90e83cddb4 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include #include #include @@ -134,7 +134,7 @@ private: updated |= write_tecs_status(&msg); updated |= write_vehicle_status(&msg); updated |= write_vehicle_status_flags(&msg); - updated |= write_wind_estimate(&msg); + updated |= write_wind(&msg); if (updated) { msg.timestamp = t / 1000; @@ -477,9 +477,9 @@ private: return false; } - bool write_wind_estimate(mavlink_high_latency2_t *msg) + bool write_wind(mavlink_high_latency2_t *msg) { - wind_estimate_s wind; + wind_s wind; if (_wind_sub.update(&wind)) { msg->wind_heading = static_cast(math::degrees(matrix::wrap_2pi(atan2f(wind.windspeed_east, @@ -506,7 +506,7 @@ private: update_local_position(); update_gps(); update_vehicle_status(); - update_wind_estimate(); + update_wind(); } void update_airspeed() @@ -586,9 +586,9 @@ private: } } - void update_wind_estimate() + void update_wind() { - wind_estimate_s wind; + wind_s wind; if (_wind_sub.update(&wind)) { _windspeed.add_value(sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east), @@ -642,7 +642,7 @@ private: uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _status_flags_sub{ORB_ID(vehicle_status_flags)}; uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; - uORB::Subscription _wind_sub{ORB_ID(wind_estimate)}; + uORB::Subscription _wind_sub{ORB_ID(wind)}; SimpleAnalyzer _airspeed; SimpleAnalyzer _airspeed_sp; diff --git a/src/modules/mavlink/streams/WIND_COV.hpp b/src/modules/mavlink/streams/WIND_COV.hpp index ec816e183f..192af05073 100644 --- a/src/modules/mavlink/streams/WIND_COV.hpp +++ b/src/modules/mavlink/streams/WIND_COV.hpp @@ -35,7 +35,7 @@ #define WIND_COV_HPP #include -#include +#include class MavlinkStreamWindCov : public MavlinkStream { @@ -50,29 +50,29 @@ public: unsigned get_size() override { - return _wind_estimate_sub.advertised() ? MAVLINK_MSG_ID_WIND_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + return _wind_sub.advertised() ? MAVLINK_MSG_ID_WIND_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: explicit MavlinkStreamWindCov(Mavlink *mavlink) : MavlinkStream(mavlink) {} - uORB::Subscription _wind_estimate_sub{ORB_ID(wind_estimate)}; + uORB::Subscription _wind_sub{ORB_ID(wind)}; uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; bool send() override { - wind_estimate_s wind_estimate; + wind_s wind; - if (_wind_estimate_sub.update(&wind_estimate)) { + if (_wind_sub.update(&wind)) { mavlink_wind_cov_t msg{}; - msg.time_usec = wind_estimate.timestamp; + msg.time_usec = wind.timestamp; - msg.wind_x = wind_estimate.windspeed_north; - msg.wind_y = wind_estimate.windspeed_east; + msg.wind_x = wind.windspeed_north; + msg.wind_y = wind.windspeed_east; msg.wind_z = 0.0f; - msg.var_horiz = wind_estimate.variance_north + wind_estimate.variance_east; + msg.var_horiz = wind.variance_north + wind.variance_east; msg.var_vert = 0.0f; vehicle_local_position_s lpos{}; diff --git a/src/modules/muorb/adsp/CMakeLists.txt b/src/modules/muorb/adsp/CMakeLists.txt index 7da84aa170..de899113b0 100644 --- a/src/modules/muorb/adsp/CMakeLists.txt +++ b/src/modules/muorb/adsp/CMakeLists.txt @@ -35,4 +35,4 @@ px4_add_library(modules__muorb__adsp px4muorb.cpp uORBFastRpcChannel.cpp ) -target_include_directories(modules__muorb__adsp PRIVATE ${PX4_SOURCE_DIR}/src/modules/uORB) +target_include_directories(modules__muorb__adsp PRIVATE ${PX4_SOURCE_DIR}/platforms/common/uORB) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index d436e740fe..3fce626d15 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -146,13 +146,15 @@ void Mission::on_inactivation() { // Disable camera trigger - vehicle_command_s cmd = {}; + vehicle_command_s cmd {}; cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; // Pause trigger cmd.param1 = -1.0f; cmd.param3 = 1.0f; _navigator->publish_vehicle_cmd(&cmd); + _navigator->release_gimbal_control(); + if (_navigator->get_precland()->is_activated()) { _navigator->get_precland()->on_inactivation(); } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index a37c2226c1..6230d8141f 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -96,6 +96,8 @@ MissionBlock::is_mission_item_reached() case NAV_CMD_DO_CONTROL_VIDEO: case NAV_CMD_DO_MOUNT_CONFIGURE: case NAV_CMD_DO_MOUNT_CONTROL: + case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: case NAV_CMD_DO_SET_ROI: case NAV_CMD_DO_SET_ROI_LOCATION: case NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET: @@ -105,6 +107,7 @@ MissionBlock::is_mission_item_reached() case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL: case NAV_CMD_SET_CAMERA_MODE: case NAV_CMD_SET_CAMERA_ZOOM: + case NAV_CMD_SET_CAMERA_FOCUS: return true; case NAV_CMD_DO_VTOL_TRANSITION: @@ -472,7 +475,10 @@ MissionBlock::issue_command(const mission_item_s &item) } else { _action_start = hrt_absolute_time(); - // mission_item -> vehicle_command + // This is to support legacy DO_MOUNT_CONTROL as part of a mission. + if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) { + _navigator->acquire_gimbal_control(); + } // we're expecting a mission command item here so assign the "raw" inputs to the command // (MAV_FRAME_MISSION mission item) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 39e93dbdb8..886831e8fa 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -262,6 +262,8 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission) missionitem.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO && missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE && missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL && + missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW && + missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE && missionitem.nav_cmd != NAV_CMD_DO_SET_ROI && missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION && missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET && @@ -271,6 +273,7 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission) missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL && missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE && missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM && + missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d", (int)(i + 1), @@ -389,6 +392,8 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt missionitem.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO && missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE && missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL && + missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW && + missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE && missionitem.nav_cmd != NAV_CMD_DO_SET_ROI && missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION && missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET && @@ -398,6 +403,7 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL && missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE && missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM && + missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION); } } diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 50cd362f26..5c6251d603 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -86,6 +86,9 @@ enum NAV_CMD { NAV_CMD_OBLIQUE_SURVEY = 260, NAV_CMD_SET_CAMERA_MODE = 530, NAV_CMD_SET_CAMERA_ZOOM = 531, + NAV_CMD_SET_CAMERA_FOCUS = 532, + NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000, + NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001, NAV_CMD_IMAGE_START_CAPTURE = 2000, NAV_CMD_IMAGE_STOP_CAPTURE = 2001, NAV_CMD_DO_TRIGGER_CONTROL = 2003, diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index af96b4ce45..12a40909a0 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -302,6 +302,9 @@ public: bool force_vtol(); + void acquire_gimbal_control(); + void release_gimbal_control(); + private: DEFINE_PARAMETERS( (ParamFloat) _param_nav_loiter_rad, /**< loiter radius for fixedwing */ @@ -403,7 +406,6 @@ private: float _mission_cruising_speed_fw{-1.0f}; float _mission_throttle{NAN}; - bool _mission_landing_in_progress{false}; // this flag gets set if the mission is currently executing on a landing pattern // if mission mode is inactive, this flag will be cleared after 2 seconds diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1f7c5debae..06c6cf45a1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1374,6 +1374,30 @@ Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t res _vehicle_cmd_ack_pub.publish(command_ack); } +void +Navigator::acquire_gimbal_control() +{ + vehicle_command_s vcmd = {}; + vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vcmd.param1 = _vstatus.system_id; + vcmd.param2 = _vstatus.component_id; + vcmd.param3 = -1.0f; // Leave unchanged. + vcmd.param4 = -1.0f; // Leave unchanged. + publish_vehicle_cmd(&vcmd); +} + +void +Navigator::release_gimbal_control() +{ + vehicle_command_s vcmd = {}; + vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vcmd.param1 = -3.0f; // Remove control if it had it. + vcmd.param2 = -3.0f; // Remove control if it had it. + vcmd.param3 = -1.0f; // Leave unchanged. + vcmd.param4 = -1.0f; // Leave unchanged. + publish_vehicle_cmd(&vcmd); +} + int Navigator::print_usage(const char *reason) { if (reason) { diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 62b339a627..f10dd8f951 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -257,6 +257,8 @@ void RTL::on_activation() const vehicle_global_position_s &global_position = *_navigator->get_global_position(); + _rtl_loiter_rad = _param_rtl_loiter_rad.get(); + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { _rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get()); @@ -410,6 +412,10 @@ void RTL::set_rtl_item() _mission_item.yaw = _destination.yaw; } + if (_navigator->get_vstatus()->is_vtol) { + _mission_item.loiter_radius = _rtl_loiter_rad; + } + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.time_inside = 0.0f; _mission_item.autocontinue = true; @@ -453,6 +459,24 @@ void RTL::set_rtl_item() break; } + case RTL_STATE_HEAD_TO_CENTER: { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + // Disable previous setpoint to prevent drift. + pos_sp_triplet->previous.valid = false; + break; + } + case RTL_STATE_TRANSITION_TO_MC: { set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); break; @@ -554,7 +578,7 @@ void RTL::advance_rtl() _rtl_state = RTL_STATE_LOITER; } else if (vtol_in_fw_mode) { - _rtl_state = RTL_STATE_TRANSITION_TO_MC; + _rtl_state = RTL_STATE_HEAD_TO_CENTER; } else { _rtl_state = RTL_STATE_LAND; @@ -573,6 +597,12 @@ void RTL::advance_rtl() _rtl_state = RTL_STATE_LAND; break; + case RTL_STATE_HEAD_TO_CENTER: + + _rtl_state = RTL_STATE_TRANSITION_TO_MC; + + break; + case RTL_STATE_TRANSITION_TO_MC: _rtl_state = RTL_MOVE_TO_LAND_HOVER_VTOL; @@ -670,12 +700,12 @@ void RTL::get_rtl_xy_z_speed(float &xy, float &z) matrix::Vector2f RTL::get_wind() { - _wind_estimate_sub.update(); + _wind_sub.update(); matrix::Vector2f wind; - if (hrt_absolute_time() - _wind_estimate_sub.get().timestamp < 1_s) { - wind(0) = _wind_estimate_sub.get().windspeed_north; - wind(1) = _wind_estimate_sub.get().windspeed_east; + if (hrt_absolute_time() - _wind_sub.get().timestamp < 1_s) { + wind(0) = _wind_sub.get().windspeed_north; + wind(1) = _wind_sub.get().windspeed_east; } return wind; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 6b5cd4118d..d7cb01028a 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include #include @@ -121,6 +121,7 @@ private: RTL_MOVE_TO_LAND_HOVER_VTOL, RTL_STATE_LAND, RTL_STATE_LANDED, + RTL_STATE_HEAD_TO_CENTER, } _rtl_state{RTL_STATE_NONE}; struct RTLPosition { @@ -148,6 +149,7 @@ private: float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position bool _rtl_alt_min{false}; + float _rtl_loiter_rad{50.0f}; // radius at which a fixed wing would loiter while descending bool _climb_and_return_done{false}; // this flag is set to true if RTL is active and we are past the climb state and return state bool _deny_mission_landing{false}; @@ -159,7 +161,8 @@ private: (ParamInt) _param_rtl_type, (ParamInt) _param_rtl_cone_half_angle_deg, (ParamFloat) _param_rtl_flt_time, - (ParamInt) _param_rtl_pld_md + (ParamInt) _param_rtl_pld_md, + (ParamFloat) _param_rtl_loiter_rad ) // These need to point at different parameters depending on vehicle type. @@ -169,7 +172,7 @@ private: param_t _param_rtl_xy_speed{PARAM_INVALID}; param_t _param_rtl_descent_speed{PARAM_INVALID}; - uORB::SubscriptionData _wind_estimate_sub{ORB_ID(wind_estimate)}; + uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; uORB::Publication _rtl_flight_time_pub{ORB_ID(rtl_flight_time)}; }; diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 769d56fa5f..463ee1a435 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -159,3 +159,17 @@ PARAM_DEFINE_FLOAT(RTL_FLT_TIME, 15); * @group Return To Land */ PARAM_DEFINE_INT32(RTL_PLD_MD, 0); + +/** + * Loiter radius for rtl descend + * + * Set the radius for loitering to a safe altitude for VTOL transition. + * + * @unit m + * @min 25 + * @max 1000 + * @decimal 1 + * @increment 0.5 + * @group Return Mode + */ +PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 5422101bcc..9e668de4c2 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -133,11 +133,13 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *st24_updated = false; - for (unsigned i = 0; i < n_bytes; i++) { - /* set updated flag if one complete packet was parsed */ - st24_rssi = RC_INPUT_RSSI_MAX; - *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &lost_count, - &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); + if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_SUMD))) { + for (unsigned i = 0; i < n_bytes; i++) { + /* set updated flag if one complete packet was parsed */ + st24_rssi = RC_INPUT_RSSI_MAX; + *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &lost_count, + &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); + } } if (*st24_updated && lost_count == 0) { @@ -161,11 +163,13 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated = false; - for (unsigned i = 0; i < n_bytes; i++) { - /* set updated flag if one complete packet was parsed */ - sumd_rssi = RC_INPUT_RSSI_MAX; - *sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count, - &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS, &sumd_failsafe_state)); + if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_ST24))) { + for (unsigned i = 0; i < n_bytes; i++) { + /* set updated flag if one complete packet was parsed */ + sumd_rssi = RC_INPUT_RSSI_MAX; + *sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count, + &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS, &sumd_failsafe_state)); + } } if (*sumd_updated) { @@ -263,35 +267,39 @@ controls_tick() perf_begin(c_gather_sbus); #endif - bool sbus_failsafe, sbus_frame_drop; - bool sbus_updated = sbus_input(_sbus_fd, r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, - PX4IO_RC_INPUT_CHANNELS); + bool sbus_updated = false; - if (sbus_updated) { - atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SBUS); + if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_DSM | PX4IO_P_STATUS_FLAGS_RC_ST24 | PX4IO_P_STATUS_FLAGS_RC_SUMD))) { + bool sbus_failsafe, sbus_frame_drop; + sbus_updated = sbus_input(_sbus_fd, r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, + PX4IO_RC_INPUT_CHANNELS); - unsigned sbus_rssi = RC_INPUT_RSSI_MAX; + if (sbus_updated) { + atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_RC_SBUS); - if (sbus_frame_drop) { - r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; - sbus_rssi = RC_INPUT_RSSI_MAX / 2; + unsigned sbus_rssi = RC_INPUT_RSSI_MAX; + + if (sbus_frame_drop) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; + sbus_rssi = RC_INPUT_RSSI_MAX / 2; + + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + } + + if (sbus_failsafe) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + + /* set RSSI to an emulated value if ADC RSSI is off */ + if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { + _rssi = sbus_rssi; + } - } else { - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); } - - if (sbus_failsafe) { - r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; - - } else { - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); - } - - /* set RSSI to an emulated value if ADC RSSI is off */ - if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { - _rssi = sbus_rssi; - } - } #if defined(PX4IO_PERF) @@ -321,7 +329,7 @@ controls_tick() bool dsm_updated = false, st24_updated = false, sumd_updated = false; - if (!((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) || (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM))) { + if (!(r_status_flags & (PX4IO_P_STATUS_FLAGS_RC_SBUS | PX4IO_P_STATUS_FLAGS_RC_PPM))) { #if defined(PX4IO_PERF) perf_begin(c_gather_dsm); #endif diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index 6cb2c7121f..5686182dca 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -61,7 +61,7 @@ Sih::Sih() : parameters_updated(); init_variables(); - init_sensors(); + gps_no_fix(); const hrt_abstime task_start = hrt_absolute_time(); _last_run = task_start; @@ -125,14 +125,17 @@ void Sih::Run() _px4_gyro.update(_now, _gyro(0), _gyro(1), _gyro(2)); // magnetometer published at 50 Hz - if (_now - _mag_time >= 20_ms) { + if (_now - _mag_time >= 20_ms + && fabs(_mag_offset_x) < 10000 + && fabs(_mag_offset_y) < 10000 + && fabs(_mag_offset_z) < 10000) { _mag_time = _now; - _px4_mag.update(_now, _mag(0), _mag(1), _mag(2)); } // baro published at 20 Hz - if (_now - _baro_time >= 50_ms) { + if (_now - _baro_time >= 50_ms + && fabs(_baro_offset_m) < 10000) { _baro_time = _now; _px4_baro.set_temperature(_baro_temp_c); _px4_baro.update(_now, _baro_p_mBar); @@ -182,6 +185,12 @@ void Sih::parameters_updated() _Im1 = inv(_I); _mu_I = Vector3f(_sih_mu_x.get(), _sih_mu_y.get(), _sih_mu_z.get()); + + _gps_used = _sih_gps_used.get(); + _baro_offset_m = _sih_baro_offset.get(); + _mag_offset_x = _sih_mag_offset_x.get(); + _mag_offset_y = _sih_mag_offset_y.get(); + _mag_offset_z = _sih_mag_offset_z.get(); } // initialization of the variables for the simulator @@ -197,10 +206,10 @@ void Sih::init_variables() _u[0] = _u[1] = _u[2] = _u[3] = 0.0f; } -void Sih::init_sensors() +void Sih::gps_fix() { _sensor_gps.fix_type = 3; // 3D fix - _sensor_gps.satellites_used = 8; + _sensor_gps.satellites_used = _gps_used; _sensor_gps.heading = NAN; _sensor_gps.heading_offset = NAN; _sensor_gps.s_variance_m_s = 0.5f; @@ -211,6 +220,21 @@ void Sih::init_sensors() _sensor_gps.vdop = 1.1f; } +void Sih::gps_no_fix() +{ + _sensor_gps.fix_type = 0; // 3D fix + _sensor_gps.satellites_used = _gps_used; + _sensor_gps.heading = NAN; + _sensor_gps.heading_offset = NAN; + _sensor_gps.s_variance_m_s = 100.f; + _sensor_gps.c_variance_rad = 100.f; + _sensor_gps.eph = 100.f; + _sensor_gps.epv = 100.f; + _sensor_gps.hdop = 100.f; + _sensor_gps.vdop = 100.f; +} + + // read the motor signals outputted from the mixer void Sih::read_motors() { @@ -282,9 +306,12 @@ void Sih::reconstruct_sensors_signals() _acc = _C_IB.transpose() * (_v_I_dot - Vector3f(0.0f, 0.0f, CONSTANTS_ONE_G)) + noiseGauss3f(0.5f, 1.7f, 1.4f); _gyro = _w_B + noiseGauss3f(0.14f, 0.07f, 0.03f); _mag = _C_IB.transpose() * _mu_I + noiseGauss3f(0.02f, 0.02f, 0.03f); + _mag(0) += _mag_offset_x; + _mag(1) += _mag_offset_y; + _mag(2) += _mag_offset_z; // barometer - float altitude = (_H0 - _p_I(2)) + generate_wgn() * 0.14f; // altitude with noise + float altitude = (_H0 - _p_I(2)) + _baro_offset_m + generate_wgn() * 0.14f; // altitude with noise _baro_p_mBar = CONSTANTS_STD_PRESSURE_MBAR * // reconstructed pressure in mBar powf((1.0f + altitude * TEMP_GRADIENT / T1_K), -CONSTANTS_ONE_G / (TEMP_GRADIENT * CONSTANTS_AIR_GAS_CONST)); _baro_temp_c = T1_K + CONSTANTS_ABSOLUTE_NULL_CELSIUS + TEMP_GRADIENT * altitude; // reconstructed temperture in celcius @@ -316,6 +343,13 @@ void Sih::send_gps() _sensor_gps.cog_rad = atan2(_gps_vel(1), _gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) + if (_gps_used >= 4) { + gps_fix(); + + } else { + gps_no_fix(); + } + _sensor_gps_pub.publish(_sensor_gps); } diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index 960eda4743..4f877669a3 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -118,7 +118,8 @@ private: static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre void init_variables(); - void init_sensors(); + void gps_fix(); + void gps_no_fix(); void read_motors(); void generate_force_and_torques(); void equations_of_motion(); @@ -174,6 +175,9 @@ private: matrix::Matrix3f _Im1; // inverse of the intertia matrix matrix::Vector3f _mu_I; // NED magnetic field in inertial frame [G] + int _gps_used; + float _baro_offset_m, _mag_offset_x, _mag_offset_y, _mag_offset_z; + // parameters defined in sih_params.c DEFINE_PARAMETERS( (ParamInt) _imu_gyro_ratemax, @@ -196,6 +200,11 @@ private: (ParamFloat) _sih_h0, (ParamFloat) _sih_mu_x, (ParamFloat) _sih_mu_y, - (ParamFloat) _sih_mu_z + (ParamFloat) _sih_mu_z, + (ParamInt) _sih_gps_used, + (ParamFloat) _sih_baro_offset, + (ParamFloat) _sih_mag_offset_x, + (ParamFloat) _sih_mag_offset_y, + (ParamFloat) _sih_mag_offset_z ) }; diff --git a/src/modules/sih/sih_params.c b/src/modules/sih/sih_params.c index cefc23365e..a0aefd5637 100644 --- a/src/modules/sih/sih_params.c +++ b/src/modules/sih/sih_params.c @@ -343,3 +343,51 @@ PARAM_DEFINE_FLOAT(SIH_LOC_MU_Y, -0.045f); * @group Simulation In Hardware */ PARAM_DEFINE_FLOAT(SIH_LOC_MU_Z, 0.504f); + +/** + * Number of GPS satellites used + * + * @min 0 + * @max 50 + * @group Simulation In Hardware + */ +PARAM_DEFINE_INT32(SIH_GPS_USED, 10); + +/** + * Barometer offset in meters + * + * Absolute value superior to 10000 will disable barometer + * + * @unit m + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_BARO_OFFSET, 0.0f); + +/** + * magnetometer X offset in Gauss + * + * Absolute value superior to 10000 will disable magnetometer + * + * @unit gauss + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_MAG_OFFSET_X, 0.0f); + +/** + * magnetometer Y offset in Gauss + * + * Absolute value superior to 10000 will disable magnetometer + * + * @unit gauss + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_MAG_OFFSET_Y, 0.0f); +/** + * magnetometer Z offset in Gauss + * + * Absolute value superior to 10000 will disable magnetometer + * + * @unit gauss + * @group Simulation In Hardware + */ +PARAM_DEFINE_FLOAT(SIH_MAG_OFFSET_Z, 0.0f); diff --git a/src/modules/vmount/common.h b/src/modules/vmount/common.h index a1b3a6eece..d4ecfd8c0c 100644 --- a/src/modules/vmount/common.h +++ b/src/modules/vmount/common.h @@ -41,11 +41,9 @@ #include - namespace vmount { - /** * @struct ControlData * This defines the common API between an input and an output of the vmount driver. @@ -58,7 +56,6 @@ struct ControlData { Neutral = 0, /**< move to neutral position */ Angle, /**< control the roll, pitch & yaw angle directly */ LonLat /**< control via lon, lat */ - //TODO: add more, like smooth curve, ... ? }; @@ -66,13 +63,15 @@ struct ControlData { union TypeData { struct TypeAngle { - float angles[3]; /**< attitude angles (roll, pitch, yaw) in rad, [-pi, +pi] or rad/s (if is angular rate) */ + float q[4]; /**< attitude quaternion */ + float angular_velocity[3]; // angular velocity + // according to DO_MOUNT_CONFIGURE enum class Frame : uint8_t { - AngleBodyFrame = 0, /**< Angle in body frame. */ - AngularRate, /**< Angular rate in rad/s. */ - AngleAbsoluteFrame /**< Angle in absolute frame. */ - } frames[3]; /**< Frame of given angle. */ + AngleBodyFrame = 0, /**< Follow mode, angle relative to vehicle (usually default for yaw axis). */ + AngularRate = 1, /**< Angular rate set only, for compatibility with MAVLink v1 protocol. */ + AngleAbsoluteFrame = 2/**< Lock mode, angle relative to horizon/world, lock mode. (usually default for roll and pitch). */ + } frames[3]; /**< Mode. */ } angle; struct TypeLonLat { @@ -89,10 +88,8 @@ struct ControlData { bool stabilize_axis[3] = { false, false, false }; /**< whether the vmount driver should stabilize an axis (if the output supports it, this can also be done externally) */ - bool gimbal_shutter_retract = false; /**< whether to lock the gimbal (only in RC output mode) */ }; - } /* namespace vmount */ diff --git a/src/modules/vmount/input.cpp b/src/modules/vmount/input.cpp index 2bf3200c38..f6e544076e 100644 --- a/src/modules/vmount/input.cpp +++ b/src/modules/vmount/input.cpp @@ -76,5 +76,11 @@ void InputBase::control_data_set_lon_lat(double lon, double lat, float altitude, _control_data.type_data.lonlat.yaw_angle_offset = 0.f; } -} /* namespace vmount */ +void InputBase::set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize) +{ + _control_data.stabilize_axis[0] = roll_stabilize; + _control_data.stabilize_axis[1] = pitch_stabilize; + _control_data.stabilize_axis[2] = yaw_stabilize; +} +} /* namespace vmount */ diff --git a/src/modules/vmount/input.h b/src/modules/vmount/input.h index 9c2b2603e4..e7b26a697e 100644 --- a/src/modules/vmount/input.h +++ b/src/modules/vmount/input.h @@ -52,7 +52,8 @@ namespace vmount class InputBase { public: - virtual ~InputBase() {} + InputBase() = default; + virtual ~InputBase() = default; /** * Wait for an input update, with a timeout. @@ -72,6 +73,8 @@ public: /** report status to stdout */ virtual void print_status() = 0; + void set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize); + protected: virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) = 0; diff --git a/src/modules/vmount/input_mavlink.cpp b/src/modules/vmount/input_mavlink.cpp index 570dd2d086..1c0fb12a61 100644 --- a/src/modules/vmount/input_mavlink.cpp +++ b/src/modules/vmount/input_mavlink.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,15 +40,19 @@ #include "input_mavlink.h" #include +#include #include #include #include +#include +#include #include #include #include #include #include #include +#include namespace vmount { @@ -115,6 +119,7 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_ _control_data.type = ControlData::Type::Neutral; *control_data = &_control_data; + _cur_roi_mode = vehicle_roi.mode; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) { _control_data.type = ControlData::Type::LonLat; @@ -126,22 +131,17 @@ int InputMavlinkROI::update_impl(unsigned int timeout_ms, ControlData **control_ _control_data.type_data.lonlat.yaw_angle_offset = vehicle_roi.yaw_offset; *control_data = &_control_data; + _cur_roi_mode = vehicle_roi.mode; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) { control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt); *control_data = &_control_data; + _cur_roi_mode = vehicle_roi.mode; } else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) { //TODO is this even suported? } - - _cur_roi_mode = vehicle_roi.mode; - - //set all other control data fields to defaults - for (int i = 0; i < 3; ++i) { - _control_data.stabilize_axis[i] = false; - } } // check whether the position setpoint got updated @@ -175,20 +175,8 @@ void InputMavlinkROI::print_status() } -InputMavlinkCmdMount::InputMavlinkCmdMount(bool stabilize) - : _stabilize {stabilize, stabilize, stabilize} +InputMavlinkCmdMount::InputMavlinkCmdMount() { - param_t handle = param_find("MAV_SYS_ID"); - - if (handle != PARAM_INVALID) { - param_get(handle, &_mav_sys_id); - } - - handle = param_find("MAV_COMP_ID"); - - if (handle != PARAM_INVALID) { - param_get(handle, &_mav_comp_id); - } } InputMavlinkCmdMount::~InputMavlinkCmdMount() @@ -259,10 +247,6 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con continue; } - for (int i = 0; i < 3; ++i) { - _control_data.stabilize_axis[i] = _stabilize[i]; - } - _control_data.gimbal_shutter_retract = false; if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) { @@ -279,24 +263,30 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con *control_data = &_control_data; break; - case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: - _control_data.type = ControlData::Type::Angle; - _control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - _control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - _control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - // vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0 - _control_data.type_data.angle.angles[0] = vehicle_command.param2 * M_DEG_TO_RAD_F; - // vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1 - _control_data.type_data.angle.angles[1] = vehicle_command.param1 * M_DEG_TO_RAD_F; - // both specs have yaw on channel 2 - _control_data.type_data.angle.angles[2] = vehicle_command.param3 * M_DEG_TO_RAD_F; + case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: { + _control_data.type = ControlData::Type::Angle; + _control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + _control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + _control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - // We expect angle of [-pi..+pi]. If the input range is [0..2pi] we can fix that. - if (_control_data.type_data.angle.angles[2] > M_PI_F) { - _control_data.type_data.angle.angles[2] -= 2 * M_PI_F; + // vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0 + const float roll = math::radians(vehicle_command.param2); + // vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1 + const float pitch = math::radians(vehicle_command.param1); + // both specs have yaw on channel 2 + float yaw = math::radians(vehicle_command.param3); + + matrix::Eulerf euler(roll, pitch, yaw); + + matrix::Quatf q(euler); + q.copyTo(_control_data.type_data.angle.q); + + _control_data.type_data.angle.angular_velocity[0] = NAN; + _control_data.type_data.angle.angular_velocity[1] = NAN; + _control_data.type_data.angle.angular_velocity[2] = NAN; + + *control_data = &_control_data; } - - *control_data = &_control_data; break; case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING: @@ -312,9 +302,11 @@ int InputMavlinkCmdMount::update_impl(unsigned int timeout_ms, ControlData **con _ack_vehicle_command(&vehicle_command); } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) { - _stabilize[0] = (int)(vehicle_command.param2 + 0.5f) == 1; - _stabilize[1] = (int)(vehicle_command.param3 + 0.5f) == 1; - _stabilize[2] = (int)(vehicle_command.param4 + 0.5f) == 1; + + _control_data.stabilize_axis[0] = (int)(vehicle_command.param2 + 0.5f) == 1; + _control_data.stabilize_axis[1] = (int)(vehicle_command.param3 + 0.5f) == 1; + _control_data.stabilize_axis[2] = (int)(vehicle_command.param4 + 0.5f) == 1; + const int params[] = { (int)((float)vehicle_command.param5 + 0.5f), @@ -378,5 +370,539 @@ void InputMavlinkCmdMount::print_status() PX4_INFO("Input: Mavlink (CMD_MOUNT)"); } +InputMavlinkGimbalV2::InputMavlinkGimbalV2(uint8_t mav_sys_id, uint8_t mav_comp_id, + float &mnt_rate_pitch, float &mnt_rate_yaw) : + _mav_sys_id(mav_sys_id), + _mav_comp_id(mav_comp_id), + _mnt_rate_pitch(mnt_rate_pitch), + _mnt_rate_yaw(mnt_rate_yaw) +{ + _stream_gimbal_manager_information(); +} + +InputMavlinkGimbalV2::~InputMavlinkGimbalV2() +{ + if (_vehicle_roi_sub >= 0) { + orb_unsubscribe(_vehicle_roi_sub); + } + + if (_position_setpoint_triplet_sub >= 0) { + orb_unsubscribe(_position_setpoint_triplet_sub); + } + + if (_gimbal_manager_set_attitude_sub >= 0) { + orb_unsubscribe(_gimbal_manager_set_attitude_sub); + } + + if (_vehicle_command_sub >= 0) { + orb_unsubscribe(_vehicle_command_sub); + } + + if (_gimbal_manager_set_manual_control_sub >= 0) { + orb_unsubscribe(_gimbal_manager_set_manual_control_sub); + } +} + + +void InputMavlinkGimbalV2::print_status() +{ + PX4_INFO("Input: Mavlink (Gimbal V2)"); +} + +int InputMavlinkGimbalV2::initialize() +{ + _vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi)); + + if (_vehicle_roi_sub < 0) { + return -errno; + } + + _position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + + if (_position_setpoint_triplet_sub < 0) { + return -errno; + } + + _gimbal_manager_set_attitude_sub = orb_subscribe(ORB_ID(gimbal_manager_set_attitude)); + + if (_gimbal_manager_set_attitude_sub < 0) { + return -errno; + } + + if ((_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command))) < 0) { + return -errno; + } + + if ((_gimbal_manager_set_manual_control_sub = orb_subscribe(ORB_ID(gimbal_manager_set_manual_control))) < 0) { + return -errno; + } + + // rate-limit inputs to 100Hz. If we don't do this and the output is configured to mavlink mode, + // it will publish vehicle_command's as well, causing the input poll() in here to return + // immediately, which in turn will cause an output update and thus a busy loop. + orb_set_interval(_vehicle_command_sub, 10); + + return 0; +} + +void InputMavlinkGimbalV2::_stream_gimbal_manager_status() +{ + gimbal_device_attitude_status_s gimbal_device_attitude_status{}; + + if (_gimbal_device_attitude_status_sub.updated()) { + _gimbal_device_attitude_status_sub.copy(&gimbal_device_attitude_status); + } + + gimbal_manager_status_s gimbal_manager_status{}; + gimbal_manager_status.timestamp = hrt_absolute_time(); + gimbal_manager_status.flags = gimbal_device_attitude_status.device_flags; + gimbal_manager_status.gimbal_device_id = 0; + gimbal_manager_status.primary_control_sysid = _sys_id_primary_control; + gimbal_manager_status.primary_control_compid = _comp_id_primary_control; + gimbal_manager_status.secondary_control_sysid = 0; // TODO: support secondary control + gimbal_manager_status.secondary_control_compid = 0; // TODO: support secondary control + _gimbal_manager_status_pub.publish(gimbal_manager_status); +} + +void InputMavlinkGimbalV2::_stream_gimbal_manager_information() +{ + // TODO: Take gimbal_device_information into account. + + gimbal_manager_information_s gimbal_manager_info; + gimbal_manager_info.timestamp = hrt_absolute_time(); + + gimbal_manager_info.cap_flags = + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK | + gimbal_manager_information_s::GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL; + + gimbal_manager_info.pitch_max = M_PI_F / 2; + gimbal_manager_info.pitch_min = -M_PI_F / 2; + gimbal_manager_info.yaw_max = M_PI_F; + gimbal_manager_info.yaw_min = -M_PI_F; + + _gimbal_manager_info_pub.publish(gimbal_manager_info); +} + +int InputMavlinkGimbalV2::update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active) +{ + _stream_gimbal_manager_status(); + + // Default to no change, set if we receive anything. + *control_data = nullptr; + + const int num_poll = 5; + px4_pollfd_struct_t polls[num_poll]; + polls[0].fd = _gimbal_manager_set_attitude_sub; + polls[0].events = POLLIN; + polls[1].fd = _vehicle_roi_sub; + polls[1].events = POLLIN; + polls[2].fd = _position_setpoint_triplet_sub; + polls[2].events = POLLIN; + polls[3].fd = _vehicle_command_sub; + polls[3].events = POLLIN; + polls[4].fd = _gimbal_manager_set_manual_control_sub; + polls[4].events = POLLIN; + + int poll_timeout = (int)timeout_ms; + + bool exit_loop = false; + + while (!exit_loop && poll_timeout >= 0) { + hrt_abstime poll_start = hrt_absolute_time(); + + int ret = px4_poll(polls, num_poll, poll_timeout); + + if (ret < 0) { + return -errno; + } + + poll_timeout -= (hrt_absolute_time() - poll_start) / 1000; + + // if we get a command that we need to handle, we exit the loop, otherwise we poll until we reach the timeout + exit_loop = true; + + if (ret == 0) { + // Timeout control_data already null. + + } else { + if (polls[0].revents & POLLIN) { + gimbal_manager_set_attitude_s set_attitude; + orb_copy(ORB_ID(gimbal_manager_set_attitude), _gimbal_manager_set_attitude_sub, &set_attitude); + + if (set_attitude.origin_sysid == _sys_id_primary_control && + set_attitude.origin_compid == _comp_id_primary_control) { + const matrix::Quatf q(set_attitude.q); + const matrix::Vector3f angular_velocity(set_attitude.angular_velocity_x, + set_attitude.angular_velocity_y, + set_attitude.angular_velocity_z); + + _set_control_data_from_set_attitude(set_attitude.flags, q, angular_velocity); + *control_data = &_control_data; + + } else { + PX4_DEBUG("Ignoring gimbal_manager_set_attitude from %d/%d", + set_attitude.origin_sysid, set_attitude.origin_compid); + } + } + + if (polls[1].revents & POLLIN) { + vehicle_roi_s vehicle_roi; + orb_copy(ORB_ID(vehicle_roi), _vehicle_roi_sub, &vehicle_roi); + + _control_data.gimbal_shutter_retract = false; + + if (vehicle_roi.mode == vehicle_roi_s::ROI_NONE) { + + _control_data.type = ControlData::Type::Neutral; + *control_data = &_control_data; + _cur_roi_mode = vehicle_roi.mode; + + } else if (vehicle_roi.mode == vehicle_roi_s::ROI_WPNEXT) { + _control_data.type = ControlData::Type::LonLat; + _read_control_data_from_position_setpoint_sub(); + _control_data.type_data.lonlat.pitch_fixed_angle = -10.f; + + _control_data.type_data.lonlat.roll_angle = vehicle_roi.roll_offset; + _control_data.type_data.lonlat.pitch_angle_offset = vehicle_roi.pitch_offset; + _control_data.type_data.lonlat.yaw_angle_offset = vehicle_roi.yaw_offset; + + *control_data = &_control_data; + _cur_roi_mode = vehicle_roi.mode; + + } else if (vehicle_roi.mode == vehicle_roi_s::ROI_LOCATION) { + control_data_set_lon_lat(vehicle_roi.lon, vehicle_roi.lat, vehicle_roi.alt); + + *control_data = &_control_data; + _cur_roi_mode = vehicle_roi.mode; + + } else if (vehicle_roi.mode == vehicle_roi_s::ROI_TARGET) { + //TODO is this even suported? + exit_loop = false; + + } else { + exit_loop = false; + } + } + + // check whether the position setpoint got updated + if (polls[2].revents & POLLIN) { + if (_cur_roi_mode == vehicle_roi_s::ROI_WPNEXT) { + _read_control_data_from_position_setpoint_sub(); + *control_data = &_control_data; + + } else { // must do an orb_copy() in *every* case + position_setpoint_triplet_s position_setpoint_triplet; + orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); + exit_loop = false; + } + } + + if (polls[3].revents & POLLIN) { + vehicle_command_s vehicle_command; + orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command); + + // Process only if the command is for us or for anyone (component id 0). + const bool sysid_correct = (vehicle_command.target_system == _mav_sys_id) || (vehicle_command.target_system == 0); + const bool compid_correct = ((vehicle_command.target_component == _mav_comp_id) || + (vehicle_command.target_component == 0)); + + if (!sysid_correct || !compid_correct) { + exit_loop = false; + continue; + } + + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL) { + // FIXME: Remove me later. For now, we support this for legacy missions + // using gimbal v1 protocol. + + switch ((int)vehicle_command.param7) { + case vehicle_command_s::VEHICLE_MOUNT_MODE_RETRACT: + _control_data.gimbal_shutter_retract = true; + + /* FALLTHROUGH */ + + case vehicle_command_s::VEHICLE_MOUNT_MODE_NEUTRAL: + _control_data.type = ControlData::Type::Neutral; + + *control_data = &_control_data; + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING: { + _control_data.type = ControlData::Type::Angle; + _control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + _control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + _control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + + // vmount spec has roll on channel 0, MAVLink spec has pitch on channel 0 + const float roll = math::radians(vehicle_command.param2); + // vmount spec has pitch on channel 1, MAVLink spec has roll on channel 1 + const float pitch = math::radians(vehicle_command.param1); + // both specs have yaw on channel 2 + float yaw = math::radians(vehicle_command.param3); + + // We expect angle of [-pi..+pi]. If the input range is [0..2pi] we can fix that. + if (yaw > M_PI_F) { + yaw -= 2 * M_PI_F; + } + + matrix::Eulerf euler(roll, pitch, yaw); + + matrix::Quatf q(euler); + q.copyTo(_control_data.type_data.angle.q); + + _control_data.type_data.angle.angular_velocity[0] = NAN; + _control_data.type_data.angle.angular_velocity[1] = NAN; + _control_data.type_data.angle.angular_velocity[2] = NAN; + + *control_data = &_control_data; + } + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_RC_TARGETING: + break; + + case vehicle_command_s::VEHICLE_MOUNT_MODE_GPS_POINT: + control_data_set_lon_lat((double)vehicle_command.param6, (double)vehicle_command.param5, vehicle_command.param4); + + *control_data = &_control_data; + break; + } + + _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) { + + _control_data.stabilize_axis[0] = (int)(vehicle_command.param2 + 0.5f) == 1; + _control_data.stabilize_axis[1] = (int)(vehicle_command.param3 + 0.5f) == 1; + _control_data.stabilize_axis[2] = (int)(vehicle_command.param4 + 0.5f) == 1; + + + const int params[] = { + (int)((float)vehicle_command.param5 + 0.5f), + (int)((float)vehicle_command.param6 + 0.5f), + (int)(vehicle_command.param7 + 0.5f) + }; + + for (int i = 0; i < 3; ++i) { + + if (params[i] == 0) { + _control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + + } else if (params[i] == 1) { + _control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngularRate; + + } else if (params[i] == 2) { + _control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; + + } else { + // Not supported, fallback to body angle. + _control_data.type_data.angle.frames[i] = + ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + } + } + + _control_data.type = ControlData::Type::Neutral; //always switch to neutral position + + *control_data = &_control_data; + _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE) { + + const int param_sys_id = roundf(vehicle_command.param1); + const int param_comp_id = roundf(vehicle_command.param2); + + uint8_t new_sys_id_primary_control = [&]() { + if (param_sys_id >= 0 && param_sys_id < 256) { + // Valid new sysid. + return (uint8_t)param_sys_id; + + } else if (param_sys_id == -1) { + // leave unchanged + return _sys_id_primary_control; + + } else if (param_sys_id == -2) { + // set itself + return _mav_sys_id; + + } else if (param_sys_id == -3) { + // release control if in control + if (_sys_id_primary_control == vehicle_command.source_system) { + return (uint8_t)0; + + } else { + return _sys_id_primary_control; + } + + } else { + PX4_WARN("Unknown param1 value for DO_GIMBAL_MANAGER_CONFIGURE"); + return _sys_id_primary_control; + } + }(); + + uint8_t new_comp_id_primary_control = [&]() { + if (param_comp_id >= 0 && param_comp_id < 256) { + // Valid new compid. + return (uint8_t)param_comp_id; + + } else if (param_comp_id == -1) { + // leave unchanged + return _comp_id_primary_control; + + } else if (param_comp_id == -2) { + // set itself + return _mav_comp_id; + + } else if (param_comp_id == -3) { + // release control if in control + if (_comp_id_primary_control == vehicle_command.source_component) { + return (uint8_t)0; + + } else { + return _comp_id_primary_control; + } + + } else { + PX4_WARN("Unknown param2 value for DO_GIMBAL_MANAGER_CONFIGURE"); + return _comp_id_primary_control; + } + }(); + + + if (new_sys_id_primary_control != _sys_id_primary_control || + new_comp_id_primary_control != _comp_id_primary_control) { + PX4_INFO("Configured primary gimbal control sysid/compid from %d/%d to %d/%d", + _sys_id_primary_control, _comp_id_primary_control, + new_sys_id_primary_control, new_comp_id_primary_control); + _sys_id_primary_control = new_sys_id_primary_control; + _comp_id_primary_control = new_comp_id_primary_control; + } + + // TODO: support secondary control + // TODO: support gimbal device id for multiple gimbals + + _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW) { + + if (vehicle_command.source_system == _sys_id_primary_control && + vehicle_command.source_component == _comp_id_primary_control) { + + const matrix::Eulerf euler(0.0f, math::radians(vehicle_command.param1), math::radians(vehicle_command.param2)); + const matrix::Quatf q(euler); + const matrix::Vector3f angular_velocity(0.0f, vehicle_command.param3, vehicle_command.param4); + const uint32_t flags = vehicle_command.param5; + + // TODO: support gimbal device id for multiple gimbals + + _set_control_data_from_set_attitude(flags, q, angular_velocity); + *control_data = &_control_data; + _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + + } else { + PX4_ERR("GIMBAL_MANAGER_PITCHYAW denied, not in control"); + _ack_vehicle_command(&vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); + } + + } else { + exit_loop = false; + } + + } + + if (polls[4].revents & POLLIN) { + gimbal_manager_set_manual_control_s set_manual_control; + orb_copy(ORB_ID(gimbal_manager_set_manual_control), _gimbal_manager_set_manual_control_sub, &set_manual_control); + + if (set_manual_control.origin_sysid == _sys_id_primary_control && + set_manual_control.origin_compid == _comp_id_primary_control) { + + const matrix::Quatf q = + (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) ? + matrix::Quatf(matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw)) : + matrix::Quatf(NAN, NAN, NAN, NAN); + + const matrix::Vector3f angular_velocity = + (PX4_ISFINITE(set_manual_control.pitch_rate) && PX4_ISFINITE(set_manual_control.yaw_rate)) ? + matrix::Vector3f(0.0f, + math::radians(_mnt_rate_pitch) * set_manual_control.pitch_rate, + math::radians(_mnt_rate_yaw) * set_manual_control.yaw_rate) : + matrix::Vector3f(NAN, NAN, NAN); + + _set_control_data_from_set_attitude(set_manual_control.flags, q, angular_velocity); + *control_data = &_control_data; + + } else { + PX4_DEBUG("Ignoring gimbal_manager_set_manual_control from %d/%d", + set_manual_control.origin_sysid, set_manual_control.origin_compid); + } + } + } + } + + return 0; +} + +void InputMavlinkGimbalV2::_set_control_data_from_set_attitude(const uint32_t flags, const matrix::Quatf &q, + const matrix::Vector3f &angular_velocity) +{ + if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_RETRACT) != 0) { + // not implemented in ControlData + } else if ((flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL) != 0) { + _control_data.type = ControlData::Type::Neutral; + + } else { + _control_data.type = ControlData::Type::Angle; + + q.copyTo(_control_data.type_data.angle.q); + + _control_data.type_data.angle.angular_velocity[0] = angular_velocity(0); + _control_data.type_data.angle.angular_velocity[1] = angular_velocity(1); + _control_data.type_data.angle.angular_velocity[2] = angular_velocity(2); + + _control_data.type_data.angle.frames[0] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK) + ? ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame + : ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + + _control_data.type_data.angle.frames[1] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK) + ? ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame + : ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + + _control_data.type_data.angle.frames[2] = (flags & gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_YAW_LOCK) + ? ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame + : ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; + } +} + +//TODO move this one to input.cpp such that it can be shared across functions +void InputMavlinkGimbalV2::_ack_vehicle_command(vehicle_command_s *cmd, uint8_t result) +{ + vehicle_command_ack_s vehicle_command_ack{}; + + vehicle_command_ack.timestamp = hrt_absolute_time(); + vehicle_command_ack.command = cmd->command; + vehicle_command_ack.result = result; + vehicle_command_ack.target_system = cmd->source_system; + vehicle_command_ack.target_component = cmd->source_component; + + uORB::Publication cmd_ack_pub{ORB_ID(vehicle_command_ack)}; + cmd_ack_pub.publish(vehicle_command_ack); +} + +void InputMavlinkGimbalV2::_read_control_data_from_position_setpoint_sub() +{ + position_setpoint_triplet_s position_setpoint_triplet; + orb_copy(ORB_ID(position_setpoint_triplet), _position_setpoint_triplet_sub, &position_setpoint_triplet); + _control_data.type_data.lonlat.lon = position_setpoint_triplet.current.lon; + _control_data.type_data.lonlat.lat = position_setpoint_triplet.current.lat; + _control_data.type_data.lonlat.altitude = position_setpoint_triplet.current.alt; +} } /* namespace vmount */ diff --git a/src/modules/vmount/input_mavlink.h b/src/modules/vmount/input_mavlink.h index b3661466ec..dfe491d792 100644 --- a/src/modules/vmount/input_mavlink.h +++ b/src/modules/vmount/input_mavlink.h @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -43,8 +43,16 @@ #include "input_rc.h" #include +#include +#include +#include +#include +#include #include #include +#include +#include + namespace vmount { @@ -69,7 +77,7 @@ private: int _vehicle_roi_sub = -1; int _position_setpoint_triplet_sub = -1; - uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE; + uint8_t _cur_roi_mode {vehicle_roi_s::ROI_NONE}; }; @@ -80,7 +88,7 @@ private: class InputMavlinkCmdMount : public InputBase { public: - InputMavlinkCmdMount(bool stabilize); + InputMavlinkCmdMount(); virtual ~InputMavlinkCmdMount(); virtual void print_status(); @@ -93,11 +101,51 @@ private: void _ack_vehicle_command(vehicle_command_s *cmd); int _vehicle_command_sub = -1; - bool _stabilize[3] = { false, false, false }; int32_t _mav_sys_id{1}; ///< our mavlink system id int32_t _mav_comp_id{1}; ///< our mavlink component id }; +class InputMavlinkGimbalV2 : public InputBase +{ +public: + InputMavlinkGimbalV2(uint8_t mav_sys_id, uint8_t mav_comp_id, float &mnt_rate_pitch, float &mnt_rate_yaw); + virtual ~InputMavlinkGimbalV2(); + + virtual void print_status(); + +protected: + virtual int update_impl(unsigned int timeout_ms, ControlData **control_data, bool already_active); + virtual int initialize(); + +private: + void _set_control_data_from_set_attitude(const uint32_t flags, const matrix::Quatf &q, + const matrix::Vector3f &angular_velocity); + void _ack_vehicle_command(vehicle_command_s *cmd, uint8_t result); + void _stream_gimbal_manager_information(); + void _stream_gimbal_manager_status(); + void _read_control_data_from_position_setpoint_sub(); + + int _vehicle_roi_sub = -1; + int _gimbal_manager_set_attitude_sub = -1; + int _gimbal_manager_set_manual_control_sub = -1; + int _position_setpoint_triplet_sub = -1; + int _vehicle_command_sub = -1; + + uint8_t _mav_sys_id{1}; ///< our mavlink system id + uint8_t _mav_comp_id{1}; ///< our mavlink component id + + uint8_t _sys_id_primary_control{0}; + uint8_t _comp_id_primary_control{0}; + + float &_mnt_rate_pitch; + float &_mnt_rate_yaw; + + uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; + uORB::Publication _gimbal_manager_info_pub{ORB_ID(gimbal_manager_information)}; + uORB::Publication _gimbal_manager_status_pub{ORB_ID(gimbal_manager_status)}; + uint8_t _cur_roi_mode = vehicle_roi_s::ROI_NONE; +}; } /* namespace vmount */ diff --git a/src/modules/vmount/input_rc.cpp b/src/modules/vmount/input_rc.cpp index b917914807..de5a5d8633 100644 --- a/src/modules/vmount/input_rc.cpp +++ b/src/modules/vmount/input_rc.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,6 +41,7 @@ #include #include +#include #include #include @@ -49,8 +50,7 @@ namespace vmount { -InputRC::InputRC(bool do_stabilization, int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw) - : _do_stabilization(do_stabilization) +InputRC::InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw) { _aux_channels[0] = aux_channel_roll; _aux_channels[1] = aux_channel_pitch; @@ -131,11 +131,14 @@ bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bo _first_time = false; - for (int i = 0; i < 3; ++i) { - control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - control_data.type_data.angle.angles[i] = new_aux_values[i] * M_PI_F; - control_data.stabilize_axis[i] = _do_stabilization; + matrix::Eulerf euler(new_aux_values[0] * M_PI_F, new_aux_values[1] * M_PI_F, + new_aux_values[2] * M_PI_F); + matrix::Quatf q(euler); + q.copyTo(control_data.type_data.angle.q); + for (int i = 0; i < 3; ++i) { + // We always use follow mode with RC input for now. + control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; _last_set_aux_values[i] = new_aux_values[i]; } diff --git a/src/modules/vmount/input_rc.h b/src/modules/vmount/input_rc.h index 14c39c4053..89cf3a6034 100644 --- a/src/modules/vmount/input_rc.h +++ b/src/modules/vmount/input_rc.h @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -57,12 +57,11 @@ class InputRC : public InputBase public: /** - * @param do_stabilization * @param aux_channel_roll which aux channel to use for roll (set to 0 to use a fixed angle of 0) * @param aux_channel_pitch * @param aux_channel_yaw */ - InputRC(bool do_stabilization, int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw); + InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw); virtual ~InputRC(); virtual void print_status(); @@ -81,7 +80,6 @@ protected: float _get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx); private: - const bool _do_stabilization; int _aux_channels[3]; int _manual_control_setpoint_sub = -1; diff --git a/src/modules/vmount/input_test.cpp b/src/modules/vmount/input_test.cpp index 2654b8500e..4670521ec3 100644 --- a/src/modules/vmount/input_test.cpp +++ b/src/modules/vmount/input_test.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016-2017 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -42,6 +42,8 @@ #include #include +#include +#include namespace vmount @@ -69,11 +71,13 @@ int InputTest::update(unsigned int timeout_ms, ControlData **control_data, bool _control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame; _control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame; - for (int i = 0; i < 3; ++i) { - _control_data.type_data.angle.angles[i] = _angles[i] * M_DEG_TO_RAD_F; + matrix::Eulerf euler( + math::radians(_angles[0]), + math::radians(_angles[1]), + math::radians(_angles[2])); + matrix::Quatf q(euler); - _control_data.stabilize_axis[i] = true; - } + q.copyTo(_control_data.type_data.angle.q); _control_data.gimbal_shutter_retract = false; *control_data = &_control_data; diff --git a/src/modules/vmount/output.cpp b/src/modules/vmount/output.cpp index 706190eab9..913274aad0 100644 --- a/src/modules/vmount/output.cpp +++ b/src/modules/vmount/output.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -51,8 +51,6 @@ #include #include -using matrix::wrap_pi; - namespace vmount { @@ -95,19 +93,29 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data) { _cur_control_data = control_data; - for (int i = 0; i < 3; ++i) { - _stabilize[i] = control_data->stabilize_axis[i]; - _angle_speeds[i] = 0.f; - } - switch (control_data->type) { case ControlData::Type::Angle: - for (int i = 0; i < 3; ++i) { - if (control_data->type_data.angle.frames[i] == ControlData::TypeData::TypeAngle::Frame::AngularRate) { - _angle_speeds[i] = control_data->type_data.angle.angles[i]; - } else { - _angle_setpoints[i] = control_data->type_data.angle.angles[i]; + { + for (int i = 0; i < 3; ++i) { + switch (control_data->type_data.angle.frames[i]) { + case ControlData::TypeData::TypeAngle::Frame::AngularRate: + break; + + case ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame: + _absolute_angle[i] = false; + break; + + case ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame: + _absolute_angle[i] = true; + break; + } + + _angle_velocity[i] = control_data->type_data.angle.angular_velocity[i]; + } + + for (int i = 0; i < 4; ++i) { + _q_setpoint[i] = control_data->type_data.angle.q[i]; } } @@ -118,11 +126,19 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data) break; case ControlData::Type::Neutral: - _angle_setpoints[0] = 0.f; - _angle_setpoints[1] = 0.f; - _angle_setpoints[2] = 0.f; + _q_setpoint[0] = 1.f; + _q_setpoint[1] = 0.f; + _q_setpoint[2] = 0.f; + _q_setpoint[3] = 0.f; + _angle_velocity[0] = NAN; + _angle_velocity[1] = NAN; + _angle_velocity[2] = NAN; break; } + + for (int i = 0; i < 3; ++i) { + _stabilize[i] = control_data->stabilize_axis[i]; + } } void OutputBase::_handle_position_update(bool force_update) @@ -155,56 +171,69 @@ void OutputBase::_handle_position_update(bool force_update) const double &lon = _cur_control_data->type_data.lonlat.lon; const float &alt = _cur_control_data->type_data.lonlat.altitude; - _angle_setpoints[0] = _cur_control_data->type_data.lonlat.roll_angle; + float roll = _cur_control_data->type_data.lonlat.roll_angle; // interface: use fixed pitch value > -pi otherwise consider ROI altitude - if (_cur_control_data->type_data.lonlat.pitch_fixed_angle >= -M_PI_F) { - _angle_setpoints[1] = _cur_control_data->type_data.lonlat.pitch_fixed_angle; + float pitch = (_cur_control_data->type_data.lonlat.pitch_fixed_angle >= -M_PI_F) ? + _cur_control_data->type_data.lonlat.pitch_fixed_angle : + _calculate_pitch(lon, lat, alt, vehicle_global_position); - } else { - _angle_setpoints[1] = _calculate_pitch(lon, lat, alt, vehicle_global_position); - } - - _angle_setpoints[2] = get_bearing_to_next_waypoint(vlat, vlon, lat, lon) - vehicle_local_position.heading; + float yaw = get_bearing_to_next_waypoint(vlat, vlon, lat, lon) - vehicle_local_position.heading; // add offsets from VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET - _angle_setpoints[1] += _cur_control_data->type_data.lonlat.pitch_angle_offset; - _angle_setpoints[2] += _cur_control_data->type_data.lonlat.yaw_angle_offset; + pitch += _cur_control_data->type_data.lonlat.pitch_angle_offset; + yaw += _cur_control_data->type_data.lonlat.yaw_angle_offset; - // make sure yaw is wrapped correctly for the output - _angle_setpoints[2] = wrap_pi(_angle_setpoints[2]); + matrix::Quatf(matrix::Eulerf(roll, pitch, yaw)).copyTo(_q_setpoint); + + _angle_velocity[0] = NAN; + _angle_velocity[1] = NAN; + _angle_velocity[2] = NAN; } -void OutputBase::_calculate_output_angles(const hrt_abstime &t) +void OutputBase::_calculate_angle_output(const hrt_abstime &t) { - //take speed into account - float dt = (t - _last_update) / 1.e6f; - - for (int i = 0; i < 3; ++i) { - _angle_setpoints[i] += dt * _angle_speeds[i]; - } - //get the output angles and stabilize if necessary vehicle_attitude_s vehicle_attitude{}; - matrix::Eulerf euler; + matrix::Eulerf euler_vehicle; - if (_stabilize[0] || _stabilize[1] || _stabilize[2]) { - _vehicle_attitude_sub.copy(&vehicle_attitude); - euler = matrix::Quatf(vehicle_attitude.q); - } + // We only need to apply additional compensation if the required angle is + // absolute (world frame) as well as the gimbal is not capable of doing that + // calculation. (Most gimbals stabilize at least roll and pitch + // and only need compensation for yaw, if at all.) + bool compensate[3]; for (int i = 0; i < 3; ++i) { - if (_stabilize[i]) { - _angle_outputs[i] = _angle_setpoints[i] - euler(i); + compensate[i] = _stabilize[i] && _absolute_angle[i]; + } - } else { - _angle_outputs[i] = _angle_setpoints[i]; + if (compensate[0] || compensate[1] || compensate[2]) { + _vehicle_attitude_sub.copy(&vehicle_attitude); + euler_vehicle = matrix::Quatf(vehicle_attitude.q); + } + + float dt = (t - _last_update) / 1.e6f; + + matrix::Eulerf euler_gimbal = matrix::Quatf(_q_setpoint); + + for (int i = 0; i < 3; ++i) { + + if (PX4_ISFINITE(euler_gimbal(i))) { + _angle_outputs[i] = euler_gimbal(i); } - //bring angles into proper range [-pi, pi] - while (_angle_outputs[i] > M_PI_F) { _angle_outputs[i] -= 2.f * M_PI_F; } + if (PX4_ISFINITE(_angle_velocity[i])) { + _angle_outputs[i] += dt * _angle_velocity[i]; + } - while (_angle_outputs[i] < -M_PI_F) { _angle_outputs[i] += 2.f * M_PI_F; } + if (compensate[i]) { + _angle_outputs[i] -= euler_vehicle(i); + } + + if (PX4_ISFINITE(_angle_outputs[i])) { + //bring angles into proper range [-pi, pi] + _angle_outputs[i] = matrix::wrap_pi(_angle_outputs[i]); + } } } diff --git a/src/modules/vmount/output.h b/src/modules/vmount/output.h index 5b34a6c4d5..21e6f09275 100644 --- a/src/modules/vmount/output.h +++ b/src/modules/vmount/output.h @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -67,8 +67,8 @@ struct OutputConfig { float roll_offset; /**< Offset for roll channel in radians */ float yaw_offset; /**< Offset for yaw channel in radians */ - uint32_t mavlink_sys_id; /**< Mavlink target system id for mavlink output */ - uint32_t mavlink_comp_id; + uint32_t mavlink_sys_id_v1; /**< Mavlink target system id for mavlink output only for v1 */ + uint32_t mavlink_comp_id_v1; }; @@ -112,12 +112,18 @@ protected: void _handle_position_update(bool force_update = false); const ControlData *_cur_control_data = nullptr; - float _angle_setpoints[3] = { 0.f, 0.f, 0.f }; ///< [rad] - float _angle_speeds[3] = { 0.f, 0.f, 0.f }; + + float _q_setpoint[4] = { NAN, NAN, NAN, NAN }; ///< can be NAN if not specifically set + float _angle_velocity[3] = { NAN, NAN, NAN }; //< [rad/s], can be NAN if not specifically set + bool _stabilize[3] = { false, false, false }; + // Pitch and role are by default aligned with the horizon. + // Yaw follows the vehicle (not lock/absolute mode). + bool _absolute_angle[3] = {true, true, false }; + /** calculate the _angle_outputs (with speed) and stabilize if needed */ - void _calculate_output_angles(const hrt_abstime &t); + void _calculate_angle_output(const hrt_abstime &t); float _angle_outputs[3] = { 0.f, 0.f, 0.f }; ///< calculated output angles (roll, pitch, yaw) [rad] hrt_abstime _last_update; diff --git a/src/modules/vmount/output_mavlink.cpp b/src/modules/vmount/output_mavlink.cpp index 9babd00497..c8ea6555de 100644 --- a/src/modules/vmount/output_mavlink.cpp +++ b/src/modules/vmount/output_mavlink.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016-2019 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,6 +41,7 @@ #include "output_mavlink.h" #include +#include #include #include @@ -49,17 +50,17 @@ namespace vmount { -OutputMavlink::OutputMavlink(const OutputConfig &output_config) +OutputMavlinkV1::OutputMavlinkV1(const OutputConfig &output_config) : OutputBase(output_config) { } -int OutputMavlink::update(const ControlData *control_data) +int OutputMavlinkV1::update(const ControlData *control_data) { vehicle_command_s vehicle_command{}; vehicle_command.timestamp = hrt_absolute_time(); - vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id; - vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id; + vehicle_command.target_system = (uint8_t)_config.mavlink_sys_id_v1; + vehicle_command.target_component = (uint8_t)_config.mavlink_comp_id_v1; if (control_data) { //got new command @@ -83,9 +84,9 @@ int OutputMavlink::update(const ControlData *control_data) vehicle_command.param7 = static_cast(control_data->type_data.angle.frames[2]); } - vehicle_command.param2 = control_data->stabilize_axis[0]; - vehicle_command.param3 = control_data->stabilize_axis[1]; - vehicle_command.param4 = control_data->stabilize_axis[2]; + vehicle_command.param2 = _stabilize[0] ? 1.0f : 0.0f; + vehicle_command.param3 = _stabilize[1] ? 1.0f : 0.0f; + vehicle_command.param4 = _stabilize[2] ? 1.0f : 0.0f; _vehicle_command_pub.publish(vehicle_command); } @@ -93,16 +94,16 @@ int OutputMavlink::update(const ControlData *control_data) _handle_position_update(); hrt_abstime t = hrt_absolute_time(); - _calculate_output_angles(t); + _calculate_angle_output(t); vehicle_command.timestamp = t; vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL; // vmount spec has roll, pitch on channels 0, 1, respectively; MAVLink spec has roll, pitch on channels 1, 0, respectively // vmount uses radians, MAVLink uses degrees - vehicle_command.param1 = (_angle_outputs[1] + _config.pitch_offset) * M_RAD_TO_DEG_F; - vehicle_command.param2 = (_angle_outputs[0] + _config.roll_offset) * M_RAD_TO_DEG_F; - vehicle_command.param3 = (_angle_outputs[2] + _config.yaw_offset) * M_RAD_TO_DEG_F; + vehicle_command.param1 = math::degrees(_angle_outputs[1] + _config.pitch_offset); + vehicle_command.param2 = math::degrees(_angle_outputs[0] + _config.roll_offset); + vehicle_command.param3 = math::degrees(_angle_outputs[2] + _config.yaw_offset); vehicle_command.param7 = 2.0f; // MAV_MOUNT_MODE_MAVLINK_TARGETING; _vehicle_command_pub.publish(vehicle_command); @@ -112,10 +113,102 @@ int OutputMavlink::update(const ControlData *control_data) return 0; } -void OutputMavlink::print_status() +void OutputMavlinkV1::print_status() { - PX4_INFO("Output: Mavlink"); + PX4_INFO("Output: MAVLink gimbal protocol v1"); +} + +OutputMavlinkV2::OutputMavlinkV2(int32_t mav_sys_id, int32_t mav_comp_id, const OutputConfig &output_config) + : OutputBase(output_config), + _mav_sys_id(mav_sys_id), + _mav_comp_id(mav_comp_id) +{ +} + +int OutputMavlinkV2::update(const ControlData *control_data) +{ + _check_for_gimbal_device_information(); + + hrt_abstime t = hrt_absolute_time(); + + if (!_gimbal_device_found && t - _last_gimbal_device_checked > 1000000) { + _request_gimbal_device_information(); + _last_gimbal_device_checked = t; + + } else { + if (control_data) { + //got new command + _set_angle_setpoints(control_data); + } + + _handle_position_update(); + _publish_gimbal_device_set_attitude(); + _last_update = t; + } + + return 0; +} + +void OutputMavlinkV2::_request_gimbal_device_information() +{ + vehicle_command_s vehicle_cmd{}; + vehicle_cmd.timestamp = hrt_absolute_time(); + vehicle_cmd.command = vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE; + vehicle_cmd.param1 = vehicle_command_s::VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION; + vehicle_cmd.target_system = 0; + vehicle_cmd.target_component = 0; + vehicle_cmd.source_system = _mav_sys_id; + vehicle_cmd.source_component = _mav_comp_id; + vehicle_cmd.confirmation = 0; + vehicle_cmd.from_external = false; + + uORB::Publication vehicle_command_pub{ORB_ID(vehicle_command)}; + vehicle_command_pub.publish(vehicle_cmd); +} + +void OutputMavlinkV2::_check_for_gimbal_device_information() +{ + gimbal_device_information_s gimbal_device_information; + + if (_gimbal_device_information_sub.update(&gimbal_device_information)) { + _gimbal_device_found = true; + _gimbal_device_compid = gimbal_device_information.gimbal_device_compid; + } +} + +void OutputMavlinkV2::print_status() +{ + PX4_INFO("Output: MAVLink gimbal protocol v2"); +} + +void OutputMavlinkV2::_publish_gimbal_device_set_attitude() +{ + gimbal_device_set_attitude_s set_attitude{}; + set_attitude.timestamp = hrt_absolute_time(); + set_attitude.target_system = (uint8_t)_mav_sys_id; + set_attitude.target_component = _gimbal_device_compid; + + set_attitude.angular_velocity_x = _angle_velocity[0]; + set_attitude.angular_velocity_y = _angle_velocity[1]; + set_attitude.angular_velocity_z = _angle_velocity[2]; + set_attitude.q[0] = _q_setpoint[0]; + set_attitude.q[1] = _q_setpoint[1]; + set_attitude.q[2] = _q_setpoint[2]; + set_attitude.q[3] = _q_setpoint[3]; + + if (_absolute_angle[0]) { + set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_ROLL_LOCK; + } + + if (_absolute_angle[1]) { + set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_PITCH_LOCK; + } + + if (_absolute_angle[2]) { + set_attitude.flags |= gimbal_device_set_attitude_s::GIMBAL_DEVICE_FLAGS_YAW_LOCK; + } + + _gimbal_device_set_attitude_pub.publish(set_attitude); } } /* namespace vmount */ - diff --git a/src/modules/vmount/output_mavlink.h b/src/modules/vmount/output_mavlink.h index 5ace7f6582..89eb4a67c4 100644 --- a/src/modules/vmount/output_mavlink.h +++ b/src/modules/vmount/output_mavlink.h @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2016 PX4 Development Team. All rights reserved. +* Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -43,6 +43,9 @@ #include #include +#include +#include + namespace vmount { @@ -50,20 +53,43 @@ namespace vmount ** class OutputMavlink * Output via vehicle_command topic */ -class OutputMavlink : public OutputBase +class OutputMavlinkV1 : public OutputBase { public: - OutputMavlink(const OutputConfig &output_config); - virtual ~OutputMavlink() = default; + OutputMavlinkV1(const OutputConfig &output_config); + virtual ~OutputMavlinkV1() = default; virtual int update(const ControlData *control_data); virtual void print_status(); private: - uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; }; +class OutputMavlinkV2 : public OutputBase +{ +public: + OutputMavlinkV2(int32_t mav_sys_id, int32_t mav_comp_id, const OutputConfig &output_config); + virtual ~OutputMavlinkV2() = default; + + virtual int update(const ControlData *control_data); + + virtual void print_status(); + +private: + void _publish_gimbal_device_set_attitude(); + void _request_gimbal_device_information(); + void _check_for_gimbal_device_information(); + + uORB::Publication _gimbal_device_set_attitude_pub{ORB_ID(gimbal_device_set_attitude)}; + uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)}; + + int32_t _mav_sys_id{1}; ///< our mavlink system id + int32_t _mav_comp_id{1}; ///< our mavlink component id + uint8_t _gimbal_device_compid{0}; + hrt_abstime _last_gimbal_device_checked{0}; + bool _gimbal_device_found {false}; +}; } /* namespace vmount */ diff --git a/src/modules/vmount/output_rc.cpp b/src/modules/vmount/output_rc.cpp index 196772a6fc..04d2df39f1 100644 --- a/src/modules/vmount/output_rc.cpp +++ b/src/modules/vmount/output_rc.cpp @@ -42,7 +42,7 @@ #include #include - +#include namespace vmount { @@ -63,7 +63,7 @@ int OutputRC::update(const ControlData *control_data) _handle_position_update(); hrt_abstime t = hrt_absolute_time(); - _calculate_output_angles(t); + _calculate_angle_output(t); actuator_controls_s actuator_controls{}; actuator_controls.timestamp = hrt_absolute_time(); @@ -73,6 +73,8 @@ int OutputRC::update(const ControlData *control_data) actuator_controls.control[2] = (_angle_outputs[2] + _config.yaw_offset) * _config.yaw_scale; actuator_controls.control[3] = _retract_gimbal ? _config.gimbal_retracted_mode_value : _config.gimbal_normal_mode_value; + _stream_device_attitude_status(); + _actuator_controls_pub.publish(actuator_controls); _last_update = t; @@ -85,5 +87,23 @@ void OutputRC::print_status() PX4_INFO("Output: AUX"); } -} /* namespace vmount */ +void OutputRC::_stream_device_attitude_status() +{ + gimbal_device_attitude_status_s attitude_status{}; + attitude_status.timestamp = hrt_absolute_time(); + attitude_status.target_system = 0; + attitude_status.target_component = 0; + attitude_status.device_flags = gimbal_device_attitude_status_s::DEVICE_FLAGS_NEUTRAL | + gimbal_device_attitude_status_s::DEVICE_FLAGS_ROLL_LOCK | + gimbal_device_attitude_status_s::DEVICE_FLAGS_PITCH_LOCK | + gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_LOCK; + matrix::Eulerf euler(_angle_outputs[0], _angle_outputs[1], _angle_outputs[2]); + matrix::Quatf q(euler); + q.copyTo(attitude_status.q); + + attitude_status.failure_flags = 0; + _attitude_status_pub.publish(attitude_status); +} + +} /* namespace vmount */ diff --git a/src/modules/vmount/output_rc.h b/src/modules/vmount/output_rc.h index 340e913cc7..52839cac69 100644 --- a/src/modules/vmount/output_rc.h +++ b/src/modules/vmount/output_rc.h @@ -43,6 +43,7 @@ #include #include +#include namespace vmount { @@ -63,7 +64,10 @@ public: virtual void print_status(); private: + void _stream_device_attitude_status(); + uORB::Publication _actuator_controls_pub{ORB_ID(actuator_controls_2)}; + uORB::Publication _attitude_status_pub{ORB_ID(gimbal_device_attitude_status)}; bool _retract_gimbal = true; }; diff --git a/src/modules/vmount/vmount.cpp b/src/modules/vmount/vmount.cpp index d853d3bd0c..046b76b2c9 100644 --- a/src/modules/vmount/vmount.cpp +++ b/src/modules/vmount/vmount.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -88,8 +88,8 @@ static volatile ThreadData *g_thread_data = nullptr; struct Parameters { int32_t mnt_mode_in; int32_t mnt_mode_out; - int32_t mnt_mav_sysid; - int32_t mnt_mav_compid; + int32_t mnt_mav_sys_id_v1; + int32_t mnt_mav_comp_id_v1; float mnt_ob_lock_mode; float mnt_ob_norm_mode; int32_t mnt_man_pitch; @@ -102,6 +102,10 @@ struct Parameters { float mnt_off_pitch; float mnt_off_roll; float mnt_off_yaw; + int32_t mav_sys_id; + int32_t mav_comp_id; + float mnt_rate_pitch; + float mnt_rate_yaw; bool operator!=(const Parameters &p) { @@ -109,8 +113,8 @@ struct Parameters { #pragma GCC diagnostic ignored "-Wfloat-equal" return mnt_mode_in != p.mnt_mode_in || mnt_mode_out != p.mnt_mode_out || - mnt_mav_sysid != p.mnt_mav_sysid || - mnt_mav_compid != p.mnt_mav_compid || + mnt_mav_sys_id_v1 != p.mnt_mav_sys_id_v1 || + mnt_mav_comp_id_v1 != p.mnt_mav_comp_id_v1 || fabsf(mnt_ob_lock_mode - p.mnt_ob_lock_mode) > 1e-6f || fabsf(mnt_ob_norm_mode - p.mnt_ob_norm_mode) > 1e-6f || mnt_man_pitch != p.mnt_man_pitch || @@ -122,7 +126,9 @@ struct Parameters { mnt_range_yaw != p.mnt_range_yaw || mnt_off_pitch != p.mnt_off_pitch || mnt_off_roll != p.mnt_off_roll || - mnt_off_yaw != p.mnt_off_yaw; + mnt_off_yaw != p.mnt_off_yaw || + mav_sys_id != p.mav_sys_id || + mav_comp_id != p.mav_comp_id; #pragma GCC diagnostic pop } @@ -131,8 +137,8 @@ struct Parameters { struct ParameterHandles { param_t mnt_mode_in; param_t mnt_mode_out; - param_t mnt_mav_sysid; - param_t mnt_mav_compid; + param_t mnt_mav_sys_id_v1; + param_t mnt_mav_comp_id_v1; param_t mnt_ob_lock_mode; param_t mnt_ob_norm_mode; param_t mnt_man_pitch; @@ -145,6 +151,10 @@ struct ParameterHandles { param_t mnt_off_pitch; param_t mnt_off_roll; param_t mnt_off_yaw; + param_t mav_sys_id; + param_t mav_comp_id; + param_t mnt_rate_pitch; + param_t mnt_rate_yaw; }; @@ -223,14 +233,14 @@ static int vmount_thread_main(int argc, char *argv[]) output_config.gimbal_normal_mode_value = params.mnt_ob_norm_mode; output_config.gimbal_retracted_mode_value = params.mnt_ob_lock_mode; - output_config.pitch_scale = 1.0f / ((params.mnt_range_pitch / 2.0f) * M_DEG_TO_RAD_F); - output_config.roll_scale = 1.0f / ((params.mnt_range_roll / 2.0f) * M_DEG_TO_RAD_F); - output_config.yaw_scale = 1.0f / ((params.mnt_range_yaw / 2.0f) * M_DEG_TO_RAD_F); - output_config.pitch_offset = params.mnt_off_pitch * M_DEG_TO_RAD_F; - output_config.roll_offset = params.mnt_off_roll * M_DEG_TO_RAD_F; - output_config.yaw_offset = params.mnt_off_yaw * M_DEG_TO_RAD_F; - output_config.mavlink_sys_id = params.mnt_mav_sysid; - output_config.mavlink_comp_id = params.mnt_mav_compid; + output_config.pitch_scale = 1.0f / (math::radians(params.mnt_range_pitch / 2.0f)); + output_config.roll_scale = 1.0f / (math::radians(params.mnt_range_roll / 2.0f)); + output_config.yaw_scale = 1.0f / (math::radians(params.mnt_range_yaw / 2.0f)); + output_config.pitch_offset = math::radians(params.mnt_off_pitch); + output_config.roll_offset = math::radians(params.mnt_off_roll); + output_config.yaw_offset = math::radians(params.mnt_off_yaw); + output_config.mavlink_sys_id_v1 = params.mnt_mav_sys_id_v1; + output_config.mavlink_comp_id_v1 = params.mnt_mav_comp_id_v1; bool alloc_failed = false; thread_data.input_objs_len = 1; @@ -243,20 +253,22 @@ static int vmount_thread_main(int argc, char *argv[]) case 0: // Automatic - thread_data.input_objs[0] = new InputMavlinkCmdMount(params.mnt_do_stab); + thread_data.input_objs[0] = new InputMavlinkCmdMount(); thread_data.input_objs[1] = new InputMavlinkROI(); // RC is on purpose last here so that if there are any mavlink // messages, they will take precedence over RC. // This logic is done further below while update() is called. - thread_data.input_objs[2] = new InputRC(params.mnt_do_stab, params.mnt_man_roll, params.mnt_man_pitch, + thread_data.input_objs[2] = new InputRC(params.mnt_man_roll, + params.mnt_man_pitch, params.mnt_man_yaw); thread_data.input_objs_len = 3; break; case 1: //RC - thread_data.input_objs[0] = new InputRC(params.mnt_do_stab, params.mnt_man_roll, params.mnt_man_pitch, + thread_data.input_objs[0] = new InputRC(params.mnt_man_roll, + params.mnt_man_pitch, params.mnt_man_yaw); break; @@ -265,7 +277,15 @@ static int vmount_thread_main(int argc, char *argv[]) break; case 3: //MAVLINK_DO_MOUNT - thread_data.input_objs[0] = new InputMavlinkCmdMount(params.mnt_do_stab); + thread_data.input_objs[0] = new InputMavlinkCmdMount(); + break; + + case 4: //MAVLINK_V2 + thread_data.input_objs[0] = new InputMavlinkGimbalV2( + params.mav_sys_id, + params.mav_comp_id, + params.mnt_rate_pitch, + params.mnt_rate_yaw); break; default: @@ -288,8 +308,15 @@ static int vmount_thread_main(int argc, char *argv[]) break; - case 1: //MAVLINK - thread_data.output_obj = new OutputMavlink(output_config); + case 1: //MAVLink v1 gimbal protocol + thread_data.output_obj = new OutputMavlinkV1(output_config); + + if (!thread_data.output_obj) { alloc_failed = true; } + + break; + + case 2: //MAVLink v2 gimbal protocol + thread_data.output_obj = new OutputMavlinkV2(params.mav_sys_id, params.mav_comp_id, output_config); if (!thread_data.output_obj) { alloc_failed = true; } @@ -327,6 +354,17 @@ static int vmount_thread_main(int argc, char *argv[]) for (int i = 0; i < thread_data.input_objs_len; ++i) { + if (params.mnt_do_stab == 1) { + thread_data.input_objs[i]->set_stabilize(true, true, true); + + } else if (params.mnt_do_stab == 2) { + thread_data.input_objs[i]->set_stabilize(false, false, true); + + } else { + thread_data.input_objs[i]->set_stabilize(false, false, false); + } + + bool already_active = (last_active == i); ControlData *control_data_to_check = nullptr; @@ -352,9 +390,9 @@ static int vmount_thread_main(int argc, char *argv[]) break; } - //only publish the mount orientation if the mode is not mavlink - //if the gimbal speaks mavlink it publishes its own orientation - if (params.mnt_mode_out != 1) { // 1 = MAVLINK + // Only publish the mount orientation if the mode is not mavlink v1 or v2 + // If the gimbal speaks mavlink it publishes its own orientation. + if (params.mnt_mode_out != 1 && params.mnt_mode_out != 2) { // 1 = MAVLink v1, 2 = MAVLink v2 thread_data.output_obj->publish(); } @@ -527,8 +565,8 @@ void update_params(ParameterHandles ¶m_handles, Parameters ¶ms, bool &go Parameters prev_params = params; param_get(param_handles.mnt_mode_in, ¶ms.mnt_mode_in); param_get(param_handles.mnt_mode_out, ¶ms.mnt_mode_out); - param_get(param_handles.mnt_mav_sysid, ¶ms.mnt_mav_sysid); - param_get(param_handles.mnt_mav_compid, ¶ms.mnt_mav_compid); + param_get(param_handles.mnt_mav_sys_id_v1, ¶ms.mnt_mav_sys_id_v1); + param_get(param_handles.mnt_mav_comp_id_v1, ¶ms.mnt_mav_comp_id_v1); param_get(param_handles.mnt_ob_lock_mode, ¶ms.mnt_ob_lock_mode); param_get(param_handles.mnt_ob_norm_mode, ¶ms.mnt_ob_norm_mode); param_get(param_handles.mnt_man_pitch, ¶ms.mnt_man_pitch); @@ -541,6 +579,10 @@ void update_params(ParameterHandles ¶m_handles, Parameters ¶ms, bool &go param_get(param_handles.mnt_off_pitch, ¶ms.mnt_off_pitch); param_get(param_handles.mnt_off_roll, ¶ms.mnt_off_roll); param_get(param_handles.mnt_off_yaw, ¶ms.mnt_off_yaw); + param_get(param_handles.mav_sys_id, ¶ms.mav_sys_id); + param_get(param_handles.mav_comp_id, ¶ms.mav_comp_id); + param_get(param_handles.mnt_rate_pitch, ¶ms.mnt_rate_pitch); + param_get(param_handles.mnt_rate_yaw, ¶ms.mnt_rate_yaw); got_changes = prev_params != params; } @@ -549,8 +591,8 @@ bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms) { param_handles.mnt_mode_in = param_find("MNT_MODE_IN"); param_handles.mnt_mode_out = param_find("MNT_MODE_OUT"); - param_handles.mnt_mav_sysid = param_find("MNT_MAV_SYSID"); - param_handles.mnt_mav_compid = param_find("MNT_MAV_COMPID"); + param_handles.mnt_mav_sys_id_v1 = param_find("MNT_MAV_SYSID"); + param_handles.mnt_mav_comp_id_v1 = param_find("MNT_MAV_COMPID"); param_handles.mnt_ob_lock_mode = param_find("MNT_OB_LOCK_MODE"); param_handles.mnt_ob_norm_mode = param_find("MNT_OB_NORM_MODE"); param_handles.mnt_man_pitch = param_find("MNT_MAN_PITCH"); @@ -563,11 +605,15 @@ bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_handles.mnt_off_pitch = param_find("MNT_OFF_PITCH"); param_handles.mnt_off_roll = param_find("MNT_OFF_ROLL"); param_handles.mnt_off_yaw = param_find("MNT_OFF_YAW"); + param_handles.mav_sys_id = param_find("MAV_SYS_ID"); + param_handles.mav_comp_id = param_find("MAV_COMP_ID"); + param_handles.mnt_rate_pitch = param_find("MNT_RATE_PITCH"); + param_handles.mnt_rate_yaw = param_find("MNT_RATE_YAW"); if (param_handles.mnt_mode_in == PARAM_INVALID || param_handles.mnt_mode_out == PARAM_INVALID || - param_handles.mnt_mav_sysid == PARAM_INVALID || - param_handles.mnt_mav_compid == PARAM_INVALID || + param_handles.mnt_mav_sys_id_v1 == PARAM_INVALID || + param_handles.mnt_mav_comp_id_v1 == PARAM_INVALID || param_handles.mnt_ob_lock_mode == PARAM_INVALID || param_handles.mnt_ob_norm_mode == PARAM_INVALID || param_handles.mnt_man_pitch == PARAM_INVALID || @@ -579,7 +625,12 @@ bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_handles.mnt_range_yaw == PARAM_INVALID || param_handles.mnt_off_pitch == PARAM_INVALID || param_handles.mnt_off_roll == PARAM_INVALID || - param_handles.mnt_off_yaw == PARAM_INVALID) { + param_handles.mnt_off_yaw == PARAM_INVALID || + param_handles.mav_sys_id == PARAM_INVALID || + param_handles.mav_comp_id == PARAM_INVALID || + param_handles.mnt_rate_pitch == PARAM_INVALID || + param_handles.mnt_rate_yaw == PARAM_INVALID + ) { return false; } diff --git a/src/modules/vmount/vmount_params.c b/src/modules/vmount/vmount_params.c index 556987360e..565b729db2 100644 --- a/src/modules/vmount/vmount_params.c +++ b/src/modules/vmount/vmount_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved. +* Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -48,8 +48,9 @@ * @value -1 DISABLED * @value 0 AUTO * @value 1 RC -* @value 2 MAVLINK_ROI -* @value 3 MAVLINK_DO_MOUNT +* @value 2 MAVLINK_ROI (protocol v1) +* @value 3 MAVLINK_DO_MOUNT (protocol v1) +* @value 4 MAVlink gimbal protocol v2 * @min -1 * @max 3 * @group Mount @@ -65,9 +66,10 @@ PARAM_DEFINE_INT32(MNT_MODE_IN, -1); * to control a mount (set MNT_MAV_SYSID & MNT_MAV_COMPID) * * @value 0 AUX -* @value 1 MAVLINK +* @value 1 MAVLink gimbal protocol v1 +* @value 2 MAVLink gimbal protocol v2 * @min 0 -* @max 1 +* @max 2 * @group Mount */ PARAM_DEFINE_INT32(MNT_MODE_OUT, 0); @@ -75,7 +77,7 @@ PARAM_DEFINE_INT32(MNT_MODE_OUT, 0); /** * Mavlink System ID of the mount * -* If MNT_MODE_OUT is MAVLINK, mount configure/control commands will be sent with this target ID. +* If MNT_MODE_OUT is MAVLink gimbal protocol v1, mount configure/control commands will be sent with this target ID. * * @group Mount */ @@ -84,7 +86,7 @@ PARAM_DEFINE_INT32(MNT_MAV_SYSID, 1); /** * Mavlink Component ID of the mount * -* If MNT_MODE_OUT is MAVLINK, mount configure/control commands will be sent with this component ID. +* If MNT_MODE_OUT is MAVLink protocol v2, mount configure/control commands will be sent with this component ID. * * @group Mount */ @@ -162,9 +164,14 @@ PARAM_DEFINE_INT32(MNT_MAN_YAW, 0); /** * Stabilize the mount (set to true for servo gimbal, false for passthrough). -* Does not affect MAVLINK_ROI input. +* (This is required for a gimbal which is not capable of stabilizing itself +* and relies on the IMU's attitude estimation.) * -* @boolean +* @value 0 Disable +* @value 1 Stabilize all axis +* @value 2 Stabilize yaw for absolute/lock mode. +* @min 0 +* @max 2 * @group Mount */ PARAM_DEFINE_INT32(MNT_DO_STAB, 0); @@ -228,3 +235,23 @@ PARAM_DEFINE_FLOAT(MNT_OFF_ROLL, 0.0f); * @group Mount */ PARAM_DEFINE_FLOAT(MNT_OFF_YAW, 0.0f); + +/** + * Angular pitch rate for manual input in degrees/second. + * Full stick input [-1..1] translats to [-pitch rate..pitch rate]. + * + * @min 1.0 + * @max 90.0 + * @group Mount + */ +PARAM_DEFINE_FLOAT(MNT_RATE_PITCH, 30.0f); + +/** + * Angular yaw rate for manual input in degrees/second. + * Full stick input [-1..1] translats to [-yaw rate..yaw rate]. + * + * @min 1.0 + * @max 90.0 + * @group Mount + */ +PARAM_DEFINE_FLOAT(MNT_RATE_YAW, 30.0f); diff --git a/src/modules/vtol_att_control/standard_params.c b/src/modules/vtol_att_control/standard_params.c index 853aa908d6..37ac140eae 100644 --- a/src/modules/vtol_att_control/standard_params.c +++ b/src/modules/vtol_att_control/standard_params.c @@ -53,6 +53,8 @@ * @value 2 Enable FW forward actuation in hover in altitude, position and auto modes if above MPC_LAND_ALT1. * @value 3 Enable FW forward actuation in hover in altitude, position and auto modes if above MPC_LAND_ALT2. * @value 4 Enable FW forward actuation in hover in altitude, position and auto modes. + * @value 5 Enable FW forward actuation in hover in altitude, position and auto modes if above MPC_LAND_ALT1 (except LANDING). + * @value 6 Enable FW forward actuation in hover in altitude, position and auto modes if above MPC_LAND_ALT2 (except LANDING). * * @group VTOL Attitude Control */ diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 27b93b9362..031d1d2f13 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -497,6 +497,26 @@ float VtolType::pusher_assist() return 0.0f; } + break; + + case ENABLE_ABOVE_MPC_LAND_ALT1_WITHOUT_LAND: // disable if below MPC_LAND_ALT1 or in land mode + if ((_attc->get_pos_sp_triplet()->current.valid + && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND + && _v_control_mode->flag_control_auto_enabled) || + (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt1))) { + return 0.0f; + } + + break; + + case ENABLE_ABOVE_MPC_LAND_ALT2_WITHOUT_LAND: // disable if below MPC_LAND_ALT2 or in land mode + if ((_attc->get_pos_sp_triplet()->current.valid + && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND + && _v_control_mode->flag_control_auto_enabled) || + (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt2))) { + return 0.0f; + } + break; } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 355fe534fb..68c5a89cb6 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -100,7 +100,9 @@ enum VtolForwardActuationMode { ENABLE_WITHOUT_LAND, ENABLE_ABOVE_MPC_LAND_ALT1, ENABLE_ABOVE_MPC_LAND_ALT2, - ENABLE_ALL_MODES + ENABLE_ALL_MODES, + ENABLE_ABOVE_MPC_LAND_ALT1_WITHOUT_LAND, + ENABLE_ABOVE_MPC_LAND_ALT2_WITHOUT_LAND }; // these are states that can be applied to a selection of multirotor motors. diff --git a/src/systemcmds/uorb/CMakeLists.txt b/src/systemcmds/uorb/CMakeLists.txt new file mode 100644 index 0000000000..2c1362394b --- /dev/null +++ b/src/systemcmds/uorb/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__uorb + MAIN uorb + COMPILE_FLAGS + SRCS + uorb.cpp + DEPENDS + ) diff --git a/src/modules/uORB/uORBMain.cpp b/src/systemcmds/uorb/uorb.cpp similarity index 64% rename from src/modules/uORB/uORBMain.cpp rename to src/systemcmds/uorb/uorb.cpp index 84e05f34ff..b9fb565c14 100644 --- a/src/modules/uORB/uORBMain.cpp +++ b/src/systemcmds/uorb/uorb.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,36 +33,49 @@ #include -#include "uORBManager.hpp" -#include "uORB.h" -#include "uORBCommon.hpp" +#include #include #include extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } -static uORB::DeviceMaster *g_dev = nullptr; -static void usage() +static void usage(); + +int uorb_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + return -1; + } + + if (!strcmp(argv[1], "start")) { + return uorb_start(); + + } else if (!strcmp(argv[1], "status")) { + return uorb_status(); + + } else if (!strcmp(argv[1], "top")) { + return uorb_top(argv + 2, argc - 2); + } + + usage(); + return 0; +} + +void usage() { PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description uORB is the internal pub-sub messaging system, used for communication between modules. -It is typically started as one of the very first modules and most other modules depend on it. - ### Implementation -No thread or work queue is needed, the module start only makes sure to initialize the shared global state. -Communication is done via shared memory. The implementation is asynchronous and lock-free, ie. a publisher does not wait for a subscriber and vice versa. This is achieved by having a separate buffer between a publisher and a subscriber. The code is optimized to minimize the memory footprint and the latency to exchange messages. -The interface is based on file descriptors: internally it uses `read`, `write` and `ioctl`. Except for the -publications, which use `orb_advert_t` handles, so that they can be used from interrupts as well (on NuttX). - Messages are defined in the `/msg` directory. They are converted into C/C++ code at build-time. If compiled with ORB_USE_PUBLISHER_RULES, a file with uORB publication rules can be used to configure which @@ -74,80 +87,9 @@ $ uorb top )DESCR_STR"); PRINT_MODULE_USAGE_NAME("uorb", "communication"); - PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print topic statistics"); PRINT_MODULE_USAGE_COMMAND_DESCR("top", "Monitor topic publication rates"); PRINT_MODULE_USAGE_PARAM_FLAG('a', "print all instead of only currently publishing topics with subscribers", true); PRINT_MODULE_USAGE_PARAM_FLAG('1', "run only once, then exit", true); PRINT_MODULE_USAGE_ARG(" []", "topic(s) to match (implies -a)", true); } - -int -uorb_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage(); - return -EINVAL; - } - - /* - * Start/load the driver. - */ - if (!strcmp(argv[1], "start")) { - - if (g_dev != nullptr) { - PX4_WARN("already loaded"); - /* user wanted to start uorb, its already running, no error */ - return 0; - } - - if (!uORB::Manager::initialize()) { - PX4_ERR("uorb manager alloc failed"); - return -ENOMEM; - } - - /* create the driver */ - g_dev = uORB::Manager::get_instance()->get_device_master(); - - if (g_dev == nullptr) { - return -errno; - } - -#if !defined(__PX4_QURT) - /* FIXME: this fails on Snapdragon (see https://github.com/PX4/Firmware/issues/5406), - * so we disable logging messages to the ulog for now. This needs further investigations. - */ - px4_log_initialize(); -#endif - - return OK; - } - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "status")) { - if (g_dev != nullptr) { - g_dev->printStatistics(); - - } else { - PX4_INFO("uorb is not running"); - } - - return OK; - } - - if (!strcmp(argv[1], "top")) { - if (g_dev != nullptr) { - g_dev->showTop(argv + 2, argc - 2); - - } else { - PX4_INFO("uorb is not running"); - } - - return OK; - } - - usage(); - return -EINVAL; -} diff --git a/src/templates/template_module/CMakeLists.txt b/src/templates/template_module/CMakeLists.txt index 19e9e5d313..ddda8ba394 100644 --- a/src/templates/template_module/CMakeLists.txt +++ b/src/templates/template_module/CMakeLists.txt @@ -36,7 +36,5 @@ px4_add_module( MAIN template_module SRCS template_module.cpp - DEPENDS - modules__uORB ) diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index 693f49af26..3a4e19aca7 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -45,7 +45,7 @@ void AutopilotTester::connect(const std::string uri) std::cout << time_str() << "Waiting for system connect" << std::endl; REQUIRE(poll_condition_with_timeout( - [this]() { return _mavsdk.is_connected(); }, adjust_to_lockstep_speed(std::chrono::seconds(25)))); + [this]() { return _mavsdk.is_connected(); }, std::chrono::seconds(25))); auto &system = _mavsdk.system(); @@ -179,15 +179,7 @@ void AutopilotTester::prepare_square_mission(MissionOptions mission_options) _mission->set_return_to_launch_after_mission(mission_options.rtl_at_end); - std::promise prom; - auto fut = prom.get_future(); - - _mission->upload_mission_async(mission_plan, [&prom](Mission::Result result) { - REQUIRE(Mission::Result::Success == result); - prom.set_value(); - }); - - REQUIRE(fut.wait_for(std::chrono::seconds(2)) == std::future_status::ready); + REQUIRE(_mission->upload_mission(mission_plan) == Mission::Result::Success); } void AutopilotTester::prepare_straight_mission(MissionOptions mission_options) @@ -203,15 +195,7 @@ void AutopilotTester::prepare_straight_mission(MissionOptions mission_options) _mission->set_return_to_launch_after_mission(mission_options.rtl_at_end); - std::promise prom; - auto fut = prom.get_future(); - - _mission->upload_mission_async(mission_plan, [&prom](Mission::Result result) { - REQUIRE(Mission::Result::Success == result); - prom.set_value(); - }); - - REQUIRE(fut.wait_for(std::chrono::seconds(2)) == std::future_status::ready); + REQUIRE(_mission->upload_mission(mission_plan) == Mission::Result::Success); } void AutopilotTester::execute_mission() @@ -361,29 +345,29 @@ void AutopilotTester::fly_forward_in_posctl() const unsigned manual_control_rate_hz = 50; // Send something to make sure RC is available. - for (unsigned i = 0; i < 5 * manual_control_rate_hz; ++i) { + for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) { CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); - std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz))); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); } CHECK(_manual_control->start_position_control() == ManualControl::Result::Success); - // Climb up for 10 seconds - for (unsigned i = 0; i < 10 * manual_control_rate_hz; ++i) { + // Climb up for 20 seconds + for (unsigned i = 0; i < 20 * manual_control_rate_hz; ++i) { CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 1.f, 0.f) == ManualControl::Result::Success); - std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz))); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); } // Fly forward for 60 seconds for (unsigned i = 0; i < 60 * manual_control_rate_hz; ++i) { CHECK(_manual_control->set_manual_control_input(0.5f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); - std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz))); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); } // Descend until disarmed for (unsigned i = 0; i < 60 * manual_control_rate_hz; ++i) { CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.0f, 0.f) == ManualControl::Result::Success); - std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz))); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); if (!_telemetry->in_air()) { break; @@ -396,29 +380,29 @@ void AutopilotTester::fly_forward_in_altctl() const unsigned manual_control_rate_hz = 50; // Send something to make sure RC is available. - for (unsigned i = 0; i < 5 * manual_control_rate_hz; ++i) { + for (unsigned i = 0; i < 1 * manual_control_rate_hz; ++i) { CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); - std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz))); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); } CHECK(_manual_control->start_altitude_control() == ManualControl::Result::Success); - // Climb up for 10 seconds - for (unsigned i = 0; i < 10 * manual_control_rate_hz; ++i) { + // Climb up for 20 seconds + for (unsigned i = 0; i < 20 * manual_control_rate_hz; ++i) { CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 1.f, 0.f) == ManualControl::Result::Success); - std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz))); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); } // Fly forward for 60 seconds for (unsigned i = 0; i < 60 * manual_control_rate_hz; ++i) { CHECK(_manual_control->set_manual_control_input(0.5f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success); - std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz))); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); } // Descend until disarmed for (unsigned i = 0; i < 60 * manual_control_rate_hz; ++i) { CHECK(_manual_control->set_manual_control_input(0.f, 0.f, 0.0f, 0.f) == ManualControl::Result::Success); - std::this_thread::sleep_for(adjust_to_lockstep_speed(std::chrono::milliseconds(1000 / manual_control_rate_hz))); + sleep_for(std::chrono::milliseconds(1000 / manual_control_rate_hz)); if (!_telemetry->in_air()) { break; @@ -619,28 +603,3 @@ void AutopilotTester::wait_for_mission_finished(std::chrono::seconds timeout) REQUIRE(fut.wait_for(timeout) == std::future_status::ready); } - -std::chrono::milliseconds AutopilotTester::adjust_to_lockstep_speed(std::chrono::milliseconds duration_ms) -{ - if (_info == nullptr) { - return duration_ms; - } - - auto speed_factor = _info->get_speed_factor(); - - if (speed_factor.first == Info::Result::Success) { - // FIXME: Remove this again: - // Sanitize speed factor to avoid test failures. - if (speed_factor.second > 20.0f) { - speed_factor.second = 20.0f; - } - - return static_cast( - static_cast( - std::round( - static_cast(duration_ms.count()) / speed_factor.second))); - - } else { - return duration_ms; - } -} diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index 55003f33f7..9993e1e4d9 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -131,8 +131,6 @@ private: void wait_for_landed_state(Telemetry::LandedState landed_state, std::chrono::seconds timeout); void wait_for_mission_finished(std::chrono::seconds timeout); - std::chrono::milliseconds adjust_to_lockstep_speed(std::chrono::milliseconds duration_ms); - template bool poll_condition_with_timeout( std::function fun, std::chrono::duration duration) @@ -153,8 +151,6 @@ private: // busy and PX4 doesn't run fast enough. const int64_t elapsed_time_us = _telemetry->attitude_quaternion().timestamp_us - start_time_us; - std::cout << time_str() << "start_time_us: " << start_time_us << ", elapsed_time_us: " << elapsed_time_us << '\n'; - if (elapsed_time_us > duration_us.count()) { std::cout << time_str() << "Timeout, connected to vehicle but waiting for test for " << static_cast (elapsed_time_us) / 1e6 << " seconds\n"; @@ -184,6 +180,31 @@ private: return true; } + template + void sleep_for(std::chrono::duration duration) + { + const std::chrono::microseconds duration_us(duration); + + if (_telemetry && _telemetry->attitude_quaternion().timestamp_us != 0) { + + const int64_t start_time_us = _telemetry->attitude_quaternion().timestamp_us; + + while (true) { + // Hopefully this is often enough not to have PX4 time out on us. + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + + const int64_t elapsed_time_us = _telemetry->attitude_quaternion().timestamp_us - start_time_us; + + if (elapsed_time_us > duration_us.count()) { + return; + } + } + + } else { + std::this_thread::sleep_for(duration); + } + } + mavsdk::Mavsdk _mavsdk{}; std::unique_ptr _action{}; std::unique_ptr _failure{}; diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 4360202070..5f5acfc3e1 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -16,6 +16,16 @@ import process_helper as ph from typing import Any, Dict, List, NoReturn, TextIO, Optional from types import FrameType +try: + import requests +except ImportError as e: + print("Failed to import requests: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user requests") + print("") + sys.exit(1) + def main() -> NoReturn: @@ -37,6 +47,8 @@ def main() -> NoReturn: "(or multiple cases with wildcard '*')") parser.add_argument("--debugger", default="", help="choice from valgrind, callgrind, gdb, lldb") + parser.add_argument("--upload", default=False, action='store_true', + help="Upload logs to logs.px4.io") parser.add_argument("--verbose", default=False, action='store_true', help="enable more verbose output") parser.add_argument("config_file", help="JSON config file to use") @@ -67,7 +79,8 @@ def main() -> NoReturn: args.debugger, args.log_dir, args.gui, - args.verbose + args.verbose, + args.upload ) signal.signal(signal.SIGINT, tester.sigint_handler) @@ -124,7 +137,8 @@ class Tester: debugger: str, log_dir: str, gui: bool, - verbose: bool): + verbose: bool, + upload: bool): self.config = config self.active_runners: List[ph.Runner] self.iterations = iterations @@ -135,6 +149,7 @@ class Tester: self.log_dir = log_dir self.gui = gui self.verbose = verbose + self.upload = upload self.start_time = datetime.datetime.now() self.log_fd: Any[TextIO] = None @@ -364,6 +379,12 @@ class Tester: print(" - {}".format(logfile_path)) for runner in self.active_runners: print(" - {}".format(runner.get_log_filename())) + + if self.upload: + ulog_file = self.parse_for_ulog_file( + self.get_combined_log(logfile_path)) + self.upload_log(ulog_file, test['model'], case, is_success) + return is_success def start_runners(self, @@ -459,6 +480,65 @@ class Tester: if self.verbose: print(line, end="") + def parse_for_ulog_file(self, log: str) -> Optional[str]: + + match = "[logger] Opened full log file: ./" + for line in log.splitlines(): + found = line.find(match) + if found != -1: + return os.getcwd() \ + + "/build/px4_sitl_default/tmp_mavsdk_tests/rootfs/" \ + + line[found+len(match):] + return None + + def upload_log(self, ulog_path: Optional[str], + model: str, case: str, success: bool) -> None: + if not ulog_path: + print(" Could not find ulog log file to upload") + return + + if not os.getenv('GITHUB_RUN_ID'): + print(" Upload only implemented for GitHub Actions CI") + return + + print(" Uploading logfile '{}' ...".format(ulog_path)) + + server = "https://logs.px4.io" + + result_str = "passing" if success else "failing" + + payload = { + "type": "flightreport", + "description": "SITL integration test - {}: '{}' -> {}" + .format(model, case, result_str), + "feedback": + "{}/{}/actions/runs/{}" + .format(os.getenv("GITHUB_SERVER_URL"), + os.getenv("GITHUB_REPOSITORY"), + os.getenv("GITHUB_RUN_ID")), + "email": "", + "source": "CI", + "videoUrl": "", + "rating": "notset", + "windSpeed": -1, + "public": "true" + } + + with open(ulog_path, 'rb') as f: + r = requests.post(server + "/upload", + data=payload, + files={'filearg': f}, + allow_redirects=False) + if r.status_code == 302: # redirect + if 'Location' in r.headers: + plot_url = r.headers['Location'] + if len(plot_url) > 0 and plot_url[0] == '/': + plot_url = server + plot_url + print(" Uploaded to: " + plot_url) + else: + print(" Upload failed with status_code: {}" + .format(r.status_code)) + def start_combined_log(self, filename: str) -> None: self.log_fd = open(filename, 'w') diff --git a/test/mavsdk_tests/test_multicopter_mission.cpp b/test/mavsdk_tests/test_multicopter_mission.cpp index 9e736c7563..3ddd566d9d 100644 --- a/test/mavsdk_tests/test_multicopter_mission.cpp +++ b/test/mavsdk_tests/test_multicopter_mission.cpp @@ -91,7 +91,7 @@ TEST_CASE("Fly straight Multicopter Mission", "[multicopter]") mission_options.rtl_at_end = false; mission_options.fly_through = true; tester.prepare_straight_mission(mission_options); - tester.check_mission_item_speed_above(2, 4.5); + tester.check_mission_item_speed_above(2, 4.0); tester.check_tracks_mission(5.f); tester.arm(); tester.execute_mission(); diff --git a/test_data/ghst_rc_channels.txt b/test_data/ghst_rc_channels.txt new file mode 100644 index 0000000000..88f5309847 --- /dev/null +++ b/test_data/ghst_rc_channels.txt @@ -0,0 +1,3395 @@ +GHST RC test input file: lines starting with 'INPUT' contain bytes read from the +Ghost UART, lines starting with 'DECODED' contain the (expected) decoded RC +channels +------------------------------------------------------------------------------- +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xcd, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0xffff, 0xffff, 0xffff, 0xffff, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0x00, 0x0a, 0x05, 0x00, 0x2c, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0xffff, 0xffff, 0xffff, 0xffff, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xea, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x12, 0x0c, 0x05, 0x00, 0x55, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x12, 0x0a, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x60, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8a, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x12, 0x0b, 0x05, 0x00, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, +INPUT 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, +INPUT 0x12, 0x09, 0x05, 0x00, 0xa5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, +INPUT 0x1b, 0x1b, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x12, 0x0a, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x12, 0x0c, 0x05, 0x00, 0x55, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x12, 0x0a, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, +INPUT 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x12, 0x09, 0x05, 0x00, 0xa5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x12, 0x0a, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x12, 0x09, 0x05, 0x00, 0xa5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, +INPUT 0xd8, 0x47, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x93, +DECODED 0x05e3, 0x05e7, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0xe7, 0x7f, 0x5c, 0x87, 0x74, 0x12, 0x0d, 0x05, 0x00, 0xfb, +DECODED 0x05e3, 0x05ef, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x84, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8e, +DECODED 0x05e3, 0x0604, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0xe7, 0x8b, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x20, +DECODED 0x05e3, 0x062b, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x94, 0x5c, 0x47, 0x74, 0x12, 0x09, 0x05, 0x00, 0x9d, +DECODED 0x05e3, 0x0654, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0xa7, 0xa1, 0x5c, 0x47, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x02, +DECODED 0x05e3, 0x0698, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x87, 0xb1, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xe5, +DECODED 0x05e3, 0x06e7, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x87, 0xcf, 0x5c, 0x87, 0x74, 0x12, 0x0a, 0x05, 0x00, 0x0d, +DECODED 0x05e3, 0x077d, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x67, 0xe2, 0x5e, 0x47, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x3b, +DECODED 0x05e3, 0x07dc, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x67, 0xe2, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x73, +DECODED 0x05e3, 0x07dc, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x67, +INPUT 0xe2, 0x5c, 0x87, 0x74, 0x12, 0x0a, 0x05, 0x00, 0x4b, +DECODED 0x05e3, 0x07dc, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x67, 0xe2, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xa9, +DECODED 0x05e3, 0x07dc, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x67, 0xe2, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x8e, +DECODED 0x05e3, 0x07dc, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x67, 0xe2, 0x5a, 0x87, 0x74, 0x12, 0x0a, 0x05, 0x00, 0x99, +DECODED 0x05e3, 0x07dc, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x67, 0xe2, 0x5a, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x7b, +DECODED 0x05e3, 0x07dc, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x67, 0xe2, 0x58, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xa1, +DECODED 0x05e3, 0x07dc, 0x05bb, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x67, 0xe2, 0x56, 0x87, 0x74, 0x12, 0x0a, 0x05, 0x00, 0xe8, +DECODED 0x05e3, 0x07dc, 0x05bb, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x67, 0xe2, 0x56, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x0a, +DECODED 0x05e3, 0x07dc, 0x05bb, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x67, 0xe0, 0x52, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x84, +DECODED 0x05e3, 0x07d2, 0x05b9, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x67, 0xd2, 0x18, 0x87, 0x74, 0x12, 0x0a, 0x05, 0x00, 0x6c, +DECODED 0x05e3, 0x078c, 0x05a7, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x67, 0xc2, 0xe4, 0x86, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xfc, +DECODED 0x05e3, 0x073c, 0x0597, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0xad, 0x86, 0x86, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x07, +DECODED 0x05e3, 0x06d1, 0x057a, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x87, 0x9f, 0x80, 0x86, 0x74, 0x12, 0x0a, 0x05, 0x00, 0x40, +DECODED 0x05e3, 0x068d, 0x0578, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x96, 0x68, 0x46, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x6e, +DECODED 0x05e3, 0x065e, 0x0570, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x90, 0xa0, 0x86, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x63, +DECODED 0x05e3, 0x0640, 0x0582, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0xe7, 0x89, 0xe0, 0x46, 0x74, 0x64, 0x0a, 0x05, 0x00, 0x94, +DECODED 0x05e3, 0x0621, 0x0596, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x85, 0x1c, 0x47, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x2d, +DECODED 0x05e3, 0x0609, 0x05a9, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0xa7, 0x7e, 0x52, 0x47, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x7f, +DECODED 0x05e3, 0x05e9, 0x05b9, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x87, 0x7e, 0x56, 0x47, 0x74, 0x64, 0x0c, 0x05, 0x00, 0x33, +DECODED 0x05e2, 0x05e8, 0x05bb, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd0, 0x67, 0x7e, 0x58, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x3c, +DECODED 0x05e1, 0x05e8, 0x05bb, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xb6, 0x67, 0x7e, 0x58, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x9c, +DECODED 0x05d9, 0x05e8, 0x05bb, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xb4, 0x47, 0x7e, 0x58, 0x87, 0x74, 0x64, 0x0a, 0x05, 0x00, 0x02, +DECODED 0x05d8, 0x05e7, 0x05bb, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x98, 0x47, 0x7e, 0x5a, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x96, +DECODED 0x05cf, 0x05e7, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x94, 0x47, 0x7e, 0x5a, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x7d, +DECODED 0x05ce, 0x05e7, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x78, 0x47, 0x7e, 0x5a, 0x87, 0x74, 0x64, 0x0a, 0x05, 0x00, 0x24, +DECODED 0x05c5, 0x05e7, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x56, 0x47, 0x7e, 0x5a, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xdc, +DECODED 0x05bb, 0x05e7, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x36, 0x27, 0x7e, 0x5a, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xb9, +DECODED 0x05b1, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xe6, 0x26, 0x7e, 0x5a, 0x87, 0x74, 0x64, 0x0a, 0x05, 0x00, 0x81, +DECODED 0x0598, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x6a, 0x26, 0x7e, 0x5a, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x1e, +DECODED 0x0571, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xc4, 0x25, 0x7e, 0x5a, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xcb, +DECODED 0x053d, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x50, 0x25, 0x7e, 0x5a, 0x87, 0x74, 0x64, 0x0a, 0x05, 0x00, 0x3e, +DECODED 0x0519, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xbe, 0x24, 0x7e, 0x5a, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xe6, +DECODED 0x04eb, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x2e, 0x24, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x7f, +DECODED 0x04be, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x7a, 0x23, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0c, 0x05, 0x00, 0xb8, +DECODED 0x0486, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xfc, 0x22, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x1a, +DECODED 0x045f, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x7a, 0x22, 0x7e, 0x5a, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x93, +DECODED 0x0436, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xf8, 0x21, 0x7e, 0x5c, 0x47, 0x74, 0x64, 0x0a, 0x05, 0x00, 0x92, +DECODED 0x040d, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xa0, 0x21, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xf4, +DECODED 0x03f2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xa0, 0x21, 0x7e, 0x5a, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x01, +DECODED 0x03f2, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xa0, 0x21, 0x7e, 0x5c, 0x47, 0x74, 0x64, 0x0a, 0x05, 0x00, 0xd2, +DECODED 0x03f2, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xa0, 0x21, 0x7e, 0x5a, 0x47, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x49, +DECODED 0x03f2, 0x05e6, 0x05bc, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xa0, 0x21, 0x7e, 0x5c, 0x47, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xbc, +DECODED 0x03f2, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xa0, 0x21, 0x7e, 0x5a, 0x47, 0x74, 0x64, 0x0a, 0x05, 0x00, 0x00, +DECODED 0x03f2, 0x05e6, 0x05bc, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xa0, 0x21, 0x7e, 0x5a, 0x47, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x49, +DECODED 0x03f2, 0x05e6, 0x05bc, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x9e, 0x21, 0x7e, 0x5c, 0x27, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xa6, +DECODED 0x03f1, 0x05e6, 0x05bd, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xb6, 0x21, 0x7e, 0x5a, 0x27, 0x74, 0x64, 0x0a, 0x05, 0x00, 0xcd, +DECODED 0x03f9, 0x05e6, 0x05bc, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x22, 0x22, 0x7e, 0x5a, 0x27, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x46, +DECODED 0x041b, 0x05e6, 0x05bc, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xcc, 0x22, 0x7e, 0x5a, 0x07, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x3f, +DECODED 0x0450, 0x05e6, 0x05bc, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x50, 0x23, 0x7e, 0x40, 0x07, 0x74, 0x64, 0x0c, 0x05, 0x00, 0x79, +DECODED 0x0479, 0x05e6, 0x05b4, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xe0, 0x23, 0x7e, 0x9c, 0x06, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x01, +DECODED 0x04a6, 0x05e6, 0x0581, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x50, 0x24, 0x7e, 0x00, 0x06, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x25, +DECODED 0x04c9, 0x05e6, 0x0550, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xe0, 0x04, 0x7e, 0x2a, 0x05, 0x74, 0x64, 0x0a, 0x05, 0x00, 0xd9, +DECODED 0x04f6, 0x05e6, 0x050d, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x5c, 0x05, 0x7e, 0x6e, 0x04, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xd3, +DECODED 0x051d, 0x05e6, 0x04d2, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xf2, 0x05, 0x7e, 0x40, 0x23, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xe4, +DECODED 0x054b, 0x05e6, 0x0474, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x52, 0x06, 0x7e, 0x34, 0x22, 0x74, 0x64, 0x0a, 0x05, 0x00, 0x0c, +DECODED 0x0569, 0x05e6, 0x0420, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xc2, 0x06, 0x7e, 0x5a, 0x21, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xfa, +DECODED 0x058c, 0x05e6, 0x03dc, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x04, 0xe7, 0x7d, 0x5a, 0x21, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xac, +DECODED 0x05a1, 0x05e5, 0x03dc, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x6e, 0x87, 0x7f, 0x5a, 0x21, 0x74, 0x64, 0x0a, 0x05, 0x00, 0xac, +DECODED 0x05c2, 0x05ed, 0x03dc, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xa2, 0x87, 0x7f, 0x5a, 0x21, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x3e, +DECODED 0x05d2, 0x05ed, 0x03dc, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xba, 0x07, 0x80, 0x5a, 0x21, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x9a, +DECODED 0x05da, 0x05f0, 0x03dc, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xc0, 0x27, 0x80, 0x5a, 0x21, 0x74, 0x64, 0x0c, 0x05, 0x00, 0x08, +DECODED 0x05dc, 0x05f0, 0x03dc, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xc0, 0x47, 0x7e, 0x5a, 0x21, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x9e, +DECODED 0x05dc, 0x05e7, 0x03dc, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x11, 0xc4, 0x27, 0x7e, 0x5a, 0x21, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xed, +DECODED 0x05dd, 0x05e6, 0x03dc, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xc6, 0x07, 0x7e, 0xae, 0x21, 0x74, 0x64, 0x0a, 0x05, 0x00, 0x09, +DECODED 0x05de, 0x05e6, 0x03f6, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xc8, 0x07, 0x7e, 0xf2, 0x22, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x22, +DECODED 0x05de, 0x05e6, 0x045c, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xca, 0xe7, 0x7d, 0x92, 0x44, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xa6, +DECODED 0x05df, 0x05e5, 0x04de, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xca, 0xe7, 0x7d, 0x90, 0x25, 0x74, 0x64, 0x0a, 0x05, 0x00, 0x70, +DECODED 0x05df, 0x05e5, 0x052d, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xce, 0xe7, 0x7d, 0x78, 0x26, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x04, +DECODED 0x05e0, 0x05e5, 0x0575, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xce, 0xe7, 0x7d, 0x2e, 0x47, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x87, +DECODED 0x05e0, 0x05e5, 0x05ae, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xce, 0xe7, 0x7d, 0x60, 0x47, 0x74, 0x64, 0x0a, 0x05, 0x00, 0xea, +DECODED 0x05e0, 0x05e5, 0x05be, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd0, 0xe7, 0x7d, 0x5e, 0x47, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x15, +DECODED 0x05e1, 0x05e5, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd0, 0xe7, 0x7d, 0x5e, 0x47, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x32, +DECODED 0x05e1, 0x05e5, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd0, 0xe7, +INPUT 0x7d, 0x5c, 0x47, 0x74, 0x64, 0x0a, 0x05, 0x00, 0xa1, +DECODED 0x05e1, 0x05e5, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd0, 0xe7, 0x7d, 0x5c, 0x47, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xe8, +DECODED 0x05e1, 0x05e5, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd0, 0xe7, 0x7d, 0x5c, 0x47, 0x74, +INPUT 0x7d, 0x7c, 0x7c, 0x7c, 0xcf, +DECODED 0x05e1, 0x05e5, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd0, 0xe7, 0x7d, 0x5c, 0x47, 0x74, 0x64, 0x0c, 0x05, 0x00, 0x01, +DECODED 0x05e1, 0x05e5, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd0, 0xe7, 0x7d, 0x5c, 0x47, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xe8, +DECODED 0x05e1, 0x05e5, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd0, 0xe7, 0x7d, 0x5c, 0x47, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xcf, +DECODED 0x05e1, 0x05e5, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd0, 0xe7, 0x7d, 0x5c, 0x47, 0x74, 0x64, 0x0a, 0x05, 0x00, 0xa1, +DECODED 0x05e1, 0x05e5, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xec, 0xe7, 0x7d, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xd1, +DECODED 0x05ea, 0x05e5, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x24, 0xe8, 0x7d, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x32, +DECODED 0x05fb, 0x05e5, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x98, 0xe8, 0x7d, 0x5c, 0x87, 0x74, 0x64, 0x0b, 0x05, 0x00, 0x93, +DECODED 0x061f, 0x05e5, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd0, 0xe8, 0x7d, 0x5c, 0x87, 0x74, +INPUT 0x7c, 0x7c, 0x1b, 0x1b, 0x6f, +DECODED 0x0631, 0x05e5, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x42, 0xe9, 0x7d, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x82, +DECODED 0x0654, 0x05e5, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x90, 0xe9, 0x7d, 0x5c, 0x87, 0x74, 0x64, 0x0b, 0x05, 0x00, 0x9f, +DECODED 0x066d, 0x05e5, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xf0, 0xe9, 0x7d, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x49, +DECODED 0x068b, 0x05e5, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x6c, 0xea, 0x7d, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x97, +DECODED 0x06b1, 0x05e5, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xe8, 0xea, 0x7d, 0x5e, 0x87, 0x74, 0x64, 0x0a, 0x05, 0x00, 0x14, +DECODED 0x06d8, 0x05e5, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x3a, 0xeb, 0x7d, 0x60, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x07, +DECODED 0x06f2, 0x05e5, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xa8, 0xeb, 0x7d, 0x60, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xdd, +DECODED 0x0714, 0x05e5, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xec, 0xeb, 0x7d, 0x60, 0x87, 0x74, 0x64, 0x0c, 0x05, 0x00, 0xe9, +DECODED 0x0729, 0x05e5, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x4e, 0xec, 0x7d, 0x60, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xe2, +DECODED 0x0748, 0x05e5, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x98, 0xec, 0x7d, 0x60, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xc2, +DECODED 0x075f, 0x05e5, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xf4, 0xec, 0x7d, 0x60, 0x87, 0x74, 0x64, 0x0b, 0x05, 0x00, 0x02, +DECODED 0x077c, 0x05e5, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x10, 0x26, 0xed, 0x7d, 0x60, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x0f, +DECODED 0x078c, 0x05e5, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x68, 0xed, 0x7d, 0x62, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x85, +DECODED 0x07a0, 0x05e5, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xc8, 0xed, 0x7d, 0x62, 0x87, 0x74, 0x64, 0x0a, 0x05, 0x00, 0x1d, +DECODED 0x07be, 0x05e5, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x26, 0xee, 0x7d, 0x62, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xab, +DECODED 0x07dc, 0x05e5, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x26, 0xee, 0x7d, 0x62, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x8c, +DECODED 0x07dc, 0x05e5, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x26, 0xee, 0x7d, 0x62, 0x87, 0x74, 0x64, 0x0b, 0x05, 0x00, 0x61, +DECODED 0x07dc, 0x05e5, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x26, 0x0e, 0x7e, 0x62, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x9c, +DECODED 0x07dc, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x26, 0x0e, 0x7e, 0x62, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xbb, +DECODED 0x07dc, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x26, 0xee, 0x7d, 0x62, 0xc7, 0x74, 0x64, 0x0b, 0x05, 0x00, 0x44, +DECODED 0x07dc, 0x05e5, 0x05be, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x26, 0xee, 0x7d, 0x62, 0xc7, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8e, +DECODED 0x07dc, 0x05e5, 0x05be, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x26, 0x0e, +INPUT 0x7e, 0x62, 0xc7, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x9e, +DECODED 0x07dc, 0x05e6, 0x05be, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x26, 0x0e, 0x7e, 0x62, 0xc7, 0x74, 0x64, 0x0d, 0x05, 0x00, 0xd3, +DECODED 0x07dc, 0x05e6, 0x05be, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x26, 0x0e, 0x7e, 0x62, 0xc7, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xb9, +DECODED 0x07dc, 0x05e6, 0x05be, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x26, 0xee, 0x7d, 0x62, 0xc7, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xa9, +DECODED 0x07dc, 0x05e5, 0x05be, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x26, 0xee, 0x7d, 0x62, 0xc7, 0x74, 0x64, 0x0a, 0x05, 0x00, 0xc7, +DECODED 0x07dc, 0x05e5, 0x05be, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x26, 0xee, 0x7d, 0x62, 0xe7, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x07dc, 0x05e5, 0x05be, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xfe, 0x0d, 0x7e, 0x66, 0xe7, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x4a, +DECODED 0x07cf, 0x05e6, 0x05c0, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xa0, 0x0d, 0x7e, 0x68, 0xe7, 0x74, 0x64, 0x0b, 0x05, 0x00, 0x0d, +DECODED 0x07b2, 0x05e6, 0x05c0, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x28, 0x0d, 0x7e, 0x66, 0xe7, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x6a, +DECODED 0x078c, 0x05e6, 0x05c0, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xb0, 0x0c, 0x7e, 0x66, 0xe7, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x2d, +DECODED 0x0767, 0x05e6, 0x05c0, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x50, 0x2c, 0x7e, 0x62, 0xe7, 0x74, 0x64, 0x0a, 0x05, 0x00, 0xf6, +DECODED 0x0749, 0x05e6, 0x05be, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xe8, 0x4b, 0x7e, 0x60, 0xe7, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xdf, +DECODED 0x0728, 0x05e7, 0x05be, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x68, 0x8b, 0x7e, 0x60, 0xe7, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x71, +DECODED 0x0700, 0x05e8, 0x05be, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0xaa, 0x7e, 0x5e, 0xe7, 0x74, 0x64, 0x0b, 0x05, 0x00, 0x1b, +DECODED 0x06d2, 0x05e9, 0x05bd, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x62, 0xaa, 0x7e, 0x5e, 0xe7, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x37, +DECODED 0x06ae, 0x05e9, 0x05bd, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xda, 0xa9, 0x7e, 0x5e, 0xe7, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xf2, +DECODED 0x0684, 0x05e9, 0x05bd, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x82, 0x89, 0x7e, 0x5e, 0xe7, 0x74, 0x64, 0x0a, 0x05, 0x00, 0xbd, +DECODED 0x0668, 0x05e8, 0x05bd, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x26, 0x89, 0x7e, 0x5e, 0xe7, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xf5, +DECODED 0x064c, 0x05e8, 0x05bd, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xbe, 0x68, 0x7e, 0x5e, 0xe7, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x40, +DECODED 0x062b, 0x05e8, 0x05bd, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x70, 0x68, 0x7e, 0x62, 0xe7, 0x74, 0x64, 0x0a, 0x05, 0x00, 0x04, +DECODED 0x0613, 0x05e8, 0x05be, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x46, 0x68, 0x7e, 0x80, 0xc7, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, +INPUT 0x5d, +DECODED 0x0606, 0x05e8, 0x05c8, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xfe, 0x47, 0x7e, 0xa6, 0xc7, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x37, +DECODED 0x05ef, 0x05e7, 0x05d4, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xde, 0x47, 0x7e, 0xc0, 0xc7, 0x74, 0x64, 0x0a, 0x05, 0x00, 0x45, +DECODED 0x05e5, 0x05e7, 0x05dc, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xde, 0x47, 0x7e, 0xde, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x19, +DECODED 0x05e5, 0x05e7, 0x05e5, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xde, 0x47, 0x7e, 0xf8, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x41, +DECODED 0x05e5, 0x05e7, 0x05ed, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xde, 0x47, 0x7e, 0x2e, 0x88, 0x74, 0x64, 0x0b, 0x05, 0x00, 0xda, +DECODED 0x05e5, 0x05e7, 0x05fe, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xdc, 0x47, 0x7e, 0x30, 0x88, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xb1, +DECODED 0x05e5, 0x05e7, 0x05ff, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x47, 0x7e, 0x68, 0x88, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x0c, +DECODED 0x05e3, 0x05e7, 0x0610, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x27, 0x80, 0x8e, 0x88, 0x74, 0x64, 0x0c, 0x05, 0x00, 0x4a, +DECODED 0x05e2, 0x05f0, 0x061c, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd2, 0x47, 0x84, 0xb0, 0x88, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x22, +DECODED 0x05e1, 0x0605, 0x0627, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd0, 0xe7, 0x8b, 0xd4, 0x88, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xd2, +DECODED 0x05e1, 0x062b, 0x0632, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xce, 0x27, 0x9a, 0x0c, 0x89, 0x74, 0x64, 0x0a, 0x05, 0x00, 0xb7, +DECODED 0x05e0, 0x0672, 0x0644, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xca, 0xa7, 0xa8, 0x4c, 0x89, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xd6, +DECODED 0x05df, 0x06bb, 0x0658, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xca, 0xa7, 0xbd, 0xa4, 0x49, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x7d, +DECODED 0x05df, 0x0724, 0x0673, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xca, 0x87, 0xd0, 0xf2, 0x49, 0x74, 0x64, 0x0b, 0x05, 0x00, 0x36, +DECODED 0x05df, 0x0782, 0x068b, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xca, 0x67, 0xe2, 0x2a, 0x4a, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xd4, +DECODED 0x05df, 0x07dc, 0x069d, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xce, 0x67, 0xe2, 0x30, 0x4a, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x1b, +DECODED 0x05e0, 0x07dc, 0x069f, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd0, 0x67, 0xe2, 0x34, 0x4a, 0x74, 0x64, 0x0a, 0x05, 0x00, 0x71, +DECODED 0x05e1, 0x07dc, 0x06a0, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd0, 0x67, 0xe2, 0x34, 0x4a, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x38, +DECODED 0x05e1, 0x07dc, 0x06a0, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd0, 0x67, 0xe2, 0x32, 0x4a, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xcd, +DECODED 0x05e1, 0x07dc, 0x069f, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd2, 0x67, 0xe2, 0x14, 0x4a, 0x74, 0x64, 0x0d, 0x05, 0x00, 0x6e, +DECODED 0x05e1, 0x07dc, 0x0696, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd2, 0x67, 0xe2, 0xc8, 0x49, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xf0, +DECODED 0x05e1, 0x07dc, 0x067e, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd2, 0x67, 0xe2, 0x82, 0x49, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xfb, +DECODED 0x05e1, 0x07dc, 0x0668, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd2, 0xe7, 0xda, 0xe6, 0x48, 0x74, 0x64, 0x0d, 0x05, 0x00, 0x92, +DECODED 0x05e1, 0x07b6, 0x0638, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd2, 0x87, 0xca, 0x52, 0x88, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x0f, +DECODED 0x05e1, 0x0764, 0x0609, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0xe7, 0xaf, 0x7c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xd1, +DECODED 0x05e2, 0x06df, 0x05c7, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x07, 0x9e, 0x5e, 0x87, 0x74, 0x64, 0x0d, 0x05, 0x00, 0xfa, +DECODED 0x05e2, 0x0686, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0xa7, 0x96, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xc7, +DECODED 0x05e2, 0x0661, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0xa7, 0x94, 0x5c, 0x47, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xf4, +DECODED 0x05e2, 0x0657, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x87, 0x91, 0x5c, 0x47, 0x74, 0x64, 0x0c, 0x05, 0x00, 0xc1, +DECODED 0x05e2, 0x0647, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x47, 0x8e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x61, +DECODED 0x05e2, 0x0637, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, 0x7a, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xb9, +DECODED 0x05e2, 0x05d2, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x07, 0x7b, 0x5c, 0x87, 0x74, 0x64, 0x0f, 0x05, 0x00, 0x64, +DECODED 0x05e2, 0x05d7, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x47, 0x7d, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x40, +DECODED 0x05e2, 0x05e2, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x81, 0x5c, 0x47, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xf1, +DECODED 0x05e2, 0x05f5, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x27, 0x80, 0x5c, 0x87, 0x74, 0x64, 0x0e, 0x05, 0x00, 0xc0, +DECODED 0x05e2, 0x05f0, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xea, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x38, 0x28, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x23, +DECODED 0x0601, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x88, 0x08, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x11, 0x05, 0x00, 0x03, +DECODED 0x061a, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xe0, 0x08, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x3f, +DECODED 0x0636, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xe2, 0x08, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x89, +DECODED 0x0636, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xe2, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x10, 0x05, 0x00, 0x23, +DECODED 0x05e6, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xb6, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xca, +DECODED 0x05d9, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xb6, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xed, +DECODED 0x05d9, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xf6, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x16, 0x05, 0x00, 0x02, +DECODED 0x05ed, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xc0, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x3b, +DECODED 0x05dc, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xc6, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x7a, +DECODED 0x05de, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xdc, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x10, 0x05, 0x00, 0xe4, +DECODED 0x05e5, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xdc, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x7c, +DECODED 0x05e5, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x16, 0x05, 0x00, 0xb3, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x0e, 0x05, 0x00, 0x4c, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x10, 0x05, 0x00, 0x13, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x0d, 0x05, 0x00, 0x1c, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x0e, 0x05, 0x00, 0x4c, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0e, 0x05, 0x00, 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0f, 0x05, 0x00, 0x32, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x0f, 0x05, 0x00, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x0f, 0x05, 0x00, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xe2, 0x7c, 0x1b, 0x1b, 0x9f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x0f, 0x05, 0x00, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xe2, 0x7c, 0x1b, 0x1b, 0x9f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xc3, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x10, 0x05, 0x00, 0xee, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0x7c, 0x1b, 0x1b, 0x62, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0f, 0x05, 0x00, 0x32, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xe2, 0x7c, 0x1b, 0x1b, 0x9f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x11, 0x05, 0x00, 0x90, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xe2, 0x7c, 0x1b, 0x1b, 0x9f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x0f, 0x05, 0x00, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xe2, 0x7c, 0x1b, 0x1b, 0x9f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x13, 0x05, 0x00, 0xbe, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0x7c, 0x1b, 0x1b, 0x62, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x10, 0x05, 0x00, 0xee, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0x7c, 0x1b, 0x1b, 0x62, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x13, 0x05, 0x00, 0xbe, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0x7c, 0x1b, 0x1b, 0x62, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xcd, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0e, 0x05, 0x00, 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0x7c, 0x1b, 0x1b, 0x62, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x14, 0x05, 0x00, 0x9d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0x7c, 0x1b, 0x1b, 0x62, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0e, 0x05, 0x00, 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0x7c, 0x1b, 0x1b, 0x62, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0x08, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0x7c, 0x1b, 0x1b, 0x03, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x10, 0x05, 0x00, 0x8f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xe2, 0x7c, 0x1b, 0x1b, 0x9f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x2a, 0x05, 0x00, 0x0d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x16, 0x05, 0x00, 0xb3, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x1b, 0x05, 0x00, 0x58, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xcd, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x13, 0x05, 0x00, 0xdf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x10, 0x05, 0x00, 0xee, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0f, 0x05, 0x00, 0x53, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x12, 0x05, 0x00, 0xc0, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x16, 0x05, 0x00, 0xb3, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1a, 0x05, 0x00, 0x26, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x14, 0x05, 0x00, 0x60, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1d, 0x05, 0x00, 0x64, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1d, 0x05, 0x00, 0x64, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1a, 0x05, 0x00, 0x26, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1f, 0x05, 0x00, 0xd6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1b, 0x05, 0x00, 0xa5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0x3b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x22, 0x05, 0x00, 0xeb, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0x3b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x47, 0x74, 0x64, 0x1d, 0x05, 0x00, 0x6a, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0xc6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1f, 0x05, 0x00, 0xd6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0x3b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0x3b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0xcd, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0x3b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1c, 0x05, 0x00, 0xe7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0xa7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0xc6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x16, 0x05, 0x00, 0xb3, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0xc6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x2d, 0x05, 0x00, 0xd3, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0x3b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0x7c, 0x7c, 0x1b, 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x23, 0x05, 0x00, 0x95, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, +INPUT 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x14, +INPUT 0x05, 0x00, 0x9d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, +INPUT 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x16, 0x05, 0x00, 0xb3, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, +INPUT 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x13, 0x05, 0x00, 0x43, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x14, 0x05, 0x00, 0x9d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, +INPUT 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x15, 0x05, +INPUT 0x00, 0x1e, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, +INPUT 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1d, 0x05, 0x00, 0x05, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, +INPUT 0x05, 0x00, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, +INPUT 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x25, 0x05, 0x00, 0xc8, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, +INPUT 0x1b, 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x18, +INPUT 0x05, 0x00, 0x08, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, +INPUT 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x22, 0x05, 0x00, 0xeb, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, +INPUT 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, +INPUT 0x1a, 0x05, 0x00, 0xdb, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, +INPUT 0x7c, 0xe2, 0x7c, 0xd5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x2f, 0x05, 0x00, 0x00, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0xe2, 0x7c, 0xd5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, +INPUT 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, +INPUT 0x7d, 0x7c, 0xe2, 0x7c, 0x28, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1c, 0x05, 0x00, 0x86, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, +INPUT 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0xe2, 0x7c, 0x28, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, +INPUT 0x64, 0x1c, 0x05, 0x00, 0x86, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, +INPUT 0xe2, 0x7c, 0x28, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x1e, 0x05, 0x00, 0xa8, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, +INPUT 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0xe2, 0x7c, 0xd5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, +INPUT 0x74, 0x64, 0x19, 0x05, 0x00, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0xe2, 0x7c, 0x28, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1f, 0x05, 0x00, 0xd6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, +INPUT 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0xe2, 0x7c, 0xd5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, +INPUT 0x74, 0x64, 0x1a, 0x05, 0x00, 0xdb, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, +INPUT 0x74, 0x7d, 0x7c, 0xe2, 0x7c, 0xd5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x24, 0x05, 0x00, 0x4b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0xe2, 0x7c, 0x28, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1e, 0x05, 0x00, 0x55, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, +INPUT 0x87, 0x74, 0x7d, 0x7c, 0xe2, 0x7c, 0xd5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x23, 0x05, 0x00, 0x68, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, +INPUT 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, +INPUT 0x87, 0x74, 0x64, 0x1f, 0x05, 0x00, 0xd6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x31, 0x05, 0x00, 0xa2, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, +INPUT 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, +INPUT 0x5e, 0x87, 0x74, 0x64, 0x1d, 0x05, 0x00, 0x05, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, +INPUT 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1d, 0x05, 0x00, 0x05, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, +INPUT 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, +INPUT 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xf8, 0x06, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x8a, +DECODED 0x059d, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x76, 0x06, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1c, 0x05, 0x00, 0x1a, +DECODED 0x0575, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x9e, 0x05, 0x7e, 0x5e, +INPUT 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x73, +DECODED 0x0531, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x04, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x08, +DECODED 0x04f3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x02, 0x04, 0x7e, 0x5e, 0xc7, 0x74, 0x64, 0x17, +INPUT 0x05, 0x00, 0x87, +DECODED 0x04b1, 0x05e6, 0x05bd, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x7c, 0x03, 0x7e, 0x5e, 0xc7, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8e, +DECODED 0x0487, 0x05e6, 0x05bd, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x66, +INPUT 0x02, 0x7e, 0x5c, 0xc7, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xbf, +DECODED 0x0430, 0x05e6, 0x05bd, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xa0, 0x01, 0x7e, 0x5c, 0xc7, 0x74, 0x64, 0x1e, 0x05, 0x00, 0x93, +DECODED 0x03f2, 0x05e6, 0x05bd, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x9e, +INPUT 0x21, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x33, +DECODED 0x03f1, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x9e, 0x21, 0x80, 0x5c, 0xa7, 0x6f, 0x7d, 0x7c, 0x7c, 0x7c, 0x47, +DECODED 0x03f1, 0x05f0, 0x05bd, 0x059e, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x9e, 0x01, 0x84, 0x5c, 0x07, 0x6b, 0x64, 0x1e, 0x05, 0x00, 0xee, +DECODED 0x03f1, 0x0604, 0x05bd, 0x0587, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x9e, 0x21, 0x84, 0x5c, 0x87, 0x66, 0x7c, 0x7c, 0x1b, 0x1b, 0x6c, +DECODED 0x03f1, 0x0604, 0x05bd, 0x0570, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x9a, 0xe1, 0x85, 0x5c, 0x27, 0x60, 0x7d, 0x7c, +INPUT 0x7c, 0x7c, 0xc0, +DECODED 0x03f0, 0x060d, 0x05bd, 0x0550, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x9a, 0x01, 0x86, 0x5e, 0x67, 0x5c, 0x64, 0x21, 0x05, 0x00, 0x09, +DECODED 0x03f0, 0x060e, 0x05bd, 0x053e, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x9a, +INPUT 0x81, 0x8a, 0x5e, 0x47, 0x5c, 0x7c, 0x7c, 0x1b, 0x1b, 0xd3, +DECODED 0x03f0, 0x0624, 0x05bd, 0x053d, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x9a, 0x01, 0x93, 0x5e, 0x07, 0x5c, 0x7d, 0x7c, 0x7c, 0x7c, 0x6a, +DECODED 0x03f0, 0x064f, 0x05bd, 0x053c, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, +INPUT 0x9a, 0x41, 0x9f, 0x5e, 0x47, 0x5c, 0x64, 0x1a, 0x05, 0x00, 0x44, +DECODED 0x03f0, 0x068c, 0x05bd, 0x053d, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xc0, 0x01, 0xac, 0x60, 0x07, 0x5e, 0x7c, 0x7c, 0x1b, 0x1b, 0x44, +DECODED 0x03fc, 0x06cc, 0x05be, 0x0546, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x62, 0x42, 0xc1, 0x60, +INPUT 0x27, 0x5e, 0x7d, 0x7c, 0x7c, 0x7c, 0x41, +DECODED 0x042f, 0x0736, 0x05be, 0x0546, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x34, 0x63, 0xd3, 0x5e, 0x67, 0x5e, 0x64, 0x18, 0x05, 0x00, 0x5e, +DECODED 0x0470, 0x0791, 0x05bd, 0x0548, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x24, 0x64, 0xe2, 0x38, 0x67, 0x67, 0x7c, 0x7c, 0x1b, 0x1b, 0xc7, +DECODED 0x04bb, 0x07dc, 0x05b1, 0x0575, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xb8, 0x64, 0xe2, 0xe0, 0x06, 0x6d, 0x7d, 0x7c, 0x7c, 0x7c, 0x81, +DECODED 0x04e9, 0x07dc, 0x0596, 0x0591, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, +INPUT 0x6a, 0x65, 0xe2, 0x92, 0x46, 0x74, 0x64, 0x1a, 0x05, 0x00, 0xb9, +DECODED 0x0521, 0x07dc, 0x057d, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x80, 0x66, 0xe2, 0x76, 0x46, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x27, +DECODED 0x0578, 0x07dc, 0x0575, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x92, 0x68, 0xe2, 0x72, 0x46, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x17, +DECODED 0x061d, 0x07dc, 0x0573, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xc8, 0x67, 0xe2, 0x72, 0x46, 0x74, 0x64, 0x1c, 0x05, 0x00, 0xf9, +DECODED 0x05de, 0x07dc, 0x0573, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xea, 0x67, 0xe2, 0xd6, +INPUT 0x46, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x3d, +DECODED 0x05e9, 0x07dc, 0x0593, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xe6, 0x67, 0xe2, 0x1c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xc6, +DECODED 0x05e8, 0x07dc, 0x05a9, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xe2, 0x67, 0xe2, 0x50, 0x87, 0x74, 0x64, 0x17, +INPUT 0x05, 0x00, 0xae, +DECODED 0x05e6, 0x07dc, 0x05b9, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x18, 0x08, 0xdf, 0x52, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x03, +DECODED 0x05f7, 0x07cb, 0x05b9, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x54, 0x88, 0xd1, 0x56, 0xc7, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x86, +DECODED 0x060a, 0x0787, 0x05bb, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x70, 0x28, 0xc3, 0x56, 0xc7, 0x74, 0x64, 0x19, 0x05, 0x00, 0xc2, +DECODED 0x0613, 0x073f, 0x05bb, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x8c, 0x08, 0xb4, 0x58, 0xe7, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x9f, +DECODED 0x061c, 0x06f4, 0x05bb, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x90, 0x88, 0xaa, 0x58, 0xe7, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xa5, +DECODED 0x061d, 0x06c4, 0x05bb, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x92, 0x08, 0xa3, 0x58, +INPUT 0xe7, 0x74, 0x64, 0x17, 0x05, 0x00, 0x20, +DECODED 0x061d, 0x069f, 0x05bb, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x92, 0x88, 0x9d, 0x58, 0xe7, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x88, +DECODED 0x061d, 0x0683, 0x05bb, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x24, 0x28, 0x96, 0x5a, 0xe7, 0x74, 0x7d, 0x7c, +INPUT 0x7c, 0x7c, 0x26, +DECODED 0x05fb, 0x065e, 0x05bc, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0xa7, 0x8e, 0x5a, 0xc7, 0x74, 0x64, 0x19, 0x05, 0x00, 0xa8, +DECODED 0x05e3, 0x0639, 0x05bc, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, +INPUT 0x10, 0x92, 0x47, 0x8a, 0x5a, 0xc7, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x24, +DECODED 0x05cd, 0x0623, 0x05bc, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x3a, 0x87, 0x86, 0x5a, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xc6, +DECODED 0x05b2, 0x0610, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x8c, 0x06, 0x83, 0x5a, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0x78, +DECODED 0x057c, 0x05ff, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xf4, 0x25, 0x81, 0x5a, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xe9, +DECODED 0x054c, 0x05f5, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x36, 0x45, 0x7f, 0x5a, +INPUT 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x37, +DECODED 0x0511, 0x05ec, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xb8, 0x24, 0x7f, 0x5c, 0x47, 0x76, 0x64, 0x18, 0x05, 0x00, 0x44, +DECODED 0x04e9, 0x05eb, 0x05bd, 0x05bf, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x2a, 0xe4, 0x7e, 0x5c, 0xa7, 0x76, 0x7c, 0x7c, +INPUT 0x1b, 0x1b, 0x7d, +DECODED 0x04bd, 0x05ea, 0x05bd, 0x05c1, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xaa, 0xa3, 0x7e, 0x5c, 0xe7, 0x76, 0x7d, 0x7c, 0x7c, 0x7c, 0x22, +DECODED 0x0495, 0x05e9, 0x05bd, 0x05c2, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x13, 0xd6, 0x82, 0x7e, 0x5c, 0x07, 0x77, 0x64, 0x17, 0x05, 0x00, 0x6f, +DECODED 0x0453, 0x05e8, 0x05bd, 0x05c3, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x2e, 0x82, 0x7e, 0x5c, 0x47, 0x77, 0x7c, 0x7c, 0x1b, 0x1b, 0xba, +DECODED 0x041e, 0x05e8, 0x05bd, 0x05c4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, +INPUT 0xd0, 0x61, 0x7e, 0x5c, 0x27, 0x77, 0x7d, 0x7c, 0x7c, 0x7c, 0x3b, +DECODED 0x0401, 0x05e8, 0x05bd, 0x05c3, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xaa, 0x61, +INPUT 0x7e, 0x5e, 0xe7, 0x74, 0x64, 0x16, 0x05, 0x00, 0x79, +DECODED 0x03f5, 0x05e8, 0x05bd, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xa8, 0x41, 0x7e, 0x5c, 0xc7, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x49, +DECODED 0x03f4, 0x05e7, 0x05bd, 0x05b8, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xa6, 0x41, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xeb, +DECODED 0x03f4, 0x05e7, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xa4, 0x41, 0x7e, 0x5c, 0xe7, 0x70, 0x64, 0x17, +INPUT 0x05, 0x00, 0x9c, +DECODED 0x03f3, 0x05e7, 0x05bd, 0x05a4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xa0, 0x41, 0x7e, 0x5c, 0x87, 0x6e, 0x7c, 0x7c, 0x1b, 0x1b, 0x96, +DECODED 0x03f2, 0x05e7, 0x05bd, 0x0598, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xa0, 0x41, 0x7e, 0x5c, 0xa7, 0x6c, 0x7d, 0x7c, 0x7c, 0x7c, 0xf0, +DECODED 0x03f2, 0x05e7, 0x05bd, 0x058f, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xa0, 0x41, 0x7e, 0x5c, 0x87, 0x6c, 0x64, 0x15, 0x05, 0x00, 0xba, +DECODED 0x03f2, 0x05e7, 0x05bd, 0x058e, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, +INPUT 0xf6, 0x41, 0x7e, 0x5c, 0x87, 0x70, 0x7c, 0x7c, 0x1b, 0x1b, 0xed, +DECODED 0x040d, 0x05e7, 0x05bd, 0x05a2, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xca, 0x42, 0x7e, 0x5e, 0x27, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x2d, +DECODED 0x044f, 0x05e7, 0x05bd, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x60, 0x44, 0x7e, 0x5c, +INPUT 0x27, 0x74, 0x64, 0x18, 0x05, 0x00, 0x9a, +DECODED 0x04ce, 0x05e7, 0x05bd, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x08, 0x29, 0x7e, 0x5c, 0x27, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xe6, +DECODED 0x0642, 0x05e6, 0x05bd, 0x05b4, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x92, 0x2a, 0x7e, 0x5c, 0x47, 0x74, 0x7d, 0x7c, +INPUT 0x7c, 0x7c, 0x83, +DECODED 0x06bd, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x16, 0x26, 0x7e, 0x5c, 0x47, 0x74, 0x64, 0x17, 0x05, 0x00, 0xbe, +DECODED 0x0557, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xe2, 0x25, 0x7e, 0x5c, 0x47, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xdb, +DECODED 0x0546, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x00, 0x2a, 0x7e, 0x5c, 0x47, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x7e, +DECODED 0x0690, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, +INPUT 0xb0, 0x28, 0x7e, 0x5e, 0x47, 0x74, 0x64, 0x18, 0x05, 0x00, 0xc9, +DECODED 0x0627, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x88, 0x25, 0x7e, 0x5c, 0x47, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x90, +DECODED 0x052a, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x2a, 0x28, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x39, +DECODED 0x05fd, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x20, 0x2a, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0xda, +DECODED 0x069a, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xb4, 0x26, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, +INPUT 0x1b, 0x1b, 0xf0, +DECODED 0x0588, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x6e, 0x26, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x1c, +DECODED 0x0572, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x70, 0x29, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0xbe, +DECODED 0x0663, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xe0, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x4b, +DECODED 0x05e6, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, +INPUT 0xa8, 0x26, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x6d, +DECODED 0x0584, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd0, 0x28, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x1a, 0x05, 0x00, 0x69, +DECODED 0x0631, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xaa, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xec, +DECODED 0x05d5, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xe8, 0x26, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x60, +DECODED 0x0598, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xa4, 0x28, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0xa4, +DECODED 0x0623, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x78, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x1c, +DECODED 0x05c5, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x32, 0x28, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x16, 0x05, 0x00, 0x96, +DECODED 0x05ff, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, +INPUT 0x6c, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xfc, +DECODED 0x05c2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x34, 0x08, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x73, +DECODED 0x0600, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x8c, 0x07, 0x7e, 0x5c, +INPUT 0x87, 0x74, 0x64, 0x1f, 0x05, 0x00, 0xa7, +DECODED 0x05cc, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x18, 0x28, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x15, +DECODED 0x05f7, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xce, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, +INPUT 0x7c, 0x7c, 0xdd, +DECODED 0x05e0, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xc0, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x1a, 0x05, 0x00, 0xf7, +DECODED 0x05dc, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xe8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x70, +DECODED 0x05e8, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xe8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x36, +DECODED 0x05e8, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, +INPUT 0xd0, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0xd1, +DECODED 0x05e1, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd0, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xd1, +DECODED 0x05e1, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd2, 0x27, 0x7e, 0x5c, +INPUT 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x67, +DECODED 0x05e1, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x26, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x26, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x60, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0x01, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x47, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, +INPUT 0xd4, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x9d, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x1f, 0x05, 0x00, 0xe7, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x07, 0x7e, 0x5c, +INPUT 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x47, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x60, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x19, +INPUT 0x05, 0x00, 0x26, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xea, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xcd, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x1b, 0x05, 0x00, 0x39, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xea, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x01, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, +INPUT 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x9b, 0x7c, 0x7c, +INPUT 0x7c, 0xf2, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x0677, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1c, 0x05, 0x00, 0x86, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x0677, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1b, 0x1b, 0xea, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x0677, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xc8, 0x7c, 0x7c, 0x7c, 0xa9, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x0758, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, +INPUT 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x16, 0x05, 0x00, 0x4e, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03f7, 0x03f7, 0x0758, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1c, 0x1c, 0xee, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03fc, 0x03fc, 0x0758, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd6, 0x7c, 0x7c, 0x7c, 0x4b, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03fc, 0x03fc, 0x079e, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03fc, 0x03fc, 0x079e, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, +INPUT 0x1c, 0x1c, 0x8f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03fc, 0x03fc, 0x079e, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd6, 0x7c, 0x7c, 0x7c, 0x87, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03fc, 0x03fc, 0x079e, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x15, 0x05, 0x00, 0x7f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03fc, 0x03fc, 0x079e, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1c, 0x1c, 0x8f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03fc, 0x03fc, 0x079e, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, +INPUT 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0xc2, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03fc, 0x03fc, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x16, 0x05, 0x00, 0x2f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03fc, 0x03fc, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, +INPUT 0x87, 0x74, 0x7c, 0x7c, 0x1c, 0x1c, 0x8f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03fc, 0x03fc, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0xc2, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03fc, 0x03fc, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x16, 0x05, 0x00, 0x2f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03fc, 0x03fc, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x1c, 0x1c, 0x8f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03fc, 0x03fc, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0xa3, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03fc, 0x03fc, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x1e, 0x05, 0x00, 0xa8, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x03fc, 0x03fc, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x2f, 0x2f, 0xaa, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x045b, 0x045b, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0xa3, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x045b, 0x045b, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, +INPUT 0x87, 0x74, 0x64, 0x15, 0x05, 0x00, 0x1e, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x045b, 0x045b, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x65, 0x65, 0xf1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x0569, 0x0569, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd7, 0x7c, +INPUT 0x7c, 0x7c, 0x0e, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x0569, 0x0569, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x25, 0x05, 0x00, 0x54, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x0569, 0x0569, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x7d, 0x7d, 0x62, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0xa3, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, +INPUT 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x7d, 0x7d, 0xfe, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, +INPUT 0x87, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0xc2, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x1a, 0x05, 0x00, 0xba, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, +INPUT 0x7d, 0x7d, 0x9f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0xc2, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x16, 0x05, 0x00, 0x2f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x7d, 0x7d, 0x62, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0x0e, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x1b, 0x05, 0x00, 0x39, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, +INPUT 0x87, 0x74, 0x7c, 0x7c, 0x7d, 0x7d, 0x62, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0xc2, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x17, +INPUT 0x05, 0x00, 0xac, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0x7d, 0x7d, 0xae, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0x6f, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x1e, 0x05, 0x00, 0xa8, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, +INPUT 0xd8, 0x07, 0x7e, 0x5a, 0x87, 0x74, 0x7c, 0x7c, 0x7d, 0x7d, 0xd1, +DECODED 0x05e3, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0xe7, 0x7d, 0x5a, 0x87, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0x46, +DECODED 0x05e3, 0x05e5, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x87, 0x7d, 0x5a, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0x8b, +DECODED 0x05e3, 0x05e3, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x47, 0x7d, 0x58, 0x87, 0x74, 0x7c, 0x7c, 0x7d, 0x7d, 0x2b, +DECODED 0x05e3, 0x05e2, 0x05bb, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7d, 0x56, 0x87, 0x74, 0xd7, 0x7c, +INPUT 0x7c, 0x7c, 0xc5, +DECODED 0x05e3, 0x05e1, 0x05bb, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x67, 0x7b, 0x1c, 0x87, 0x74, 0x64, 0x1a, 0x05, 0x00, 0x6d, +DECODED 0x05e3, 0x05d9, 0x05a9, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0xa7, 0x79, 0xa0, 0x86, 0x74, 0x7c, 0x7c, 0x7d, 0x7d, 0x93, +DECODED 0x05e3, 0x05d0, 0x0582, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x78, 0x24, 0x86, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0xa4, +DECODED 0x05e3, 0x05c8, 0x055b, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, +INPUT 0xd8, 0xa7, 0x73, 0x70, 0x85, 0x74, 0x64, 0x17, 0x05, 0x00, 0x39, +DECODED 0x05e3, 0x05b2, 0x0523, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0xe7, 0x6e, 0xbc, 0x84, 0x74, 0x7c, 0x7c, 0x7d, 0x7d, 0xab, +DECODED 0x05e3, 0x059a, 0x04eb, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x6d, 0xc4, +INPUT 0x83, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0x9e, +DECODED 0x05e3, 0x0591, 0x049d, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0xc7, 0x6e, 0x28, 0x83, 0x74, 0x64, 0x1a, 0x05, 0x00, 0x28, +DECODED 0x05e3, 0x059a, 0x046c, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0xa7, 0x72, 0xbc, 0x82, 0x74, 0x7c, 0x7c, +INPUT 0x7d, 0x7d, 0xe3, +DECODED 0x05e3, 0x05ad, 0x044b, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0xe7, 0x72, 0x60, 0x82, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0x08, +DECODED 0x05e3, 0x05ae, 0x042e, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x27, 0x73, 0x00, 0x82, 0x74, 0x64, 0x1c, 0x05, 0x00, 0xee, +DECODED 0x05e2, 0x05af, 0x0410, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x67, 0x73, 0xc0, 0x81, 0x74, 0x7c, 0x7c, 0x7d, 0x7d, 0x6d, +DECODED 0x05e2, 0x05b1, 0x03fc, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, +INPUT 0xd4, 0x67, 0x73, 0xa0, 0x81, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0xef, +DECODED 0x05e2, 0x05b1, 0x03f2, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x67, 0x73, 0x9c, 0x81, 0x74, 0x64, 0x1a, 0x05, 0x00, 0xf7, +DECODED 0x05e2, 0x05b1, 0x03f1, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xb8, 0x07, 0x75, 0x9e, +INPUT 0x81, 0x74, 0x7c, 0x7c, 0x7d, 0x7d, 0x03, +DECODED 0x05d9, 0x05b9, 0x03f1, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xb8, 0xe7, 0x7a, 0x0a, 0x82, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0x0f, +DECODED 0x05d9, 0x05d6, 0x0413, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xba, 0x07, 0x7d, 0x8c, 0x82, 0x74, 0x64, 0x17, +INPUT 0x05, 0x00, 0x07, +DECODED 0x05da, 0x05e1, 0x043c, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xbe, 0x07, 0x7d, 0xe4, 0x82, 0x74, 0x7c, 0x7c, 0x7d, 0x7d, 0x42, +DECODED 0x05db, 0x05e1, 0x0457, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x68, 0x67, 0x7d, 0x70, 0x83, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0x86, +DECODED 0x05c0, 0x05e3, 0x0483, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x06, 0x67, 0x7d, 0x00, 0x84, 0x74, 0x64, 0x19, 0x05, 0x00, 0xee, +DECODED 0x05a2, 0x05e3, 0x04b0, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, +INPUT 0x6e, 0x86, 0x7d, 0xe6, 0x84, 0x74, 0x7c, 0x7c, 0x7d, 0x7d, 0x42, +DECODED 0x0572, 0x05e3, 0x04f8, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd0, 0x85, 0x7d, 0x9c, 0x85, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0xc3, +DECODED 0x0541, 0x05e3, 0x0531, 0x05b6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x12, 0xa5, 0x7d, 0x44, +INPUT 0x46, 0x74, 0x64, 0x17, 0x05, 0x00, 0xfe, +DECODED 0x0506, 0x05e4, 0x0565, 0x05b5, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x70, 0xa4, 0x7d, 0x96, 0x46, 0x74, 0x7c, 0x7c, 0x7d, 0x7d, 0x72, +DECODED 0x04d3, 0x05e4, 0x057f, 0x05b5, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x96, 0xa3, 0x7d, 0xd0, 0x46, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0x97, +DECODED 0x048f, 0x05e4, 0x0591, 0x05b5, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x02, 0xa3, 0x7d, 0xe8, 0x46, 0x74, 0x64, 0x19, 0x05, 0x00, 0x6b, +DECODED 0x0461, 0x05e4, 0x0598, 0x05b5, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x62, 0xe2, 0x7d, 0xea, 0x46, 0x74, 0x7c, 0x7c, 0x7d, 0x7d, 0x0a, +DECODED 0x042f, 0x05e5, 0x0599, 0x05b5, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x1c, 0xe2, 0x7d, 0xee, 0x46, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0x4f, +DECODED 0x0419, 0x05e5, 0x059a, 0x05b5, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, +INPUT 0xac, 0xe1, 0x7d, 0xf2, 0x46, 0x74, 0x64, 0x19, 0x05, 0x00, 0x73, +DECODED 0x03f6, 0x05e5, 0x059b, 0x05b5, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xaa, 0xe1, 0x7d, 0x10, 0x47, 0x74, 0x7c, 0x7c, 0x7d, 0x7d, 0x11, +DECODED 0x03f5, 0x05e5, 0x05a5, 0x05b5, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xa8, 0xe1, 0x7d, 0x52, 0x47, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0x52, +DECODED 0x03f4, 0x05e5, 0x05b9, 0x05b5, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xa6, 0xe1, 0x7d, 0x52, 0xa7, 0x72, 0x64, 0x19, 0x05, 0x00, 0xae, +DECODED 0x03f4, 0x05e5, 0x05b9, 0x05ad, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xa4, 0xe1, 0x7d, 0x56, 0x87, 0x6e, 0x7c, 0x7c, 0x7d, 0x7d, 0xbf, +DECODED 0x03f3, 0x05e5, 0x05bb, 0x0598, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xa4, 0xe1, 0x7d, 0x56, 0x07, 0x6a, 0xd7, 0x7c, 0x7c, 0x7c, 0xf2, +DECODED 0x03f3, 0x05e5, 0x05bb, 0x0582, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xa0, 0xe1, 0x7d, 0x56, 0xa7, 0x65, 0x64, 0x1a, 0x05, 0x00, 0xfc, +DECODED 0x03f2, 0x05e5, 0x05bb, 0x056c, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xca, 0xa1, 0x7d, 0x58, 0x07, 0x60, 0x7c, 0x7c, 0x7d, 0x7d, 0x82, +DECODED 0x03ff, 0x05e4, 0x05bb, 0x0550, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, +INPUT 0x5e, 0xa2, 0x7d, 0x58, 0xc7, 0x5d, 0xd7, 0x7c, 0x7c, 0x7c, 0xf5, +DECODED 0x042d, 0x05e4, 0x05bb, 0x0545, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xf8, 0xa2, 0x7d, 0x58, 0x47, 0x5c, 0x64, 0x1c, 0x05, 0x00, 0x41, +DECODED 0x045d, 0x05e4, 0x05bb, 0x053d, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xa2, 0x83, 0x7d, 0x58, +INPUT 0x07, 0x5c, 0x7c, 0x7c, 0x7d, 0x7d, 0x9b, +DECODED 0x0493, 0x05e3, 0x05bb, 0x053c, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x0a, 0x84, 0x7d, 0x58, 0x07, 0x5c, 0xd7, 0x7c, 0x7c, 0x7c, 0x73, +DECODED 0x04b3, 0x05e3, 0x05bb, 0x053c, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x7a, 0x64, 0x7d, 0x58, 0x27, 0x5a, 0x64, 0x17, 0x05, 0x00, 0x9e, +DECODED 0x04d6, 0x05e3, 0x05bb, 0x0532, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xbc, 0x44, 0x7d, 0x58, 0x47, 0x56, 0x7c, 0x7c, 0x7d, 0x7d, 0xa1, +DECODED 0x04eb, 0x05e2, 0x05bb, 0x051f, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x30, 0x65, 0x7d, 0x58, 0x47, 0x54, 0xd7, 0x7c, 0x7c, 0x7c, 0x38, +DECODED 0x050f, 0x05e3, 0x05bb, 0x0515, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x7a, 0x85, 0x7d, 0x58, 0x47, 0x52, 0x64, 0x19, 0x05, 0x00, 0x5b, +DECODED 0x0526, 0x05e3, 0x05bb, 0x050b, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, +INPUT 0xd2, 0x85, 0x7d, 0x5a, 0x87, 0x50, 0x7c, 0x7c, 0x7d, 0x7d, 0x35, +DECODED 0x0541, 0x05e3, 0x05bc, 0x0502, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x38, 0xa6, 0x7d, 0x5a, 0x87, 0x4e, 0xd7, 0x7c, 0x7c, 0x7c, 0x9a, +DECODED 0x0561, 0x05e4, 0x05bc, 0x04f8, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x9c, 0xa6, 0x7d, 0x5a, +INPUT 0xc7, 0x4a, 0x64, 0x16, 0x05, 0x00, 0xf4, +DECODED 0x0581, 0x05e4, 0x05bc, 0x04e6, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd0, 0xa6, 0x7d, 0x5a, 0x87, 0x4a, 0x7c, 0x7c, 0x7d, 0x7d, 0x5d, +DECODED 0x0591, 0x05e4, 0x05bc, 0x04e4, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x06, 0xe7, 0x7d, 0x5c, 0x67, 0x4a, 0xd7, 0x7c, +INPUT 0x7c, 0x7c, 0x4a, +DECODED 0x05a2, 0x05e5, 0x05bd, 0x04e4, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x60, 0x07, 0x7e, 0x5c, 0xa7, 0x4a, 0x64, 0x19, 0x05, 0x00, 0xbd, +DECODED 0x05be, 0x05e6, 0x05bd, 0x04e5, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xc0, 0xc7, 0x87, 0x80, 0x87, 0x4e, 0x7c, 0x7c, 0x7d, 0x7d, 0x46, +DECODED 0x05dc, 0x0617, 0x05c8, 0x04f8, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xc0, 0xa7, 0x93, 0x80, 0x87, 0x52, 0xd7, 0x7c, 0x7c, 0x7c, 0x8d, +DECODED 0x05dc, 0x0652, 0x05c8, 0x050c, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xc6, 0x07, 0xa3, 0xb6, 0x87, 0x56, 0x64, 0x19, 0x05, 0x00, 0x10, +DECODED 0x05de, 0x069f, 0x05d9, 0x0520, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xc8, 0x27, 0xb0, 0xf8, 0xe7, 0x5c, 0x7c, 0x7c, 0x7d, 0x7d, 0x53, +DECODED 0x05de, 0x06e0, 0x05ed, 0x0540, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xe8, 0xe7, 0xc1, 0x4a, +INPUT 0x88, 0x63, 0xd7, 0x7c, 0x7c, 0x7c, 0xbf, +DECODED 0x05e8, 0x0739, 0x0607, 0x0561, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x08, 0x08, 0xcd, 0xb4, 0x28, 0x69, 0x64, 0x1a, 0x05, 0x00, 0xd1, +DECODED 0x05f2, 0x0771, 0x0628, 0x057d, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x3e, 0x08, 0xd4, 0x08, 0x49, 0x6e, 0x7c, 0x7c, +INPUT 0x7d, 0x7d, 0x59, +DECODED 0x0603, 0x0794, 0x0642, 0x0597, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x40, 0x08, 0xd4, 0x68, 0x09, 0x70, 0xd7, 0x7c, 0x7c, 0x7c, 0x52, +DECODED 0x0604, 0x0794, 0x0660, 0x05a0, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x42, 0x68, 0xca, 0x84, 0x29, 0x72, 0x64, 0x16, 0x05, 0x00, 0xa2, +DECODED 0x0604, 0x0764, 0x0669, 0x05aa, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x42, 0x68, 0xbf, 0x82, 0xa9, 0x73, 0x7c, 0x7c, 0x7d, 0x7d, 0x4e, +DECODED 0x0604, 0x072d, 0x0668, 0x05b2, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, +INPUT 0x26, 0x08, 0xb1, 0x16, 0xe9, 0x73, 0xd7, 0x7c, 0x7c, 0x7c, 0x1e, +DECODED 0x05fc, 0x06e5, 0x0647, 0x05b3, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x0c, 0x88, 0xa5, 0xaa, 0xe8, 0x73, 0x64, 0x1e, 0x05, 0x00, 0x26, +DECODED 0x05f4, 0x06ab, 0x0625, 0x05b3, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x08, 0x08, 0x99, 0x1e, 0xe8, 0x73, 0x7c, 0x7c, 0x7d, 0x7d, 0xe7, +DECODED 0x05f2, 0x066d, 0x05f9, 0x05b3, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x02, 0x88, 0x90, 0xe2, 0xe7, 0x73, 0xd7, 0x7c, 0x7c, 0x7c, 0xe0, +DECODED 0x05f0, 0x0642, 0x05e6, 0x05b3, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x00, 0xe8, 0x8b, 0xc6, 0x07, 0x74, 0x64, 0x16, +INPUT 0x05, 0x00, 0xee, +DECODED 0x05f0, 0x062b, 0x05de, 0x05b4, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x02, 0xe8, 0x89, 0x96, 0x07, 0x74, 0x7c, 0x7c, 0x7d, 0x7d, 0x87, +DECODED 0x05f0, 0x0621, 0x05cf, 0x05b4, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x1c, 0xc8, 0x87, 0x90, 0x07, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0x87, +DECODED 0x05f9, 0x0617, 0x05cd, 0x05b4, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x20, 0x08, 0x86, 0x90, 0x27, 0x74, 0x64, 0x1e, 0x05, 0x00, 0xf2, +DECODED 0x05fa, 0x060e, 0x05cd, 0x05b4, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, +INPUT 0x20, 0x28, 0x84, 0x8c, 0x27, 0x74, 0x7c, 0x7c, 0x7d, 0x7d, 0x73, +DECODED 0x05fa, 0x0604, 0x05cc, 0x05b4, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x24, 0x28, 0x80, 0x8c, 0x47, 0x74, 0xcf, 0x7c, 0x7c, 0x7c, 0xd9, +DECODED 0x05fb, 0x05f0, 0x05cc, 0x05b5, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x077b, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x28, 0x48, 0x7e, 0x90, +INPUT 0x47, 0x74, 0x64, 0x17, 0x05, 0x00, 0x30, +DECODED 0x05fc, 0x05e7, 0x05cd, 0x05b5, 0x05dc, 0x05dc, 0x05e1, 0x05e1, 0x077b, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x2a, 0x68, 0x7e, 0x90, 0x47, 0x74, 0x7c, 0x7c, 0x67, 0x67, 0xf4, +DECODED 0x05fd, 0x05e8, 0x05cd, 0x05b5, 0x05dc, 0x05dc, 0x0573, 0x0573, 0x077b, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x2e, 0x68, 0x7e, 0x90, 0x47, 0x74, 0xa7, 0x7c, 0x7c, 0x7c, 0xb9, +DECODED 0x05fe, 0x05e8, 0x05cd, 0x05b5, 0x05dc, 0x05dc, 0x0573, 0x0573, 0x06b3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x30, 0x68, 0x7e, 0x90, 0x87, 0x74, 0x64, 0x1c, 0x05, 0x00, 0xc5, +DECODED 0x05ff, 0x05e8, 0x05cd, 0x05b6, 0x05dc, 0x05dc, 0x0573, 0x0573, 0x06b3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x50, 0x68, 0x7e, 0x90, 0x87, 0x74, 0x7c, 0x7c, 0x37, 0x37, 0x66, +DECODED 0x0609, 0x05e8, 0x05cd, 0x05b6, 0x05dc, 0x05dc, 0x0483, 0x0483, 0x06b3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x50, 0x68, 0x7e, 0x90, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xf3, +DECODED 0x0609, 0x05e8, 0x05cd, 0x05b6, 0x05dc, 0x05dc, 0x0483, 0x0483, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x50, 0x68, 0x7e, 0x90, 0x87, 0x74, 0x64, 0x16, 0x05, 0x00, 0x11, +DECODED 0x0609, 0x05e8, 0x05cd, 0x05b6, 0x05dc, 0x05dc, 0x0483, 0x0483, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x32, 0x68, 0x7e, 0x8c, 0x87, 0x74, 0x7c, 0x7c, 0x2a, 0x2a, 0x44, +DECODED 0x05ff, 0x05e8, 0x05cc, 0x05b6, 0x05dc, 0x05dc, 0x0442, 0x0442, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x30, 0x68, 0x7e, 0x70, +INPUT 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xfb, +DECODED 0x05ff, 0x05e8, 0x05c3, 0x05b6, 0x05dc, 0x05dc, 0x0442, 0x0442, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x2a, 0x68, 0x7e, 0x70, 0x87, 0x74, 0x64, 0x1c, 0x05, 0x00, 0xf0, +DECODED 0x05fd, 0x05e8, 0x05c3, 0x05b6, 0x05dc, 0x05dc, 0x0442, 0x0442, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x26, 0x48, 0x7e, 0x6a, 0x87, 0x74, 0x7c, 0x7c, +INPUT 0x28, 0x28, 0xf6, +DECODED 0x05fc, 0x05e7, 0x05c1, 0x05b6, 0x05dc, 0x05dc, 0x0438, 0x0438, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x02, 0x48, 0x7e, 0x68, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x73, +DECODED 0x05f0, 0x05e7, 0x05c0, 0x05b6, 0x05dc, 0x05dc, 0x0438, 0x0438, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xfe, 0x47, 0x7e, 0x66, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0x37, +DECODED 0x05ef, 0x05e7, 0x05c0, 0x05b6, 0x05dc, 0x05dc, 0x0438, 0x0438, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xe0, 0x47, 0x7e, 0x66, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xc0, +DECODED 0x05e6, 0x05e7, 0x05c0, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, +INPUT 0xe0, 0x27, 0x7e, 0x62, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xf1, +DECODED 0x05e6, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xde, 0x27, 0x7e, 0x62, 0x87, 0x74, 0x64, 0x1d, 0x05, 0x00, 0x62, +DECODED 0x05e5, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xde, 0x27, 0x7e, 0x60, +INPUT 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0x76, +DECODED 0x05e5, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xde, 0x27, 0x7e, 0x60, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xcb, +DECODED 0x05e5, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xdc, 0x27, 0x7e, 0x60, 0x87, 0x74, 0x64, 0x17, +INPUT 0x05, 0x00, 0x3b, +DECODED 0x05e5, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xdc, 0x27, 0x7e, 0x5e, 0xc7, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0x5f, +DECODED 0x05e5, 0x05e6, 0x05bd, 0x05b8, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xdc, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xc7, +DECODED 0x05e5, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xdc, 0x27, 0x7e, 0x5e, 0xc7, 0x74, 0x64, 0x1b, 0x05, 0x00, 0x16, +DECODED 0x05e5, 0x05e6, 0x05bd, 0x05b8, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xdc, 0x27, 0x7e, 0x60, 0xc7, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xc2, +DECODED 0x05e5, 0x05e6, 0x05be, 0x05b8, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xdc, 0x07, 0x7e, 0x7c, 0xc7, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xd3, +DECODED 0x05e5, 0x05e6, 0x05c7, 0x05b8, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x20, 0x08, 0x7e, 0xc8, +INPUT 0xc7, 0x74, 0x64, 0x19, 0x05, 0x00, 0xa8, +DECODED 0x05fa, 0x05e6, 0x05de, 0x05b8, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x88, 0x08, 0x7e, 0x36, 0xc8, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0x7a, +DECODED 0x061a, 0x05e6, 0x0601, 0x05b8, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x26, 0x09, 0x7e, 0xb8, 0xc8, 0x74, 0x7d, 0x7c, +INPUT 0x7c, 0x7c, 0x1c, +DECODED 0x064c, 0x05e6, 0x0629, 0x05b8, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xb8, 0x09, 0x7e, 0x16, 0xc9, 0x74, 0x64, 0x1b, 0x05, 0x00, 0xab, +DECODED 0x0679, 0x05e6, 0x0647, 0x05b8, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x60, 0x0a, 0x7e, 0x58, 0xc9, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xe2, +DECODED 0x06ae, 0x05e6, 0x065b, 0x05b8, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xb8, 0x2a, 0x7e, 0x76, 0x89, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x60, +DECODED 0x06c9, 0x05e6, 0x0665, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, +INPUT 0x26, 0x2b, 0x7e, 0x90, 0x89, 0x74, 0x64, 0x17, 0x05, 0x00, 0x3c, +DECODED 0x06ec, 0x05e6, 0x066d, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x6c, 0x2b, 0x7e, 0xaa, 0x89, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xf5, +DECODED 0x0701, 0x05e6, 0x0675, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xb0, 0x2b, 0x7e, 0xc4, 0x49, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x24, +DECODED 0x0717, 0x05e6, 0x067d, 0x05b5, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xee, 0x2b, 0x7e, 0xc4, 0x29, 0x74, 0x64, 0x1a, 0x05, 0x00, 0xa8, +DECODED 0x072a, 0x05e6, 0x067d, 0x05b4, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x2a, 0x2c, 0x7e, 0xc8, 0x29, 0x74, 0x7c, 0x7c, +INPUT 0x29, 0x29, 0x76, +DECODED 0x073d, 0x05e6, 0x067e, 0x05b4, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x2c, 0x2c, 0x7e, 0xe4, 0x29, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x71, +DECODED 0x073d, 0x05e6, 0x0687, 0x05b4, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x34, 0x2c, 0x7e, 0xe8, 0x29, 0x74, 0x64, 0x17, 0x05, 0x00, 0x2c, +DECODED 0x0740, 0x05e6, 0x0688, 0x05b4, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x38, 0x2c, 0x7e, 0x06, 0x4a, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xf9, +DECODED 0x0741, 0x05e6, 0x0692, 0x05b5, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, +INPUT 0x3e, 0x2c, 0x7e, 0x3e, 0x4a, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x6d, +DECODED 0x0743, 0x05e6, 0x06a3, 0x05b5, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x40, 0x2c, 0x7e, 0x5a, 0x4a, 0x74, 0x64, 0x1c, 0x05, 0x00, 0x7d, +DECODED 0x0744, 0x05e6, 0x06ac, 0x05b5, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x42, 0x2c, 0x7e, 0x78, 0x4a, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xd6, +DECODED 0x0744, 0x05e6, 0x06b5, 0x05b5, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x42, 0x2c, 0x7e, 0x78, 0x4a, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x6b, +DECODED 0x0744, 0x05e6, 0x06b5, 0x05b5, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x0e, 0x2c, 0x7e, 0x74, 0x4a, 0x74, 0x64, 0x17, +INPUT 0x05, 0x00, 0xba, +DECODED 0x0734, 0x05e6, 0x06b4, 0x05b5, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xee, 0x2b, 0x7e, 0x4c, 0x8a, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0x38, +DECODED 0x072a, 0x05e6, 0x06a7, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xb0, 0x2b, 0x7e, 0xda, 0x89, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x7b, +DECODED 0x0717, 0x05e6, 0x0684, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x50, 0x2b, 0x7e, 0x74, 0x89, 0x74, 0x64, 0x1c, 0x05, 0x00, 0xbd, +DECODED 0x06f9, 0x05e6, 0x0664, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, +INPUT 0xbc, 0x2a, 0x7e, 0xf0, 0x88, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xab, +DECODED 0x06ca, 0x05e6, 0x063b, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x50, 0x2a, 0x7e, 0x7a, 0x88, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x49, +DECODED 0x06a9, 0x05e6, 0x0616, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd0, 0x29, 0x7e, 0xe8, +INPUT 0xc7, 0x74, 0x64, 0x18, 0x05, 0x00, 0x5d, +DECODED 0x0681, 0x05e6, 0x05e8, 0x05b8, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x54, 0x29, 0x7e, 0x86, 0xc7, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0x07, +DECODED 0x065a, 0x05e6, 0x05ca, 0x05b8, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xe8, 0x28, 0x7e, 0x60, 0x87, 0x74, 0x7d, 0x7c, +INPUT 0x7c, 0x7c, 0xdf, +DECODED 0x0638, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xbc, 0x28, 0x7e, 0x60, 0x87, 0x74, 0x64, 0x1c, 0x05, 0x00, 0x84, +DECODED 0x062b, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x82, 0x28, 0x80, 0x60, 0x27, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0x47, +DECODED 0x0618, 0x05f0, 0x05be, 0x05b4, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x4a, 0x88, 0x85, 0x60, 0x47, 0x70, 0x7d, 0x7c, 0x7c, 0x7c, 0x06, +DECODED 0x0607, 0x060b, 0x05be, 0x05a1, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, +INPUT 0x0e, 0xe8, 0x89, 0x60, 0x07, 0x6c, 0x64, 0x1b, 0x05, 0x00, 0xef, +DECODED 0x05f4, 0x0621, 0x05be, 0x058c, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xea, 0xc7, 0x8b, 0x60, 0x27, 0x68, 0x7c, 0x7c, 0x29, 0x29, 0xfa, +DECODED 0x05e9, 0x062b, 0x05be, 0x0578, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x88, 0xc7, 0x8f, 0x60, 0x67, 0x61, 0x7d, 0x7c, 0x7c, 0x7c, 0xe6, +DECODED 0x05ca, 0x063f, 0x05be, 0x0557, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x38, 0x67, 0x99, 0x60, 0x87, 0x5a, 0x64, 0x1d, 0x05, 0x00, 0x60, +DECODED 0x05b1, 0x066f, 0x05be, 0x0534, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xb8, 0xa6, 0xa7, 0x60, 0x27, 0x54, 0x7c, 0x7c, 0x29, 0x29, 0xbf, +DECODED 0x0589, 0x06b6, 0x05be, 0x0515, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x3a, 0x06, 0xb2, 0x62, 0x27, 0x4f, 0x7d, 0x7c, 0x7c, 0x7c, 0x67, +DECODED 0x0562, 0x06ea, 0x05be, 0x04fc, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xf8, 0x85, 0xbc, 0x66, 0x07, 0x4b, 0x64, 0x17, 0x05, 0x00, 0xc0, +DECODED 0x054d, 0x071e, 0x05c0, 0x04e7, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xbc, 0x05, 0xbe, 0x68, 0xc7, 0x4a, 0x7c, 0x7c, 0x29, 0x29, 0x64, +DECODED 0x053b, 0x0726, 0x05c0, 0x04e6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, +INPUT 0x98, 0x65, 0xba, 0x68, 0x67, 0x4e, 0x7d, 0x7c, 0x7c, 0x7c, 0xad, +DECODED 0x052f, 0x0714, 0x05c0, 0x04f8, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x78, 0xc5, 0xb1, 0x66, 0x67, 0x53, 0x64, 0x1d, 0x05, 0x00, 0x75, +DECODED 0x0525, 0x06e8, 0x05c0, 0x0511, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x7a, 0x25, 0xa5, 0x66, 0x67, 0x59, 0x7c, 0x7c, 0x29, 0x29, 0xca, +DECODED 0x0526, 0x06a9, 0x05c0, 0x052f, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xb8, 0x05, 0x99, 0x66, 0xc7, 0x5e, 0x7d, 0x7c, 0x7c, 0x7c, 0xb3, +DECODED 0x0539, 0x066d, 0x05c0, 0x054a, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x16, 0xc6, 0x8a, 0x62, 0x27, 0x69, 0x64, 0x17, 0x05, 0x00, 0x58, +DECODED 0x0557, 0x0626, 0x05be, 0x057d, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xa8, 0x26, 0x80, 0x62, 0xe7, 0x71, 0x7c, 0x7c, 0x29, 0x29, 0x5d, +DECODED 0x0584, 0x05f0, 0x05be, 0x05a9, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x50, 0x27, 0x7e, 0x80, 0xa7, 0x73, 0x7d, 0x7c, 0x7c, 0x7c, 0x35, +DECODED 0x05b9, 0x05e6, 0x05c8, 0x05b2, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xe2, 0x27, 0x7e, 0xc6, 0xe7, 0x73, 0x64, 0x1a, 0x05, 0x00, 0x2b, +DECODED 0x05e6, 0x05e6, 0x05de, 0x05b3, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, +INPUT 0x26, 0x08, 0x7e, 0x1e, 0x28, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xb5, +DECODED 0x05fc, 0x05e6, 0x05f9, 0x05b4, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x80, 0x08, 0x7e, 0x6e, 0x48, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xdb, +DECODED 0x0618, 0x05e6, 0x0612, 0x05b5, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x98, 0x68, 0x7c, 0xd8, +INPUT 0x88, 0x74, 0x64, 0x17, 0x05, 0x00, 0xb5, +DECODED 0x061f, 0x05de, 0x0633, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x2e, 0x08, 0x7c, 0x1c, 0x89, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xdf, +DECODED 0x05fe, 0x05dc, 0x0649, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xa8, 0x87, 0x7a, 0x52, 0x89, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xfb, +DECODED 0x05d4, 0x05d4, 0x0659, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x1a, 0xa8, 0x7e, 0x7c, 0x86, 0x74, 0x64, 0x17, 0x05, 0x00, 0xa6, +DECODED 0x05f8, 0x05e9, 0x0577, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xf0, 0x67, 0x7e, 0x52, 0x88, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xb5, +DECODED 0x05eb, 0x05e8, 0x0609, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xc0, 0x67, 0x7e, 0xe0, 0x86, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x71, +DECODED 0x05dc, 0x05e8, 0x0596, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, +INPUT 0xd8, 0x47, 0x7e, 0xbc, 0x86, 0x74, 0x64, 0x18, 0x05, 0x00, 0xbb, +DECODED 0x05e3, 0x05e7, 0x058b, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x18, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xd0, +DECODED 0x05e3, 0x05e6, 0x05a7, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x18, +INPUT 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x6d, +DECODED 0x05e3, 0x05e6, 0x05a7, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x62, 0x87, 0x74, 0x64, 0x1a, 0x05, 0x00, 0x27, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x56, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xd3, +DECODED 0x05e3, 0x05e6, 0x05bb, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x56, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x6e, +DECODED 0x05e3, 0x05e6, 0x05bb, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x58, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0x46, +DECODED 0x05e3, 0x05e6, 0x05bb, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5a, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xa2, +DECODED 0x05e3, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, +INPUT 0xd8, 0x27, 0x7e, 0x5a, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x1f, +DECODED 0x05e3, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0x69, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, +INPUT 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0x70, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0xcd, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xec, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, +INPUT 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xec, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xec, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0xa7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0x8d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0xa7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, +INPUT 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0x94, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0x8d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, +INPUT 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0xc6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xec, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xec, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1c, 0x05, 0x00, 0x86, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, +INPUT 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xec, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xec, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, +INPUT 0xde, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, +INPUT 0x8d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, +INPUT 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0x94, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xec, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xde, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0x8d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xde, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0xec, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0x8d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0x8d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, +INPUT 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xde, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0x29, 0x29, 0x8d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xde, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0x7c, 0x29, 0x29, 0xb0, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xde, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0x7c, 0x29, 0x29, 0xd1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, +INPUT 0x1a, 0x05, 0x00, 0x47, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0x7c, 0x29, 0x29, 0xd1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xde, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0x94, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0x7c, 0x29, 0x29, 0xd1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xde, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, +INPUT 0x64, 0x19, 0x05, 0x00, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0x7c, 0x29, 0x29, 0xd1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xde, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1a, 0x05, 0x00, 0x47, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0x7c, 0x29, 0x29, 0xd1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0x7c, 0x29, 0x29, 0xd1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xde, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0x7c, 0x29, 0x29, 0xb0, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1a, 0x05, 0x00, 0x47, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0x7c, 0x29, 0x29, 0xd1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x27, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1b, 0x05, 0x00, 0xa5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x46, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1a, 0x05, 0x00, 0x26, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x46, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xde, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x46, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xde, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1a, 0x05, 0x00, 0x47, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, +INPUT 0x29, 0x29, 0x46, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xde, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, +INPUT 0x5e, 0x87, 0x74, 0x64, 0x1a, 0x05, 0x00, 0x26, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x27, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x46, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x27, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, +INPUT 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0xe2, 0x7c, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, +INPUT 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x27, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0xc6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x27, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0xc6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1b, 0x05, 0x00, 0xa5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x46, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0xc6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x27, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, +INPUT 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0xc6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x27, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0xc6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x27, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, +INPUT 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0xe2, 0x7c, 0x7c, 0xa7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x46, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, +INPUT 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x27, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0x94, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, +INPUT 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x46, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x27, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x27, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, +INPUT 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x15, 0xe2, 0x29, 0x29, 0x27, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1c, 0x05, 0x00, 0xe7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x29, 0x29, 0x7b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0x94, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xe2, 0xe2, 0x29, 0x29, 0x92, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, +INPUT 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xe2, 0xe2, 0x29, 0x29, 0x92, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0xe2, 0xe2, 0x29, 0x29, 0xf3, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0x94, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0xe2, 0xe2, 0x29, 0x29, 0xf3, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, +INPUT 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0x94, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0xe2, 0xe2, 0x29, 0x29, 0xf3, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0xe2, 0xe2, 0x29, 0x29, 0xf3, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xe2, 0xe2, 0x29, 0x29, 0x92, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0xe2, 0xe2, 0x29, 0x29, 0xf3, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x29, 0x29, 0x1a, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x29, 0x29, 0x1a, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x29, 0x29, 0x7b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0x94, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x29, 0x29, 0x1a, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x29, 0x29, 0x1a, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x29, 0x29, 0x1a, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x29, 0x29, 0x1a, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x29, 0x29, 0x7b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x29, 0x29, 0x7b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x76, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x29, 0x29, 0x7b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1b, 0x05, 0x00, 0xa5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x29, 0x29, 0x7b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x29, 0x29, 0x7b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0x94, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x29, 0x29, 0x7b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x2a, 0x2a, 0xad, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x0442, 0x0442, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x0442, 0x0442, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, +INPUT 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x0442, 0x0442, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x29, 0x29, 0x7b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x51, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x29, 0x29, 0x7b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0x94, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x29, 0x29, 0x1a, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1b, 0x05, 0x00, 0xa5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x29, 0x29, 0x7b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1a, 0x05, 0x00, 0x47, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x043d, 0x043d, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x2d, 0x2d, 0xa9, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x0451, 0x0451, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7d, 0x7c, 0x7c, 0x7c, 0x30, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x0451, 0x0451, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1b, 0x05, 0x00, 0xa5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x0451, 0x0451, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x4c, 0x4c, 0x44, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x04ec, 0x04ec, 0x05e1, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x98, 0x7c, 0x7c, 0x7c, 0xc0, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x04ec, 0x04ec, 0x0668, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x04ec, 0x04ec, 0x0668, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x6c, 0x6c, 0x34, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x058c, 0x058c, 0x0668, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xbf, 0x7c, +INPUT 0x7c, 0x7c, 0x47, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x058c, 0x058c, 0x072b, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1c, 0x05, 0x00, 0x86, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x058c, 0x058c, 0x072b, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x7d, 0x7d, 0x69, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x05e1, 0x05e1, 0x072b, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xce, 0x7c, 0x7c, 0x7c, 0x2e, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x05e1, 0x05e1, 0x0776, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0xfc, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x05e1, 0x05e1, 0x0776, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x7d, 0x7d, 0x69, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x05e1, 0x05e1, 0x0776, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xd7, 0x7c, 0x7c, 0x7c, 0x5e, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1a, 0x05, 0x00, 0x26, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x7d, 0x7d, 0x69, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x05e1, 0x05e1, 0x07a3, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x07, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x05e1, 0x05e1, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1b, 0x05, 0x00, 0xa5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x05e1, 0x05e1, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0x8e, 0x8e, 0x87, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x0636, 0x0636, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x07, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x0636, 0x0636, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1c, 0x05, 0x00, 0x86, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x0636, 0x0636, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0xbe, 0xbe, 0x74, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x0726, 0x0726, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x07, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x0726, 0x0726, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x0726, 0x0726, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xdb, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xd9, 0x7c, 0x7c, 0x7c, 0x42, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07ad, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1c, 0x05, 0x00, 0x86, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07ad, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xdb, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07ad, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xd9, 0x7c, 0x7c, 0x7c, 0x42, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07ad, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0xfc, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07ad, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x07, 0x7e, 0x60, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x8a, +DECODED 0x05e2, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07ad, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x07, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1b, 0x05, 0x00, 0xa5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xdb, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x07, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xdb, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, +INPUT 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x07, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1a, 0x05, 0x00, 0x26, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xdb, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x07, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xf5, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x17, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x07, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0xe7, 0x7f, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0x0b, +DECODED 0x05e2, 0x05ef, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x9a, 0x87, 0x8e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xf2, +DECODED 0x05d0, 0x0638, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x38, 0x47, 0xb0, 0x5e, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x65, +DECODED 0x05b1, 0x06e1, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xe0, 0x46, 0xcc, 0x5e, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, 0xd2, +DECODED 0x0596, 0x076d, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x98, 0x66, 0xe2, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xc3, +DECODED 0x057f, 0x07dc, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x78, 0x66, 0xe2, 0x5c, 0x47, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x76, +DECODED 0x0575, 0x07dc, 0x05bd, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x48, 0x66, 0xe2, 0x5a, 0xe7, 0x6b, 0x64, 0x1a, 0x05, 0x00, 0x7d, +DECODED 0x0566, 0x07dc, 0x05bc, 0x058b, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x28, 0x66, 0xe2, 0x58, 0xa7, 0x67, 0x7c, 0xe2, 0xe2, 0xe2, 0x85, +DECODED 0x055c, 0x07dc, 0x05bb, 0x0576, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xb8, 0x65, 0xe2, 0x3c, 0x07, 0x63, 0xd8, 0x7c, 0x7c, 0x7c, 0x74, +DECODED 0x0539, 0x07dc, 0x05b3, 0x055f, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x38, 0x65, 0xe2, 0x3e, 0x27, 0x63, 0x64, 0x19, 0x05, 0x00, 0x1a, +DECODED 0x0511, 0x07dc, 0x05b3, 0x055f, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xb8, 0x64, 0xe2, 0x58, 0xc7, 0x66, 0x7c, 0xe2, 0xe2, 0xe2, 0xec, +DECODED 0x04e9, 0x07dc, 0x05bb, 0x0572, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x2e, 0x64, 0xe2, 0x58, 0xe7, 0x66, 0xd8, 0x7c, 0x7c, 0x7c, 0xc2, +DECODED 0x04be, 0x07dc, 0x05bb, 0x0572, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xb0, 0x43, 0xd5, 0x58, 0x27, 0x67, 0x64, 0x1a, 0x05, 0x00, 0x74, +DECODED 0x0497, 0x079a, 0x05bb, 0x0573, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xde, 0x22, 0xc1, 0x58, 0x67, 0x6a, 0x7c, 0xe2, 0xe2, 0xe2, 0x19, +DECODED 0x0455, 0x0735, 0x05bb, 0x0584, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x9e, 0x01, 0xa4, 0x58, 0xc7, 0x72, 0xd8, 0x7c, 0x7c, 0x7c, 0x7b, +DECODED 0x03f1, 0x06a4, 0x05bb, 0x05ae, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x9e, 0xc1, 0x8f, 0x58, 0xc7, 0x72, 0x64, 0x1b, 0x05, 0x00, 0xa1, +DECODED 0x03f1, 0x063f, 0x05bb, 0x05ae, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x9e, 0x21, 0x80, 0x58, 0xc7, 0x72, 0x7c, 0xe2, 0xe2, 0xe2, 0x56, +DECODED 0x03f1, 0x05f0, 0x05bb, 0x05ae, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x9e, 0x01, 0x7e, 0x58, 0x87, 0x72, 0xd8, 0x7c, 0x7c, 0x7c, 0x12, +DECODED 0x03f1, 0x05e6, 0x05bb, 0x05ac, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x9e, 0x01, 0x7e, 0x5a, 0x27, 0x72, 0x64, 0x1c, 0x05, 0x00, 0xdc, +DECODED 0x03f1, 0x05e6, 0x05bc, 0x05aa, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x9e, 0x01, 0x7e, 0x5a, 0x07, 0x72, 0x7c, 0xe2, 0xe2, 0xe2, 0x79, +DECODED 0x03f1, 0x05e6, 0x05bc, 0x05aa, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x9e, 0x01, 0x7e, 0x5a, 0xe7, 0x71, 0xd8, 0x7c, 0x7c, 0x7c, 0x3d, +DECODED 0x03f1, 0x05e6, 0x05bc, 0x05a9, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x9e, 0x01, 0x7e, 0x5a, 0x27, 0x72, 0x64, 0x18, 0x05, 0x00, 0xaf, +DECODED 0x03f1, 0x05e6, 0x05bc, 0x05aa, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xa0, 0x01, 0x7e, 0x5a, 0xa7, 0x73, 0x7c, 0xe2, 0xe2, 0xe2, 0xba, +DECODED 0x03f2, 0x05e6, 0x05bc, 0x05b2, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x22, 0x02, 0x7e, 0x5a, 0xa7, 0x73, 0xd8, 0x7c, 0x7c, 0x7c, 0xb4, +DECODED 0x041b, 0x05e6, 0x05bc, 0x05b2, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x1e, 0x03, 0x7e, 0x5a, 0xc7, 0x71, 0x64, 0x19, 0x05, 0x00, 0xc0, +DECODED 0x0469, 0x05e6, 0x05bc, 0x05a9, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x0e, 0x04, 0x7e, 0x5a, 0x47, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xc5, +DECODED 0x04b4, 0x05e6, 0x05bc, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x20, 0x05, 0x7e, 0x5a, 0x47, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x9f, +DECODED 0x050a, 0x05e6, 0x05bc, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x08, 0x06, 0x7e, 0x5a, 0x47, 0x74, 0x64, 0x17, 0x05, 0x00, 0x26, +DECODED 0x0552, 0x05e6, 0x05bc, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x88, 0x09, 0x7e, 0x5a, 0x47, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x3f, +DECODED 0x066a, 0x05e6, 0x05bc, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x7e, 0x08, 0x7e, 0x5a, 0x47, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x3f, +DECODED 0x0617, 0x05e6, 0x05bc, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xe0, 0x06, 0x7e, 0x3e, 0x47, 0x74, 0x64, 0x1a, 0x05, 0x00, 0xfd, +DECODED 0x0596, 0x05e6, 0x05b3, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x88, 0x08, 0x7e, 0x1c, 0x47, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x55, +DECODED 0x061a, 0x05e6, 0x05a9, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x1a, 0x28, 0x7e, 0xb8, 0x46, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0xc4, +DECODED 0x05f8, 0x05e6, 0x0589, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x58, 0x28, 0x7e, 0x38, 0x46, 0x74, 0x64, 0x17, 0x05, 0x00, 0xa4, +DECODED 0x060b, 0x05e6, 0x0561, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xa0, 0x28, 0x7e, 0x64, 0x45, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xc6, +DECODED 0x0622, 0x05e6, 0x051f, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xea, 0x28, 0x7e, 0x9e, 0x24, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x0e, +DECODED 0x0639, 0x05e6, 0x04e1, 0x05b4, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x4c, 0x29, 0x7e, 0x8c, 0xa3, 0x73, 0x64, 0x19, 0x05, 0x00, 0x19, +DECODED 0x0658, 0x05e6, 0x048c, 0x05b2, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x8e, 0x09, 0x7e, 0xc8, 0xa2, 0x73, 0x7c, 0xe2, 0xe2, 0xe2, 0x6b, +DECODED 0x066c, 0x05e6, 0x044e, 0x05b2, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xde, 0x09, 0x7e, 0xcc, 0xe1, 0x73, 0xd8, 0x7c, 0x7c, 0x7c, 0xbb, +DECODED 0x0685, 0x05e6, 0x0400, 0x05b3, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x20, 0x0a, 0x7e, 0x5a, 0xe1, 0x73, 0x64, 0x17, 0x05, 0x00, 0xa0, +DECODED 0x069a, 0x05e6, 0x03dc, 0x05b3, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x80, 0xea, 0x7d, 0x5a, 0xe1, 0x73, 0x7c, 0xe2, 0xe2, 0xe2, 0x8a, +DECODED 0x06b8, 0x05e5, 0x03dc, 0x05b3, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xf2, 0xaa, 0x7d, 0x5a, 0x01, 0x72, 0xd8, 0x7c, 0x7c, 0x7c, 0xb3, +DECODED 0x06db, 0x05e4, 0x03dc, 0x05aa, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x6c, 0xab, 0x7d, 0x5a, 0x21, 0x6e, 0x64, 0x1a, 0x05, 0x00, 0xb3, +DECODED 0x0701, 0x05e4, 0x03dc, 0x0596, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xf8, 0xab, 0x7d, 0x5a, 0xc1, 0x6b, 0x7c, 0xe2, 0xe2, 0xe2, 0x53, +DECODED 0x072d, 0x05e4, 0x03dc, 0x058b, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xb0, 0xac, 0x7d, 0x5a, 0xa1, 0x65, 0xd8, 0x7c, 0x7c, 0x7c, 0x64, +DECODED 0x0767, 0x05e4, 0x03dc, 0x056c, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x40, 0x8d, 0x7d, 0x5a, 0x01, 0x64, 0x64, 0x1c, 0x05, 0x00, 0x3a, +DECODED 0x0794, 0x05e3, 0x03dc, 0x0564, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xda, 0xad, 0x7d, 0x5a, 0x01, 0x64, 0x7c, 0xe2, 0xe2, 0xe2, 0xc0, +DECODED 0x07c4, 0x05e4, 0x03dc, 0x0564, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x26, 0xae, 0x7d, 0x5a, 0x21, 0x64, 0xd8, 0x7c, 0x7c, 0x7c, 0xfc, +DECODED 0x07dc, 0x05e4, 0x03dc, 0x0564, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x26, 0xae, 0x7d, 0x92, 0xe1, 0x66, 0x64, 0x1c, 0x05, 0x00, 0xb1, +DECODED 0x07dc, 0x05e4, 0x03ee, 0x0572, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x26, 0xae, 0x7d, 0xea, 0x02, 0x6a, 0x7c, 0xe2, 0xe2, 0xe2, 0xfa, +DECODED 0x07dc, 0x05e4, 0x0459, 0x0582, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x26, 0xae, 0x7d, 0xea, 0x44, 0x6e, 0xd8, 0x7c, 0x7c, 0x7c, 0x5e, +DECODED 0x07dc, 0x05e4, 0x04f9, 0x0597, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x26, 0xae, 0x7d, 0x54, 0x06, 0x70, 0x64, 0x17, 0x05, 0x00, 0x99, +DECODED 0x07dc, 0x05e4, 0x056a, 0x05a0, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x26, 0xae, 0x7d, 0x60, 0x07, 0x72, 0x7c, 0xe2, 0xe2, 0xe2, 0x6d, +DECODED 0x07dc, 0x05e4, 0x05be, 0x05aa, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x26, 0xae, 0x7d, 0x60, 0xe7, 0x73, 0xd8, 0x7c, 0x7c, 0x7c, 0x90, +DECODED 0x07dc, 0x05e4, 0x05be, 0x05b3, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xf4, 0xad, 0x7d, 0x60, 0x07, 0x74, 0x64, 0x1c, 0x05, 0x00, 0x87, +DECODED 0x07cc, 0x05e4, 0x05be, 0x05b4, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x82, 0xad, 0x7d, 0x60, 0x27, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xd3, +DECODED 0x07a8, 0x05e4, 0x05be, 0x05b4, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x52, 0xac, 0x7d, 0x62, 0x27, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0xa4, +DECODED 0x0749, 0x05e4, 0x05be, 0x05b4, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xcc, 0xaa, 0x7d, 0x62, 0x47, 0x74, 0x64, 0x16, 0x05, 0x00, 0x4e, +DECODED 0x06cf, 0x05e4, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xc4, 0xa6, 0x7d, 0x62, 0x47, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xac, +DECODED 0x058d, 0x05e4, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x34, 0xe6, 0x7d, 0x62, 0x47, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x3f, +DECODED 0x0560, 0x05e5, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x72, 0xe9, 0x7d, 0x60, 0x47, 0x74, 0x64, 0x19, 0x05, 0x00, 0x30, +DECODED 0x0663, 0x05e5, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x42, 0xe8, 0x7d, 0x62, 0x47, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xcd, +DECODED 0x0604, 0x05e5, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x6c, 0xe6, 0x7d, 0x60, 0x47, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x82, +DECODED 0x0572, 0x05e5, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd2, 0xe7, 0x7d, 0x60, 0x47, 0x74, 0x64, 0x17, 0x05, 0x00, 0x5f, +DECODED 0x05e1, 0x05e5, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x04, 0x07, 0x7e, 0x60, 0x47, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x84, +DECODED 0x05a1, 0x05e6, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x88, 0xa6, 0x83, 0x5e, 0x47, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x0d, +DECODED 0x057a, 0x0602, 0x05bd, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x9e, 0x05, 0x93, 0x5e, 0x47, 0x74, 0x64, 0x17, 0x05, 0x00, 0x8d, +DECODED 0x0531, 0x064f, 0x05bd, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xbe, 0x64, 0xa3, 0x5e, 0x27, 0x74, 0x7c, 0xe2, +INPUT 0xe2, 0xe2, 0x44, +DECODED 0x04eb, 0x06a1, 0x05bd, 0x05b4, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xc6, 0xc3, 0xb6, 0x5e, 0x07, 0x6b, 0xd8, 0x7c, 0x7c, 0x7c, 0xfd, +DECODED 0x049e, 0x0701, 0x05bd, 0x0587, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x22, 0x83, 0xc7, 0x5e, 0x87, 0x61, 0x64, 0x18, 0x05, 0x00, 0xff, +DECODED 0x046b, 0x0755, 0x05bd, 0x0557, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x5a, 0x82, 0xe0, 0x5e, 0x87, 0x57, 0x7c, 0xe2, 0xe2, 0xe2, 0xf1, +DECODED 0x042c, 0x07d2, 0x05bd, 0x0525, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xb0, 0x61, 0xe2, 0x5e, 0x07, 0x51, 0xd8, 0x7c, 0x7c, 0x7c, 0x05, +DECODED 0x03f7, 0x07dc, 0x05bd, 0x0505, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xaa, 0x61, 0xe2, 0x5c, 0x47, 0x45, 0x64, 0x1e, 0x05, 0x00, 0x17, +DECODED 0x03f5, 0x07dc, 0x05bd, 0x04ca, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xa6, 0x61, 0xe2, 0x5c, 0x87, 0x3b, 0x7c, 0xe2, 0xe2, 0xe2, 0x69, +DECODED 0x03f4, 0x07dc, 0x05bd, 0x0499, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xa4, 0x61, 0xe2, 0x5c, 0x87, 0x34, 0xd8, 0x7c, 0x7c, 0x7c, 0x17, +DECODED 0x03f3, 0x07dc, 0x05bd, 0x0476, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xa0, 0x61, 0xe2, 0x58, 0x87, 0x2f, 0x64, 0x18, 0x05, 0x00, 0x4a, +DECODED 0x03f2, 0x07dc, 0x05bb, 0x045d, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xa0, 0x61, 0xe2, 0x56, 0x67, 0x2d, 0x7c, 0xe2, 0xe2, 0xe2, 0xc6, +DECODED 0x03f2, 0x07dc, 0x05bb, 0x0453, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xa0, 0x61, 0xe2, 0x52, 0x27, 0x2d, 0xd8, 0x7c, 0x7c, 0x7c, 0x10, +DECODED 0x03f2, 0x07dc, 0x05b9, 0x0452, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x9e, 0x61, 0xe2, 0x56, 0xe7, 0x2e, 0x64, 0x15, 0x05, 0x00, 0x7c, +DECODED 0x03f1, 0x07dc, 0x05bb, 0x045a, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x9e, 0x81, 0xe0, 0x56, 0x87, 0x30, 0x7c, 0xe2, 0xe2, 0xe2, 0x8b, +DECODED 0x03f1, 0x07d2, 0x05bb, 0x0462, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x9e, 0x81, 0xce, 0x58, 0x67, 0x39, 0xd8, 0x7c, 0x7c, 0x7c, 0x10, +DECODED 0x03f1, 0x0778, 0x05bb, 0x048f, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xca, 0xe1, 0xba, 0x58, 0xc7, 0x40, 0x64, 0x16, 0x05, 0x00, 0x0e, +DECODED 0x03ff, 0x0716, 0x05bb, 0x04b4, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x64, 0xa2, 0x9e, 0x58, 0xa7, 0x4a, 0x7c, 0xe2, 0xe2, 0xe2, 0x39, +DECODED 0x042f, 0x0689, 0x05bb, 0x04e5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x0e, 0xe3, 0x8a, 0x58, 0x07, 0x54, 0xd8, 0x7c, 0x7c, 0x7c, 0x98, +DECODED 0x0464, 0x0626, 0x05bb, 0x0514, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xc6, 0xe3, 0x7d, 0x58, 0x87, 0x5d, 0x64, 0x15, 0x05, 0x00, 0xcf, +DECODED 0x049e, 0x05e5, 0x05bb, 0x0543, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x4a, 0xe4, 0x7d, 0x5a, 0xe7, 0x62, 0x7c, 0xe2, 0xe2, 0xe2, 0x47, +DECODED 0x04c7, 0x05e5, 0x05bc, 0x055e, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xe4, 0xe4, 0x7d, 0x5a, 0x67, 0x6a, 0xd8, 0x7c, 0x7c, 0x7c, 0xe1, +DECODED 0x04f7, 0x05e5, 0x05bc, 0x0584, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x68, 0xe5, 0x7d, 0x5a, 0xa7, 0x70, 0x64, 0x1b, 0x05, 0x00, 0x0c, +DECODED 0x0520, 0x05e5, 0x05bc, 0x05a3, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x18, 0xe6, 0x7d, 0x58, 0xa7, 0x72, 0x7c, 0xe2, 0xe2, 0xe2, 0xf8, +DECODED 0x0557, 0x05e5, 0x05bb, 0x05ad, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xc2, 0xa6, 0x7d, 0x5a, 0x07, 0x73, 0xd8, 0x7c, 0x7c, 0x7c, 0xd4, +DECODED 0x058c, 0x05e4, 0x05bc, 0x05af, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xc6, 0x87, 0x7d, 0x58, 0x47, 0x73, 0x64, 0x19, 0x05, 0x00, 0xdc, +DECODED 0x05de, 0x05e3, 0x05bb, 0x05b0, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x66, 0x88, 0x7a, 0x58, 0x87, 0x73, 0x7c, 0xe2, 0xe2, 0xe2, 0x1c, +DECODED 0x0610, 0x05d4, 0x05bb, 0x05b1, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x76, 0x69, 0x74, 0x58, 0xa7, 0x73, 0xd8, 0x7c, 0x7c, 0x7c, 0xb3, +DECODED 0x0665, 0x05b6, 0x05bb, 0x05b2, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x50, 0x2a, 0x70, 0x5a, 0xe7, 0x73, 0x64, 0x1a, 0x05, 0x00, 0x82, +DECODED 0x06a9, 0x05a0, 0x05bc, 0x05b3, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x68, 0xeb, 0x6c, 0x5a, 0xe7, 0x73, 0x7c, 0xe2, 0xe2, 0xe2, 0x0a, +DECODED 0x0700, 0x0590, 0x05bc, 0x05b3, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x3a, 0xac, 0x6a, 0x72, 0x07, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0xe8, +DECODED 0x0742, 0x0585, 0x05c3, 0x05b4, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x2a, 0xad, 0x66, 0xe8, 0x07, 0x74, 0x64, 0x17, 0x05, 0x00, 0xf4, +DECODED 0x078d, 0x0571, 0x05e8, 0x05b4, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xda, 0xad, 0x60, 0x6c, 0x08, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x75, +DECODED 0x07c4, 0x0553, 0x0612, 0x05b4, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x26, 0x6e, 0x5a, 0x3a, 0x09, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x3d, +DECODED 0x07dc, 0x0534, 0x0652, 0x05b4, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x26, 0x0e, 0x58, 0xee, 0x29, 0x74, 0x64, 0x1d, 0x05, 0x00, 0x35, +DECODED 0x07dc, 0x0528, 0x068a, 0x05b4, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x26, 0x2e, 0x56, 0xc4, 0xca, 0x75, 0x7c, 0xe2, 0xe2, 0xe2, 0xe0, +DECODED 0x07dc, 0x051f, 0x06cd, 0x05bd, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x26, 0x0e, 0x56, 0x66, 0x0b, 0x76, 0xd8, 0x7c, 0x7c, 0x7c, 0x3e, +DECODED 0x07dc, 0x051e, 0x0700, 0x05be, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x26, 0x8e, 0x57, 0x38, 0xac, 0x79, 0x64, 0x21, 0x05, 0x00, 0xcc, +DECODED 0x07dc, 0x0525, 0x0741, 0x05d0, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x26, 0x6e, 0x5b, 0xe8, 0xac, 0x79, 0x7c, 0xe2, 0xe2, 0xe2, 0x18, +DECODED 0x07dc, 0x0539, 0x0778, 0x05d0, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x26, 0x0e, 0x63, 0xd4, 0x0d, 0x7a, 0xd8, 0x7c, 0x7c, 0x7c, 0xc2, +DECODED 0x07dc, 0x055f, 0x07c2, 0x05d2, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x26, 0x0e, 0x63, 0xd6, 0x0d, 0x7a, 0x64, 0x23, 0x05, 0x00, 0x50, +DECODED 0x07dc, 0x055f, 0x07c3, 0x05d2, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x26, 0xae, 0x62, 0xd6, 0x0d, 0x78, 0x7c, 0xe2, 0xe2, 0xe2, 0x29, +DECODED 0x07dc, 0x055d, 0x07c3, 0x05c8, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x26, 0x0e, 0x65, 0xd6, 0x6d, 0x76, 0xd8, 0x7c, 0x7c, 0x7c, 0x81, +DECODED 0x07dc, 0x0569, 0x07c3, 0x05c0, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x26, 0xce, 0x6e, 0xd6, 0x0d, 0x76, 0x64, 0x18, 0x05, 0x00, 0x9f, +DECODED 0x07dc, 0x059a, 0x07c3, 0x05be, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x26, 0xae, 0x77, 0xd6, 0xad, 0x75, 0x7c, 0xe2, 0xe2, 0xe2, 0x45, +DECODED 0x07dc, 0x05c6, 0x07c3, 0x05bc, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xf4, 0x4d, 0x7e, 0xd6, 0x8d, 0x75, 0xd8, 0x7c, 0x7c, 0x7c, 0x1e, +DECODED 0x07cc, 0x05e7, 0x07c3, 0x05bb, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x64, 0x4d, 0x7e, 0x54, 0x6d, 0x75, 0x64, 0x24, 0x05, 0x00, 0x9f, +DECODED 0x079f, 0x05e7, 0x079a, 0x05bb, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x86, 0x2c, 0x7e, 0x5a, 0x2c, 0x75, 0x7c, 0xe2, 0xe2, 0xe2, 0xc0, +DECODED 0x075a, 0x05e6, 0x074c, 0x05b9, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xba, 0x2b, 0x7e, 0x76, 0x6b, 0x75, 0xd8, 0x7c, 0x7c, 0x7c, 0x54, +DECODED 0x071a, 0x05e6, 0x0705, 0x05bb, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xbe, 0x2a, 0x7e, 0x50, 0x8a, 0x75, 0x64, 0x19, 0x05, 0x00, 0x95, +DECODED 0x06cb, 0x05e6, 0x06a9, 0x05bb, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x0a, 0x2a, 0x7e, 0x90, 0x89, 0x75, 0x7c, 0xe2, 0xe2, 0xe2, 0x76, +DECODED 0x0693, 0x05e6, 0x066d, 0x05bb, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x32, 0x29, 0x7e, 0xc6, 0x68, 0x75, 0xd8, 0x7c, 0x7c, 0x7c, 0xbc, +DECODED 0x064f, 0x05e6, 0x062e, 0x05bb, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x80, 0x28, 0x7e, 0x22, 0x28, 0x75, 0x64, 0x25, 0x05, 0x00, 0xb6, +DECODED 0x0618, 0x05e6, 0x05fa, 0x05b9, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x60, 0x27, 0x75, 0x7c, 0xe2, 0xe2, 0xe2, 0x23, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b9, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x92, 0x27, 0x7e, 0x60, 0x07, 0x75, 0xd8, 0x7c, 0x7c, 0x7c, 0xa0, +DECODED 0x05cd, 0x05e6, 0x05be, 0x05b9, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x10, 0x27, 0x7e, 0x60, 0xe7, 0x74, 0x64, 0x19, 0x05, 0x00, 0x7b, +DECODED 0x05a5, 0x05e6, 0x05be, 0x05b8, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xb4, 0x26, 0x7e, 0x5e, 0xe7, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x7d, +DECODED 0x0588, 0x05e6, 0x05bd, 0x05b8, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x4c, 0x26, 0x7e, 0x5c, 0xc7, 0x74, 0xd8, +INPUT 0x7c, 0x7c, 0x7c, 0x12, +DECODED 0x0568, 0x05e6, 0x05bd, 0x05b8, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xb2, 0x25, 0x7e, 0x38, 0x87, 0x74, 0x64, 0x22, 0x05, 0x00, 0x5f, +DECODED 0x0537, 0x05e6, 0x05b1, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x46, 0x45, 0x7e, 0x1e, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xc9, +DECODED 0x0516, 0x05e7, 0x05a9, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xc0, 0x44, 0x7e, 0x1c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0xa3, +DECODED 0x04ec, 0x05e7, 0x05a9, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xa8, 0x83, 0x80, 0xe0, 0x46, 0x74, 0x64, 0x1a, 0x05, 0x00, 0x41, +DECODED 0x0494, 0x05f2, 0x0596, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xce, 0x62, 0x88, 0xa2, 0x46, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xeb, +DECODED 0x0450, 0x061a, 0x0582, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xf6, 0x81, 0x95, 0x48, 0x46, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x44, +DECODED 0x040d, 0x065b, 0x0566, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xb0, 0x41, 0xa0, 0xd0, 0x65, 0x72, 0x64, 0x21, 0x05, 0x00, 0xaf, +DECODED 0x03f7, 0x0691, 0x0541, 0x05ac, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xac, 0xe1, 0xac, 0x16, 0x85, 0x6e, 0x7c, 0xe2, 0xe2, 0xe2, 0x25, +DECODED 0x03f6, 0x06d0, 0x0507, 0x0598, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xaa, 0x21, 0xb7, 0x74, 0xa4, 0x6a, 0xd8, 0x7c, 0x7c, 0x7c, 0x78, +DECODED 0x03f5, 0x0703, 0x04d4, 0x0585, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xa6, 0x81, 0xc0, 0xee, 0x03, 0x67, 0x64, 0x1d, 0x05, 0x00, 0x01, +DECODED 0x03f4, 0x0732, 0x04aa, 0x0573, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xa4, 0x81, 0xc4, 0x62, 0x03, 0x65, 0x7c, 0xe2, 0xe2, 0xe2, 0x94, +DECODED 0x03f3, 0x0746, 0x047f, 0x0569, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xa0, 0xc1, 0xc6, 0xf2, 0x42, 0x63, 0xd8, 0x7c, 0x7c, 0x7c, 0x2f, +DECODED 0x03f2, 0x0751, 0x045c, 0x0560, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xa0, 0xe1, 0xc6, 0xa4, 0x22, 0x63, 0x64, 0x26, 0x05, 0x00, 0xed, +DECODED 0x03f2, 0x0752, 0x0443, 0x055f, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xa0, 0x81, 0xc8, 0xa0, 0x62, 0x61, 0x7c, 0xe2, 0xe2, 0xe2, 0x86, +DECODED 0x03f2, 0x075a, 0x0442, 0x0557, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xa0, 0x61, 0xc8, 0x9c, 0x82, 0x63, 0xd8, 0x7c, 0x7c, 0x7c, 0xe6, +DECODED 0x03f2, 0x075a, 0x0441, 0x0561, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd6, 0x01, 0xc1, 0x0c, 0xe3, 0x68, 0x64, 0x1c, 0x05, 0x00, 0xb7, +DECODED 0x0403, 0x0735, 0x0464, 0x057c, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xa6, 0xc2, 0xb8, 0x96, 0xe3, 0x6a, 0x7c, 0xe2, 0xe2, 0xe2, 0xeb, +DECODED 0x0444, 0x070b, 0x048f, 0x0586, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xe2, 0x83, 0xab, 0x9a, 0xe4, 0x6a, 0xd8, 0x7c, 0x7c, 0x7c, 0x20, +DECODED 0x04a7, 0x06c9, 0x04e0, 0x0586, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xa8, 0x84, 0x9f, 0x74, 0x45, 0x6d, 0x64, 0x22, 0x05, 0x00, 0x57, +DECODED 0x04e4, 0x068d, 0x0524, 0x0592, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x46, 0x45, 0x8e, 0x7c, 0x86, 0x73, 0x7c, 0xe2, 0xe2, 0xe2, 0xc2, +DECODED 0x0516, 0x0637, 0x0577, 0x05b1, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x9c, 0xe5, 0x7f, 0x18, 0x87, 0x73, 0xd8, 0x7c, 0x7c, 0x7c, 0xd7, +DECODED 0x0531, 0x05ef, 0x05a7, 0x05b1, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xea, 0xe5, 0x7d, 0x58, 0xa7, 0x73, 0x64, 0x1d, 0x05, 0x00, 0xd5, +DECODED 0x0549, 0x05e5, 0x05bb, 0x05b2, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x4a, 0xe6, 0x7d, 0x58, 0xe7, 0x73, 0x7c, 0xe2, 0xe2, 0xe2, 0x81, +DECODED 0x0567, 0x05e5, 0x05bb, 0x05b3, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xa0, 0xe6, 0x7d, 0x58, 0x07, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x33, +DECODED 0x0582, 0x05e5, 0x05bb, 0x05b4, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xe8, 0xe6, 0x7d, 0x5a, 0x07, 0x74, 0x64, 0x28, 0x05, 0x00, 0x21, +DECODED 0x0598, 0x05e5, 0x05bc, 0x05b4, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x06, 0xe7, 0x7d, 0x5a, 0x27, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x4d, +DECODED 0x05a2, 0x05e5, 0x05bc, 0x05b4, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x26, 0xe7, 0x7d, 0x5c, 0x27, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0xaf, +DECODED 0x05ac, 0x05e5, 0x05bd, 0x05b4, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x42, 0xe7, 0x7d, 0x5c, 0x47, 0x74, 0x64, 0x24, 0x05, 0x00, 0x28, +DECODED 0x05b4, 0x05e5, 0x05bd, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, +INPUT 0x5e, 0xe7, 0x7d, 0x5c, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x6d, +DECODED 0x05bd, 0x05e5, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x98, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0xf7, +DECODED 0x05cf, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xc0, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x23, 0x05, 0x00, 0xd8, +DECODED 0x05dc, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xc4, 0xe7, 0x7d, 0x5c, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xab, +DECODED 0x05dd, 0x05e5, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xc6, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0xd1, +DECODED 0x05de, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xc8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x23, 0x05, 0x00, 0x1e, +DECODED 0x05de, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xca, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x3c, +DECODED 0x05df, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xce, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x17, +DECODED 0x05e0, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xce, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x22, 0x05, 0x00, 0xfb, +DECODED 0x05e0, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xce, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xcb, +DECODED 0x05e0, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd0, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x3c, +DECODED 0x05e1, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd0, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x24, 0x05, 0x00, 0x70, +DECODED 0x05e1, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd0, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x1d, +DECODED 0x05e1, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd0, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0xc1, +DECODED 0x05e1, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd0, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x27, 0x05, 0x00, 0xdd, +DECODED 0x05e1, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd0, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x7c, +DECODED 0x05e1, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xb6, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x27, +DECODED 0x05d9, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x92, 0x07, 0x87, 0x5e, 0x47, 0x74, 0x64, 0x20, 0x05, 0x00, 0x30, +DECODED 0x05cd, 0x0613, 0x05bd, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x90, 0xa7, 0x98, 0x5e, 0x47, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xc7, +DECODED 0x05cd, 0x066b, 0x05bd, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xb2, 0xc7, 0xb5, 0x7a, 0x27, 0x6e, 0xd8, 0x7c, 0x7c, 0x7c, 0xef, +DECODED 0x05d7, 0x06fc, 0x05c6, 0x0596, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xc8, 0x87, 0xc9, 0x7c, 0xc7, 0x64, 0x64, 0x26, 0x05, 0x00, 0x43, +DECODED 0x05de, 0x075f, 0x05c7, 0x0568, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xe8, 0xe7, 0xda, 0x7c, 0x87, 0x54, 0x7c, 0xe2, 0xe2, 0xe2, 0x91, +DECODED 0x05e8, 0x07b6, 0x05c7, 0x0516, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x08, 0x68, 0xe2, 0x62, 0x07, 0x45, 0xd8, 0x7c, 0x7c, 0x7c, 0xc0, +DECODED 0x05f2, 0x07dc, 0x05be, 0x04c9, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x24, 0x68, 0xe2, 0x62, 0x07, 0x30, 0x64, 0x22, 0x05, 0x00, 0xcb, +DECODED 0x05fb, 0x07dc, 0x05be, 0x0460, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x26, 0x68, 0xe2, 0x62, 0xc7, 0x21, 0x7c, 0xe2, 0xe2, 0xe2, 0x50, +DECODED 0x05fc, 0x07dc, 0x05be, 0x0419, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x28, 0x68, 0xe2, 0x62, 0xa7, 0x15, 0xd8, 0x7c, 0x7c, 0x7c, 0x5b, +DECODED 0x05fc, 0x07dc, 0x05be, 0x03dc, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x28, 0x68, 0xe2, 0x62, 0xa7, 0x15, 0x64, 0x22, 0x05, 0x00, 0xb7, +DECODED 0x05fc, 0x07dc, 0x05be, 0x03dc, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x2a, 0x68, 0xe2, 0x62, 0xa7, 0x15, 0x7c, 0xe2, 0xe2, 0xe2, 0x16, +DECODED 0x05fd, 0x07dc, 0x05be, 0x03dc, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x30, 0x68, 0xe2, 0x62, 0xa7, 0x15, 0xd8, 0x7c, 0x7c, 0x7c, 0x16, +DECODED 0x05ff, 0x07dc, 0x05be, 0x03dc, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x34, 0xa8, 0xe0, 0x62, 0xa7, 0x15, 0x64, 0x1f, 0x05, 0x00, 0x25, +DECODED 0x0600, 0x07d3, 0x05be, 0x03dc, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x4c, 0x68, 0xd8, 0x62, 0xa7, 0x15, 0x7c, 0xe2, 0xe2, 0xe2, 0xf7, +DECODED 0x0608, 0x07aa, 0x05be, 0x03dc, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x2a, 0x08, 0xc8, 0x62, 0xa7, 0x15, 0xd8, 0x7c, 0x7c, 0x7c, 0xc1, +DECODED 0x05fd, 0x0758, 0x05be, 0x03dc, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x0e, 0x08, 0xbb, 0x62, 0x07, 0x1d, 0x64, 0x1e, 0x05, 0x00, 0x71, +DECODED 0x05f4, 0x0717, 0x05be, 0x0401, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xf0, 0x07, 0xab, 0x60, 0xa7, 0x2b, 0x7c, 0xe2, 0xe2, 0xe2, +INPUT 0xa8, +DECODED 0x05eb, 0x06c7, 0x05be, 0x044a, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xea, 0xe7, 0x9f, 0x60, 0xa7, 0x39, 0xd8, 0x7c, 0x7c, 0x7c, 0x5e, +DECODED 0x05e9, 0x068f, 0x05be, 0x0490, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xe6, 0xa7, 0x93, 0x60, 0xe7, 0x49, 0x64, 0x1c, 0x05, 0x00, 0x9c, +DECODED 0x05e8, 0x0652, 0x05be, 0x04e1, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xe2, 0x27, 0x8c, 0x60, 0x27, 0x57, 0x7c, 0xe2, 0xe2, 0xe2, 0xdb, +DECODED 0x05e6, 0x062c, 0x05be, 0x0524, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xe2, 0x87, 0x87, 0x60, 0xa7, 0x67, 0xd8, 0x7c, 0x7c, 0x7c, +INPUT 0x2f, +DECODED 0x05e6, 0x0615, 0x05be, 0x0576, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x0c, 0x88, 0x85, 0x60, 0xa7, 0x75, 0x64, 0x1a, 0x05, 0x00, 0x9c, +DECODED 0x05f4, 0x060b, 0x05be, 0x05bc, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0x70, 0x88, 0x7e, 0x60, 0x67, 0x75, 0x7c, 0xe2, 0xe2, 0xe2, +INPUT 0x13, +DECODED 0x0613, 0x05e8, 0x05be, 0x05bb, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x98, 0x67, 0x7e, 0x60, 0x07, 0x75, 0xd8, 0x7c, 0x7c, 0x7c, +INPUT 0xc8, +DECODED 0x05cf, 0x05e8, 0x05be, 0x05b9, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0x28, 0x68, 0x7e, 0x60, 0xe7, 0x74, 0x64, 0x1e, 0x05, 0x00, 0xd3, +DECODED 0x05fc, 0x05e8, 0x05be, 0x05b8, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xc0, 0x47, 0x7e, 0x60, 0xc7, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xec, +DECODED 0x05dc, 0x05e7, 0x05be, 0x05b8, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0x20, 0x48, 0x7e, 0x60, 0xc7, 0x74, 0xd8, 0x7c, +INPUT 0x7c, 0x7c, 0x23, +DECODED 0x05fa, 0x05e7, 0x05be, 0x05b8, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xaa, 0x27, 0x7e, 0x60, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0x0f, +DECODED 0x05d5, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xf8, 0x27, 0x7e, 0x60, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, +INPUT 0xcb, +DECODED 0x05ed, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xde, 0x27, 0x7e, 0x60, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0x9d, +DECODED 0x05e5, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xc4, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1f, 0x05, 0x00, +INPUT 0x0d, +DECODED 0x05dd, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xca, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x5d, +DECODED 0x05df, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xce, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0xd8, 0x7c, 0x7c, +INPUT 0x7c, 0x76, +DECODED 0x05e0, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd0, 0x27, 0x7e, 0x5e, +INPUT 0x87, 0x74, 0x64, 0x16, 0x05, 0x00, 0xe9, +DECODED 0x05e1, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd2, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x10, +DECODED 0x05e1, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd2, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0x7c, 0xcc, +DECODED 0x05e1, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x1b, 0x05, 0x00, +INPUT 0x08, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x76, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0xd8, 0x7c, 0x7c, +INPUT 0x7c, 0xaa, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x16, 0x05, 0x00, 0x1e, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x27, 0x7e, 0x60, 0x87, 0x74, 0x7c, 0xe2, +INPUT 0xe2, 0xe2, 0xeb, +DECODED 0x05e2, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x60, 0x87, 0x74, 0xd8, 0xe2, 0x7c, 0x7c, 0xa0, +DECODED 0x05e2, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x27, 0x7e, 0x60, 0x87, 0x74, 0x64, 0x19, +INPUT 0x05, 0x00, 0x46, +DECODED 0x05e2, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x27, 0x7e, 0x60, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xeb, +DECODED 0x05e2, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x60, 0x87, 0x74, 0xd8, 0xe2, 0x7c, 0x7c, +INPUT 0x6c, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x60, 0x87, 0x74, 0x64, 0x15, 0x05, 0x00, 0x1f, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x60, 0x87, 0x74, 0x7c, +INPUT 0xe2, 0xe2, 0xe2, 0x27, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, 0x7e, 0x60, 0x87, 0x74, 0xd8, 0xe2, 0xe2, 0x7c, 0xb8, +DECODED 0x05e2, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x27, 0x7e, 0x60, 0x87, 0x74, 0x64, 0x19, +INPUT 0x05, 0x00, 0x46, +DECODED 0x05e2, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x07, 0x7e, 0x60, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x8a, +DECODED 0x05e2, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x60, 0x87, 0x74, 0xd8, +INPUT 0xe2, 0xe2, 0x7c, 0xd9, +DECODED 0x05e2, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x27, 0x7e, 0x60, 0x87, 0x74, 0x64, 0x13, 0x05, 0x00, 0x73, +DECODED 0x05e2, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x27, 0x7e, 0x60, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, +INPUT 0xeb, +DECODED 0x05e2, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x60, 0x87, 0x74, 0xd8, 0xe2, 0xe2, 0x7c, 0x15, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x60, 0x47, 0x74, 0x64, +INPUT 0x16, 0x05, 0x00, 0x20, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x60, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x46, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x60, 0x87, +INPUT 0x74, 0xd8, 0xe2, 0xe2, 0x7c, 0x15, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x60, 0x47, 0x74, 0x64, 0x12, 0x05, 0x00, 0x53, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x60, 0x47, 0x74, +INPUT 0x7c, 0xe2, 0xe2, 0xe2, 0x48, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x60, 0x47, 0x74, 0xd8, 0xe2, 0xe2, 0x7c, 0x7a, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x60, 0x47, 0x74, 0x64, 0x16, 0x05, 0x00, 0x20, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x60, 0x47, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x48, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x60, 0x87, 0x74, +INPUT 0xd8, 0xe2, 0xe2, 0x7c, 0x74, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x60, 0x47, 0x74, 0x64, 0x14, 0x05, 0x00, 0x92, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x60, 0x47, 0x74, 0x7c, 0xe2, +INPUT 0xe2, 0xe2, 0x48, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x60, 0x47, 0x74, 0xd8, 0xe2, 0xe2, 0x7c, 0x7a, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x60, 0x47, 0x74, 0x64, 0x15, 0x05, 0x00, 0x11, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x60, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x46, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x60, 0x87, 0x74, 0xd8, 0xe2, 0xe2, 0x7c, 0x15, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x14, 0x05, 0x00, 0x60, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x60, +INPUT 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x46, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x07da, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0xd8, 0xe2, 0x7c, 0x7c, 0x90, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x60, 0x47, +INPUT 0x74, 0x64, 0x14, 0x05, 0x00, 0x92, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0xba, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5e, 0x87, 0x74, +INPUT 0xd8, 0xe2, 0x7c, 0x7c, 0x90, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x60, 0x87, 0x74, 0x64, 0x11, 0x05, 0x00, 0x6c, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x60, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, +INPUT 0xb0, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x60, 0x87, 0x74, 0xd8, 0xe2, 0x7c, 0x7c, 0x6c, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x60, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0xcc, +DECODED 0x05e3, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x27, 0x7e, 0x60, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0x7c, +DECODED 0x05e2, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x60, +INPUT 0x87, 0x74, 0xd8, 0xe2, 0x7c, 0x7c, 0xa0, +DECODED 0x05e2, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x27, 0x7e, 0x5e, 0x87, 0x74, 0x64, 0x14, 0x05, 0x00, 0xcd, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5e, 0x87, 0x74, +INPUT 0x7c, 0x7c, 0xe2, 0xe2, 0x2d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x60, 0x87, 0x74, 0xd8, 0xe2, 0x7c, 0x7c, 0xa0, +DECODED 0x05e2, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x27, 0x7e, 0x60, 0x87, 0x74, 0x64, 0x13, 0x05, 0x00, +INPUT 0x73, +DECODED 0x05e2, 0x05e6, 0x05be, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x07, 0x7e, 0x5e, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0x80, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, 0x7e, +INPUT 0x5a, 0x87, 0x74, 0xd8, 0xe2, 0x7c, 0x7c, 0x73, +DECODED 0x05e2, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x07, 0x7e, 0x5a, 0x87, 0x74, 0x64, 0x12, 0x05, 0x00, 0x23, +DECODED 0x05e2, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x07, 0x7e, 0x5a, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xaf, +DECODED 0x05e2, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, 0x7e, 0x5a, 0x87, 0x74, 0xd8, 0xe2, 0x7c, 0x7c, 0x73, +DECODED 0x05e2, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5a, 0x87, 0x74, +INPUT 0x64, 0x13, 0x05, 0x00, 0x0d, +DECODED 0x05e3, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x27, 0x7e, 0x5a, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xce, +DECODED 0x05e2, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x5a, 0x47, 0x74, 0xd8, 0xe2, 0x7c, 0x7c, +INPUT 0x7d, +DECODED 0x05e2, 0x05e6, 0x05bc, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5a, 0x87, 0x74, 0x64, 0x14, 0x05, 0x00, 0x2e, +DECODED 0x05e3, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x07, 0x7e, +INPUT 0x5a, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xaf, +DECODED 0x05e2, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5a, 0x87, 0x74, 0xd8, 0xe2, 0x7c, 0x7c, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bc, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, +INPUT 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x14, 0x05, 0x00, 0x9d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x05dc, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0xd8, 0xe2, 0x7c, 0xe2, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0x64, 0x13, 0x05, 0x00, 0xd1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, +INPUT 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0xe2, 0x7c, 0xe2, 0x78, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, +INPUT 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x14, 0x05, 0x00, 0x9d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0x7d, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, +INPUT 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0xe2, 0x7c, 0xe2, 0x78, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x10, 0x05, 0x00, 0x8f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x47, 0x74, +INPUT 0x7c, 0x7c, 0xe2, 0xe2, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0xe2, 0x7c, 0xe2, 0x19, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x15, 0x05, 0x00, 0x1e, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xde, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, +INPUT 0x07, 0x7e, 0x5c, 0x47, 0x74, 0xd8, 0xe2, 0x7c, 0xe2, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0x64, 0x12, 0x05, 0x00, 0x52, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x47, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0xe1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0x64, 0x15, 0x05, 0x00, 0x1e, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, +INPUT 0xef, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, +INPUT 0x07, 0x7e, 0x5c, 0x47, 0x74, 0x64, 0x13, 0x05, 0x00, 0xd1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0xef, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, +INPUT 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0x08, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xd0, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0xd8, 0x7c, 0x7c, 0xe2, 0x23, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x10, 0x05, 0x00, 0xee, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, +INPUT 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0x42, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, +INPUT 0x13, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xa5, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xd0, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0x8e, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x10, 0x05, 0x00, 0xee, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0x15, 0x7c, 0xe2, 0xe2, 0xed, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0xef, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x15, 0x05, 0x00, +INPUT 0x7f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x15, 0x7c, 0xe2, 0xe2, 0x8c, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, +INPUT 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0xef, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x13, 0x05, 0x00, 0xdf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, +INPUT 0x7e, 0x5c, 0x87, 0x74, 0x15, 0x7c, 0xe2, 0xe2, 0x8c, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, +INPUT 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0x8e, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x16, 0x05, 0x00, 0x2f, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x15, 0x7c, 0xe2, 0xe2, 0x8c, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, +INPUT 0x42, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x12, 0x05, 0x00, 0xf1, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x15, 0x7c, 0xe2, 0xe2, 0xed, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0x42, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x07, +INPUT 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x14, 0x05, 0x00, 0x51, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x15, 0x7c, 0xe2, 0xe2, 0x40, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0xef, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0f, 0x05, 0x00, 0xfe, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x15, 0x7c, 0xe2, 0xe2, +INPUT 0x21, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0x23, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x13, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0xc4, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x15, 0x7c, 0xe2, 0xe2, 0x21, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, +INPUT 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0x23, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x11, 0x05, 0x00, 0x0c, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0x15, 0x7c, 0xe2, 0xe2, 0x8c, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0x23, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x16, 0x05, 0x00, +INPUT 0x4e, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x03d9, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x11, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0x23, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x12, 0x05, 0x00, 0xf1, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0x23, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x13, 0x05, 0x00, 0x72, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0x7d, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, +INPUT 0x23, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x12, 0x05, 0x00, 0x5c, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0x23, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, +INPUT 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x15, 0x05, 0x00, 0x1e, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0x8e, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x12, 0x05, 0x00, 0x3d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0xe2, 0x7c, 0xe2, 0x19, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x18, 0x05, 0x00, 0x69, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xd0, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, +INPUT 0x7e, 0x5c, 0x47, 0x74, 0xd8, 0xe2, 0x7c, 0xe2, 0xdb, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x16, 0x05, 0x00, 0x4e, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0x7c, 0x7c, 0xe2, 0xe2, 0xd0, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0xd8, 0xe2, 0x7c, 0xe2, 0x17, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0x64, 0x17, 0x05, 0x00, 0xa2, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0xe2, 0x7c, 0xe2, 0x78, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0x64, 0x16, 0x05, 0x00, 0x21, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, +INPUT 0x7e, 0x5c, 0x47, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xde, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0xe2, 0x7c, 0xe2, 0x78, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0x64, 0x16, 0x05, 0x00, 0x4e, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0x12, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0xe2, 0x7c, 0xe2, +INPUT 0x78, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x13, 0x05, 0x00, 0xdf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x47, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0xe2, 0x7c, 0xe2, 0x19, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x47, 0x74, 0x64, 0x1b, 0x05, 0x00, 0x56, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x47, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xbf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0xd8, 0xe2, 0x7c, 0xe2, 0x19, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0x64, 0x13, 0x05, 0x00, 0xd1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, +INPUT 0xde, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0xe2, 0x7c, 0xe2, 0x78, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x14, 0x05, 0x00, 0x9d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x07da, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, +INPUT 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0x23, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x11, 0x05, 0x00, 0xc0, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0xef, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x10, 0x05, 0x00, +INPUT 0xee, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x7c, 0xe2, 0x8e, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x11, 0x05, 0x00, 0x0c, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, +INPUT 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xd0, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x05dc, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xd7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0x64, 0x0f, 0x05, 0x00, 0xfe, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0x7d, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xb6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x12, 0x05, 0x00, 0x3d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xb6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, +INPUT 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0f, 0x05, 0x00, 0x32, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xd0, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0xd8, 0x7c, 0x15, 0xe2, 0x1b, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x10, 0x05, 0x00, 0x43, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, +INPUT 0xd0, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xd7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0e, 0x05, 0x00, 0xd0, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0x7c, 0xe2, 0xe2, 0xd0, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xd7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x13, 0x05, 0x00, 0xdf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0x7c, 0x7c, 0xe2, 0xe2, 0xd0, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xd7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x19, 0x05, 0x00, +INPUT 0x8b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x05dc, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x26, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x11, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0x7a, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, +INPUT 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x10, 0x05, 0x00, 0x22, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x26, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xb6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x12, 0x05, 0x00, 0x3d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x47, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, +INPUT 0xd7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0e, 0x05, 0x00, 0xd0, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x49, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xb6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, +INPUT 0x7e, 0x5c, 0x47, 0x74, 0x64, 0x10, 0x05, 0x00, 0x81, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0x7c, 0xe2, 0xe2, 0xe2, 0x49, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, +INPUT 0xd8, 0x7c, 0x15, 0xe2, 0xd9, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0x64, 0x0c, 0x05, 0x00, 0x0d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x05dc, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, +INPUT 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xd9, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x47, 0x74, 0x64, 0x0d, 0x05, 0x00, 0xef, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, +INPUT 0x7e, 0x5c, 0x47, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xd9, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x47, 0x74, 0x64, 0x0c, 0x05, 0x00, 0x6c, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xae, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xb6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0b, 0x05, 0x00, +INPUT 0x41, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xae, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xb6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0c, 0x05, 0x00, 0x62, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xae, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xd7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0x64, 0x0a, 0x05, 0x00, 0xc2, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, +INPUT 0xd7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0d, 0x05, 0x00, 0xe1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x10, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0x62, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0x1b, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0b, 0x05, 0x00, 0x41, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0xd8, 0x7c, 0x15, 0xe2, 0xd7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0c, 0x05, 0x00, 0x62, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, +INPUT 0xae, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xb6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0a, 0x05, 0x00, 0xc2, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, +INPUT 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xb6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0c, 0x05, 0x00, 0x03, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xb6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0c, 0x05, 0x00, 0x62, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x11, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0x7a, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0x64, 0x0d, 0x05, 0x00, 0x8e, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, +INPUT 0x7e, 0x5c, 0x47, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xa0, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xd9, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0x64, 0x10, 0x05, 0x00, 0xee, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xb6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x13, 0x05, 0x00, 0xbe, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5a, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0x64, +DECODED 0x05e3, 0x05e6, 0x05bc, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x07, +INPUT 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, 0x01, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xae, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0xd8, 0x7c, 0x15, 0xe2, 0x1b, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x1f, 0x05, 0x00, 0x2b, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, +INPUT 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xd9, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x13, 0xd4, 0x27, 0x7e, 0x5c, 0x47, 0x74, 0x64, 0x13, 0x05, 0x00, 0x7c, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b5, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd4, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0x6c, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b5, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0x15, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b5, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0x64, 0x15, 0x05, 0x00, 0x71, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0xe2, 0xe2, 0xe2, 0xe2, 0xae, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xb6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x12, 0x05, 0x00, +INPUT 0x3d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xb6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0e, 0x05, 0x00, 0xb1, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, +INPUT 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xb6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x15, 0x05, 0x00, 0x1e, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, +INPUT 0xb6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x11, 0x05, 0x00, 0xa1, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xae, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0x1b, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x27, +INPUT 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x14, 0x05, 0x00, 0x30, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0xd8, 0x7c, 0x15, 0xe2, 0xd7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x11, 0x05, 0x00, 0x0c, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xae, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0x1b, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x12, 0x05, 0x00, 0x3d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, +INPUT 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0x7a, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x13, 0x05, 0x00, 0xbe, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0xe2, 0xe2, 0xe2, 0xe2, 0xae, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0x7a, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x13, 0x05, 0x00, +INPUT 0xdf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xd7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x12, 0x05, 0x00, 0x5c, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xae, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xd7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0x64, 0x14, 0x05, 0x00, 0x30, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xae, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd4, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, +INPUT 0x1b, +DECODED 0x05e2, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x0f, 0x05, 0x00, 0x53, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xae, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, +INPUT 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xd7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x14, 0x05, 0x00, 0xfc, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0xd8, 0x7c, 0x15, 0xe2, 0xb6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x10, 0x05, 0x00, 0xee, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, +INPUT 0xae, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xb6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x47, 0x74, 0x64, 0x14, 0x05, 0x00, 0xf2, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b5, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x27, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xae, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x27, +INPUT 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xd7, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x11, 0x05, 0x00, 0x6d, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, +INPUT 0xe2, 0xe2, 0xe2, 0xe2, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xb6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x13, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0x64, 0x17, 0x05, 0x00, +INPUT 0xcd, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, 0x0c, 0x10, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xe2, 0xe2, 0xe2, 0xe2, 0xcf, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff, +INPUT 0x82, +INPUT 0x0c, 0x11, 0xd8, 0x07, 0x7e, 0x5c, 0x87, 0x74, 0xd8, 0x7c, 0x15, 0xe2, 0xb6, +DECODED 0x05e3, 0x05e6, 0x05bd, 0x05b6, 0x07da, 0x07da, 0x07da, 0x07da, 0x07a8, 0x05dc, 0x03d9, 0x07da, 0xffff, 0xffff, 0xffff, 0xffff,